|
|
@ -77,8 +77,33 @@ 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; |
|
|
|
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]; |
|
|
|
|
|
|
|
// TODO: check for invalid values (not centered) -> return value? |
|
|
|
|
|
|
|
printf("GYRO offsets: %4d/%4d/%4d\n\r", |
|
|
|
adc_offset[ADC_GYRO_ROLL], |
|
|
|
adc_offset[ADC_GYRO_NICK], |
|
|
|
adc_offset[ADC_GYRO_YAW]); |
|
|
|
|
|
|
|
} else { |
|
|
|
adc_offset[ADC_ACC_ROLL] = adc_cal_data[0]; |
|
|
|
adc_offset[ADC_ACC_NICK] = adc_cal_data[1]; |
|
|
|
|
|
|
|
// TODO: only 1/2 offset? |
|
|
|
adc_offset[ADC_ACC_YAW] = adc_cal_data[2]; |
|
|
|
|
|
|
|
// TODO: save to eeprom |
|
|
|
printf("ACC offsets: %4d/%4d/%4d\n\r", |
|
|
|
adc_offset[ADC_ACC_ROLL], |
|
|
|
adc_offset[ADC_ACC_NICK], |
|
|
|
adc_offset[ADC_ACC_YAW]); |
|
|
|
} |
|
|
|
|
|
|
|
adc_cal_mode = ADC_CAL_NONE; |
|
|
|
return PITC_REMOVE_TIMER; |
|
|
|
} |
|
|
|
|
|
|
@ -103,43 +128,16 @@ void adc_trigger(void) |
|
|
|
|
|
|
|
void adc_calibrate(uint32_t mode) |
|
|
|
{ |
|
|
|
if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) { |
|
|
|
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); |
|
|
|
|
|
|
|
// wait (1024 * 1ms ~> 1s) |
|
|
|
while (adc_cal_mode); |
|
|
|
|
|
|
|
if (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]; |
|
|
|
|
|
|
|
// TODO: check for invalid values (not centered) -> return value? |
|
|
|
|
|
|
|
printf("GYRO offsets: %4d/%4d/%4d\n\r", |
|
|
|
adc_offset[ADC_GYRO_ROLL], |
|
|
|
adc_offset[ADC_GYRO_NICK], |
|
|
|
adc_offset[ADC_GYRO_YAW]); |
|
|
|
|
|
|
|
} else { |
|
|
|
adc_offset[ADC_ACC_ROLL] = adc_cal_data[0]; |
|
|
|
adc_offset[ADC_ACC_NICK] = adc_cal_data[1]; |
|
|
|
|
|
|
|
// TODO: only 1/2 offset? |
|
|
|
adc_offset[ADC_ACC_YAW] = adc_cal_data[2]; |
|
|
|
|
|
|
|
// TODO: save to eeprom |
|
|
|
printf("ACC offsets: %4d/%4d/%4d\n\r", |
|
|
|
adc_offset[ADC_ACC_ROLL], |
|
|
|
adc_offset[ADC_ACC_NICK], |
|
|
|
adc_offset[ADC_ACC_YAW]); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
@ -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<<AT91C_ID_ADC); |
|
|
|
|
|
|
|
// TODO: trigger via rc |
|
|
|
adc_calibrate(ADC_CAL_GYRO); |
|
|
|
adc_calibrate(ADC_CAL_ACC); |
|
|
|
} |