159 lines
4.4 KiB
C
159 lines
4.4 KiB
C
/***************************************************************************
|
|
* Copyright (C) 01/2008 by Olaf Rempel *
|
|
* razzor@kopf-tisch.de *
|
|
* *
|
|
* This program is free software; you can redistribute it and/or modify *
|
|
* it under the terms of the GNU General Public License as published by *
|
|
* the Free Software Foundation; version 2 of the License *
|
|
* *
|
|
* This program is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU General Public License *
|
|
* along with this program; if not, write to the *
|
|
* Free Software Foundation, Inc., *
|
|
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
|
|
***************************************************************************/
|
|
#include "AT91SAM7S256.h"
|
|
#include "at91_sysc.h"
|
|
#include "at91_dbgu.h"
|
|
#include "at91_pitc.h"
|
|
#include "at91_adc.h"
|
|
#include "at91_tests.h"
|
|
#include "at91_udp.h"
|
|
#include "at91_pio.h"
|
|
#include "at91_twi.h"
|
|
|
|
#include "at91_tc1.h"
|
|
#include "memalloc.h"
|
|
|
|
#include "board.h"
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static uint32_t global_state;
|
|
|
|
#define MOTORS_RUNNING 0x0001
|
|
#define STICK_CALIBRATION 0x0002
|
|
|
|
static uint32_t stick_timer_cb(struct pitc_timer *timer)
|
|
{
|
|
static uint32_t i;
|
|
*AT91C_PIOA_SODR = i;
|
|
i = i ^ LED_GREEN;
|
|
*AT91C_PIOA_CODR = i;
|
|
|
|
struct rc_values rc;
|
|
uint32_t count = rcontrol_getswitches(&rc);
|
|
|
|
static uint32_t stick_cal_count;
|
|
|
|
if (global_state & MOTORS_RUNNING) {
|
|
if ((rc.chan[2] < 0 && rc.chan[3] > 0) || count < 4)
|
|
global_state &= ~MOTORS_RUNNING;
|
|
|
|
} else if (global_state & STICK_CALIBRATION) {
|
|
stick_cal_count--;
|
|
if (stick_cal_count == 0) {
|
|
rcontrol_calibrate(RC_CAL_END);
|
|
rcontrol_calibrate(RC_CAL_SAVE);
|
|
rcontrol_print_cal();
|
|
global_state &= ~STICK_CALIBRATION;
|
|
}
|
|
|
|
} else if (count >= 4) {
|
|
if (rc.chan[2] < 0 && rc.chan[3] < 0)
|
|
global_state |= MOTORS_RUNNING;
|
|
|
|
if (rc.chan[2] > 0 && rc.chan[3] > 0)
|
|
adc_calibrate(ADC_CAL_GYRO);
|
|
|
|
if (rc.chan[2] > 0 && rc.chan[3] < 0) {
|
|
adc_calibrate(ADC_CAL_ACC);
|
|
|
|
stick_cal_count = 3000;
|
|
global_state |= STICK_CALIBRATION;
|
|
rcontrol_calibrate(RC_CAL_START);
|
|
}
|
|
}
|
|
|
|
rcontrol_getvalues(&rc);
|
|
|
|
static uint8_t pwm[4];
|
|
if (global_state & MOTORS_RUNNING) {
|
|
int32_t pitch = rc.chan[2] * 256;
|
|
int32_t gier = rc.chan[3] * 64;
|
|
int32_t nick = rc.chan[0] * 64;
|
|
int32_t roll = rc.chan[1] * 64;
|
|
|
|
int32_t data[4];
|
|
data[0] = (pitch - nick + gier) / 1024;
|
|
data[1] = (pitch + nick + gier) / 1024;
|
|
data[2] = (pitch + roll - gier) / 1024;
|
|
data[3] = (pitch - roll - gier) / 1024;
|
|
|
|
pwm[0] = (data[0] > 0x0f) ? data[0] : 0x0f;
|
|
pwm[1] = (data[1] > 0x0f) ? data[1] : 0x0f;
|
|
pwm[2] = (data[2] > 0x0f) ? data[2] : 0x0f;
|
|
pwm[3] = (data[3] > 0x0f) ? data[3] : 0x0f;
|
|
|
|
} else {
|
|
pwm[0] = 0x00;
|
|
pwm[1] = 0x00;
|
|
pwm[2] = 0x00;
|
|
pwm[3] = 0x00;
|
|
}
|
|
|
|
// printf("%3d %3d %3d %3d\n\r", pwm[0], pwm[1], pwm[2], pwm[3]);
|
|
twi_setpwm(pwm);
|
|
|
|
// adc_trigger();
|
|
return PITC_RESTART_TIMER;
|
|
}
|
|
|
|
static struct pitc_timer stick_timer = {
|
|
.interval = 10,
|
|
.func = &stick_timer_cb,
|
|
};
|
|
|
|
int main(void)
|
|
{
|
|
/* LED outputs */
|
|
*AT91C_PIOA_PER = LED_GREEN | LED_ORANGE;
|
|
*AT91C_PIOA_OER = LED_GREEN | LED_ORANGE;
|
|
|
|
/* needed for dbgu */
|
|
at91_sysc_init();
|
|
|
|
at91_dbgu_init();
|
|
at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r");
|
|
|
|
/* triggers pinchange-isrs */
|
|
at91_pio_init();
|
|
|
|
/* timer */
|
|
at91_pitc_init();
|
|
at91_rttc_test_init();
|
|
|
|
/* twi */
|
|
at91_twi_init();
|
|
at91_twi_test();
|
|
|
|
/* remote control, needs twi */
|
|
at91_tc1_init();
|
|
|
|
/* usb */
|
|
at91_udp_init();
|
|
|
|
/* adc, need timer, twi */
|
|
at91_adc_init();
|
|
|
|
pitc_schedule_timer(&stick_timer);
|
|
|
|
printf("static alloc: %5ld bytes\n\r", static_alloc_used());
|
|
|
|
while (1);
|
|
}
|