From a8256718ab9bb006bc2f7da953ba34a53d54822a Mon Sep 17 00:00:00 2001 From: Olaf Rempel Date: Tue, 11 Mar 2008 22:02:06 +0100 Subject: [PATCH] working rc->mixer->blctrls path --- include/at91_adc.h | 1 + include/at91_tc1.h | 2 + main.c | 99 +++++++++++++++++++++++++++++++--------------- src/at91_adc.c | 80 +++++++++++++++++-------------------- src/at91_tc1.c | 23 +++++++++++ 5 files changed, 130 insertions(+), 75 deletions(-) diff --git a/include/at91_adc.h b/include/at91_adc.h index 7d9acc9..ba463ae 100644 --- a/include/at91_adc.h +++ b/include/at91_adc.h @@ -11,6 +11,7 @@ #define ADC_ACC_YAW 5 #define ADC_VOLTAGE 6 +#define ADC_CAL_NONE 0 #define ADC_CAL_GYRO 1 #define ADC_CAL_ACC 2 diff --git a/include/at91_tc1.h b/include/at91_tc1.h index 5dd21fa..c891417 100644 --- a/include/at91_tc1.h +++ b/include/at91_tc1.h @@ -11,6 +11,8 @@ struct rc_values { }; uint32_t rcontrol_getvalues(struct rc_values *rc); +uint32_t rcontrol_getswitches(struct rc_values *rc); + void rcontrol_calibrate(uint32_t mode); void rcontrol_print_cal(void); diff --git a/main.c b/main.c index 80c4268..b93ce91 100644 --- a/main.c +++ b/main.c @@ -33,54 +33,90 @@ #include #include -static uint32_t pitc_test(struct pitc_timer *timer) +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_getvalues(&rc); + uint32_t count = rcontrol_getswitches(&rc); - printf("%ld channels: ", count); - uint32_t j; - for (j = 0; j < count; j++) - printf("%+5d ", rc.chan[j]); + static uint32_t stick_cal_count; - printf("\r"); + if (global_state & MOTORS_RUNNING) { + if ((rc.chan[2] < 0 && rc.chan[3] > 0) || count < 4) + global_state &= ~MOTORS_RUNNING; - static uint32_t cnt; + } else if (global_state & STICK_CALIBRATION) { + stick_cal_count--; + if (stick_cal_count == 0) { + rcontrol_calibrate(RC_CAL_END); + rcontrol_print_cal(); + global_state &= ~STICK_CALIBRATION; + } - if (cnt++ == 300) { - rcontrol_calibrate(RC_CAL_END); - printf("\n\r"); - rcontrol_print_cal(); + } else { + 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); + } } -*/ - adc_trigger(); + 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 pitc_test_timer = { - .interval = 100, - .func = &pitc_test, +static struct pitc_timer stick_timer = { + .interval = 10, + .func = &stick_timer_cb, }; -static void motor_mixer(int16_t pitch, int16_t nick, int16_t roll, int16_t yaw) -{ - static uint8_t pwm[4]; - - pwm[0] = pitch + nick + yaw; - pwm[1] = pitch - nick + yaw; - pwm[2] = pitch + roll - yaw; - pwm[3] = pitch - roll - yaw; - - twi_setpwm(pwm); -} - int main(void) { /* LED outputs */ @@ -113,8 +149,7 @@ int main(void) /* adc, need timer */ at91_adc_init(); - pitc_schedule_timer(&pitc_test_timer); + pitc_schedule_timer(&stick_timer); -// rcontrol_calibrate(RC_CAL_START); while (1); } diff --git a/src/at91_adc.c b/src/at91_adc.c index 4384a29..6ee5fad 100644 --- a/src/at91_adc.c +++ b/src/at91_adc.c @@ -77,45 +77,7 @@ static uint32_t adc_calibrate_cb(struct pitc_timer *timer) adc_cal_data[1] /= ADC_CAL_COUNT; adc_cal_data[2] /= ADC_CAL_COUNT; - adc_cal_mode = 0; - - return PITC_REMOVE_TIMER; - } - - return PITC_RESTART_TIMER; -} - -static struct pitc_timer adc_cal_timer = { - .interval = 1, - .func = &adc_calibrate_cb, -}; - -void adc_trigger(void) -{ - printf("R:%4d:%4d N:%4d:%4d Y:%4d:%4d U:%4d\n\r", - adc_result[ADC_GYRO_ROLL], adc_result[ADC_ACC_ROLL], - adc_result[ADC_GYRO_NICK], adc_result[ADC_ACC_NICK], - adc_result[ADC_GYRO_YAW], adc_result[ADC_ACC_YAW], - adc_result[ADC_VOLTAGE]); - - *AT91C_ADC_CR = AT91C_ADC_START; -} - -void adc_calibrate(uint32_t mode) -{ - if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) { - - adc_cal_count = ADC_CAL_COUNT; - adc_cal_mode = mode; - adc_cal_data[0] = 0; - adc_cal_data[1] = 0; - adc_cal_data[2] = 0; - pitc_schedule_timer(&adc_cal_timer); - - // wait (1024 * 1ms ~> 1s) - while (adc_cal_mode); - - if (mode == ADC_CAL_GYRO) { + if (adc_cal_mode == ADC_CAL_GYRO) { adc_offset[ADC_GYRO_ROLL] = adc_cal_data[0]; adc_offset[ADC_GYRO_NICK] = adc_cal_data[1]; adc_offset[ADC_GYRO_YAW] = adc_cal_data[2]; @@ -140,6 +102,42 @@ void adc_calibrate(uint32_t mode) adc_offset[ADC_ACC_NICK], adc_offset[ADC_ACC_YAW]); } + + adc_cal_mode = ADC_CAL_NONE; + return PITC_REMOVE_TIMER; + } + + return PITC_RESTART_TIMER; +} + +static struct pitc_timer adc_cal_timer = { + .interval = 1, + .func = &adc_calibrate_cb, +}; + +void adc_trigger(void) +{ + printf("R:%4d:%4d N:%4d:%4d Y:%4d:%4d U:%4d\n\r", + adc_result[ADC_GYRO_ROLL], adc_result[ADC_ACC_ROLL], + adc_result[ADC_GYRO_NICK], adc_result[ADC_ACC_NICK], + adc_result[ADC_GYRO_YAW], adc_result[ADC_ACC_YAW], + adc_result[ADC_VOLTAGE]); + + *AT91C_ADC_CR = AT91C_ADC_START; +} + +void adc_calibrate(uint32_t mode) +{ + if (adc_cal_mode != ADC_CAL_NONE) + return; + + if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) { + adc_cal_count = ADC_CAL_COUNT; + adc_cal_mode = mode; + adc_cal_data[0] = 0; + adc_cal_data[1] = 0; + adc_cal_data[2] = 0; + pitc_schedule_timer(&adc_cal_timer); } } @@ -183,8 +181,4 @@ void at91_adc_init(void) aic->AIC_SMR[AT91C_ID_ADC] = IRQPRIO_ADC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL; aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr; aic->AIC_IECR = (1<chan[i] = tmp; } return cnt; } +uint32_t rcontrol_getswitches(struct rc_values *rc) +{ + if (valid < 5) + return 0; + + uint32_t i; + uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data)); + for (i = 0; i < cnt; i++) { + if (ch_data[i].width > (PULSE_CENTER + PULSE_SWITCH)) + rc->chan[i] = VALUE_RANGE; + + else if (ch_data[i].width < (PULSE_CENTER - PULSE_SWITCH)) + rc->chan[i] = -VALUE_RANGE; + + else + rc->chan[i] = 0; + } + return cnt; +} + void rcontrol_calibrate(uint32_t mode) { uint32_t i;