From 45e3ed27a730dd8f7da3348bd0fc4c6f04e5d139 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Sat, 4 Dec 2010 17:34:27 +0000 Subject: [PATCH] OP-156 AHRS_ADC: Make the ADC system use a callback to better separate ADC code from the AHRS code (which will facilitate code integration with new INS) and also will help set up a fifo queue for the downsampled data to allow gyro data output from AHRS faster than EKF output. Also decreased ADC interrupt priority so the SPI comms don't drop out. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2190 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/AHRS/ahrs.c | 120 +++++++++----------------- flight/AHRS/ahrs_adc.c | 121 +++++++++++++++++++-------- flight/AHRS/inc/ahrs_adc.h | 17 ++-- flight/PiOS/Boards/STM32103CB_AHRS.h | 2 +- 4 files changed, 136 insertions(+), 124 deletions(-) diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index b7ad3f798..a97e0f5c6 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -39,7 +39,6 @@ #include "insgps.h" #include "CoordinateConversions.h" -#define MAX_OVERSAMPLING 50 /* cannot have more than 50 samples */ #define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */ #define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */ #define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */ @@ -59,7 +58,7 @@ void ins_indoor_update(); void simple_update(); /* Data accessors */ -void downsample_data(void); +void adc_callback(float *); void process_mag_data(); void reset_values(); void calibrate_sensors(void); @@ -80,8 +79,6 @@ void settings_callback(AhrsObjHandle obj); * @{ * Public data. Used by both EKF and the sender */ -//! Filter coefficients used in decimation. Limited order so filter can't run between samples -int16_t fir_coeffs[MAX_OVERSAMPLING]; //! Contains the data from the mag sensor chip struct mag_sensor mag_data; @@ -107,6 +104,12 @@ static uint8_t adc_oversampling = 30; //! Offset correction of barometric alt, to match gps data static float baro_offset = 0; +typedef enum { AHRS_IDLE, AHRS_DATA_READY, AHRS_PROCESSING } states; +volatile states ahrs_state; +volatile int32_t ekf_too_slow; +volatile int32_t total_conversion_blocks; + + /** * @} */ @@ -389,7 +392,7 @@ void print_ekf_binary() {} */ void print_ahrs_raw() { - int result; + /*int result; static int previous_conversion = 0; uint8_t framing[16] = @@ -420,7 +423,7 @@ void print_ahrs_raw() PIOS_LED_Off(LED1); else { PIOS_LED_On(LED1); - } + } */ } /** @@ -451,6 +454,7 @@ int main() /* ADC system */ AHRS_ADC_Config(adc_oversampling); + AHRS_ADC_SetCallback(adc_callback); /* Setup the Accelerometer FS (Full-Scale) GPIO */ PIOS_GPIO_Enable(0); @@ -464,7 +468,7 @@ int main() strcpy((char *)mag_data.id, "ZZZ"); PIOS_HMC5843_ReadID(mag_data.id); #endif - + reset_values(); ahrs_state = AHRS_IDLE; @@ -472,12 +476,6 @@ int main() ahrs_state = AHRS_IDLE; while(!AhrsLinkReady()) { AhrsPoll(); - while(ahrs_state != AHRS_DATA_READY) ; - ahrs_state = AHRS_PROCESSING; - downsample_data(); - ahrs_state = AHRS_IDLE; - if((total_conversion_blocks % 10) == 0) - PIOS_LED_Toggle(LED1); } /* we didn't connect the callbacks before because we have to wait for all data to be up to date before doing anything*/ @@ -490,12 +488,6 @@ for all data to be up to date before doing anything*/ calibration_callback(AHRSCalibrationHandle()); //force an update - - /* Use simple averaging filter for now */ - for (int i = 0; i < adc_oversampling; i++) - fir_coeffs[i] = 1; - fir_coeffs[adc_oversampling] = adc_oversampling; - #ifdef DUMP_RAW while (1) { AhrsPoll(); @@ -543,7 +535,6 @@ for all data to be up to date before doing anything*/ idle_counts = counter_val - last_counter_idle_start; last_counter_idle_end = counter_val; - downsample_data(); process_mag_data(); print_ekf_binary(); @@ -584,68 +575,33 @@ for all data to be up to date before doing anything*/ * The accel_data values are converted into a coordinate system where X is forwards along * the fuselage, Y is along right the wing, and Z is down. */ -void downsample_data() +void adc_callback(float * downsampled_data) { - uint16_t i; + // Accel data is (y,x,z) in first third and fifth byte. Convert to m/s + accel_data.filtered.y = (downsampled_data[0] * accel_data.calibration.scale[1]) + accel_data.calibration.bias[1]; + accel_data.filtered.x = (downsampled_data[2] * accel_data.calibration.scale[0]) + accel_data.calibration.bias[0]; + accel_data.filtered.z = (downsampled_data[4] * accel_data.calibration.scale[2]) + accel_data.calibration.bias[2]; - // Get the Y data. Third byte in. Convert to m/s - accel_data.filtered.y = 0; - for (i = 0; i < adc_oversampling; i++) - accel_data.filtered.y += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i]; - accel_data.filtered.y /= (float) fir_coeffs[adc_oversampling]; - accel_data.filtered.y = (accel_data.filtered.y * accel_data.calibration.scale[1]) + accel_data.calibration.bias[1]; - - // Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s - accel_data.filtered.x = 0; - for (i = 0; i < adc_oversampling; i++) - accel_data.filtered.x += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i]; - accel_data.filtered.x /= (float) fir_coeffs[adc_oversampling]; - accel_data.filtered.x = (accel_data.filtered.x * accel_data.calibration.scale[0]) + accel_data.calibration.bias[0]; - - // Get the Z data. Third byte in. Convert to m/s - accel_data.filtered.z = 0; - for (i = 0; i < adc_oversampling; i++) - accel_data.filtered.z += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i]; - accel_data.filtered.z /= (float) fir_coeffs[adc_oversampling]; - accel_data.filtered.z = (accel_data.filtered.z * accel_data.calibration.scale[2]) + accel_data.calibration.bias[2]; - - // Get the X gyro data. Seventh byte in. Convert to deg/s. - gyro_data.filtered.x = 0; - for (i = 0; i < adc_oversampling; i++) - gyro_data.filtered.x += valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i]; - gyro_data.filtered.x /= fir_coeffs[adc_oversampling]; - gyro_data.filtered.x = (gyro_data.filtered.x * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0]; - - // Get the Y gyro data. Second byte in. Convert to deg/s. - gyro_data.filtered.y = 0; - for (i = 0; i < adc_oversampling; i++) - gyro_data.filtered.y += valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i]; - gyro_data.filtered.y /= fir_coeffs[adc_oversampling]; - gyro_data.filtered.y = (gyro_data.filtered.y * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1]; - - // Get the Z gyro data. Fifth byte in. Convert to deg/s. - gyro_data.filtered.z = 0; - for (i = 0; i < adc_oversampling; i++) - gyro_data.filtered.z += valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i]; - gyro_data.filtered.z /= fir_coeffs[adc_oversampling]; - gyro_data.filtered.z = (gyro_data.filtered.z * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2]; + // Gyro data is (x,y,z) in second, fifth and seventh byte. Convert to rad/s + gyro_data.filtered.x = (downsampled_data[1] * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0]; + gyro_data.filtered.y = (downsampled_data[3] * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1]; + gyro_data.filtered.z = (downsampled_data[5] * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2]; AttitudeRawData raw; - raw.gyros[0] = valid_data_buffer[1]; - raw.gyros[1] = valid_data_buffer[3]; - raw.gyros[2] = valid_data_buffer[5]; - raw.gyrotemp[0] = valid_data_buffer[6]; - raw.gyrotemp[1] = valid_data_buffer[7]; + raw.accels[0] = downsampled_data[2]; + raw.accels[1] = downsampled_data[0]; + raw.accels[2] = downsampled_data[4]; + raw.gyros[0] = downsampled_data[1]; + raw.gyros[1] = downsampled_data[3]; + raw.gyros[2] = downsampled_data[5]; + raw.gyrotemp[0] = downsampled_data[6]; + raw.gyrotemp[1] = downsampled_data[7]; raw.gyros_filtered[0] = gyro_data.filtered.x * 180 / M_PI; raw.gyros_filtered[1] = gyro_data.filtered.y * 180 / M_PI; raw.gyros_filtered[2] = gyro_data.filtered.z * 180 / M_PI; - raw.accels[0] = valid_data_buffer[2]; - raw.accels[1] = valid_data_buffer[0]; - raw.accels[2] = valid_data_buffer[4]; - raw.accels_filtered[0] = accel_data.filtered.x; raw.accels_filtered[1] = accel_data.filtered.y; raw.accels_filtered[2] = accel_data.filtered.z; @@ -666,6 +622,14 @@ void downsample_data() } AttitudeRawSet(&raw); + + if (ahrs_state == AHRS_IDLE) { + ahrs_state = AHRS_DATA_READY; + } else { + // Track how many times an interrupt occurred before EKF finished processing + ekf_too_slow++; + } + total_conversion_blocks++; } #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) @@ -729,7 +693,7 @@ void calibrate_sensors() for (i = 0, j = 0; i < NBIAS; i++) { while (ahrs_state != AHRS_DATA_READY) ; ahrs_state = AHRS_PROCESSING; - downsample_data(); + gyro_bias[0] += gyro_data.filtered.x / NBIAS; gyro_bias[1] += gyro_data.filtered.y / NBIAS; gyro_bias[2] += gyro_data.filtered.z / NBIAS; @@ -769,7 +733,7 @@ void calibrate_sensors() for (i = 0, j = 0; j < NVAR; j++) { while (ahrs_state != AHRS_DATA_READY) ; ahrs_state = AHRS_PROCESSING; - downsample_data(); + gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR; gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR; gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR; @@ -988,13 +952,7 @@ void settings_callback(AhrsObjHandle obj) settings.Downsampling = MAX_OVERSAMPLING; AHRSSettingsSet(&settings); } - AHRS_ADC_Config(adc_oversampling); - - /* Use simple averaging filter for now */ - for (int i = 0; i < adc_oversampling; i++) - fir_coeffs[i] = 1; - fir_coeffs[adc_oversampling] = adc_oversampling; - + AHRS_ADC_Config(adc_oversampling); } } diff --git a/flight/AHRS/ahrs_adc.c b/flight/AHRS/ahrs_adc.c index 8ad1228f3..b5c427a86 100644 --- a/flight/AHRS/ahrs_adc.c +++ b/flight/AHRS/ahrs_adc.c @@ -39,12 +39,18 @@ void DMA1_Channel1_IRQHandler() //! Where the raw data is stored volatile int16_t raw_data_buffer[MAX_SAMPLES]; // Double buffer that DMA just used -//! Swapped by interrupt handler to achieve double buffering -volatile int16_t *valid_data_buffer; -volatile int32_t total_conversion_blocks; -volatile int32_t ekf_too_slow; -volatile uint8_t adc_oversample; -volatile states ahrs_state; + +//! Various configuration settings +struct { + volatile int16_t *valid_data_buffer; + volatile uint8_t adc_oversample; + int16_t fir_coeffs[MAX_OVERSAMPLING]; +} adc_config; + +//! Filter coefficients used in decimation. Limited order so filter can't run between samples +float downsampled_buffer[PIOS_ADC_NUM_PINS]; + +static ADCCallback callback_function = (ADCCallback) NULL; /* Local Variables */ static GPIO_TypeDef *ADC_GPIO_PORT[PIOS_ADC_NUM_PINS] = PIOS_ADC_PORTS; @@ -69,6 +75,8 @@ uint8_t AHRS_ADC_Config(int32_t adc_oversample) int32_t i; + adc_config.adc_oversample = adc_oversample; + ADC_DeInit(ADC1); ADC_DeInit(ADC2); @@ -185,9 +193,66 @@ uint8_t AHRS_ADC_Config(int32_t adc_oversample) /* Finally start initial conversion */ ADC_SoftwareStartConvCmd(ADC1, ENABLE); + /* Use simple averaging filter for now */ + for (int i = 0; i < adc_oversample; i++) + adc_config.fir_coeffs[i] = 1; + adc_config.fir_coeffs[adc_oversample] = adc_oversample; + return 1; } +/** + * @brief Set a callback function that is executed whenever + * the ADC double buffer swaps + */ +void AHRS_ADC_SetCallback(ADCCallback new_function) +{ + callback_function = new_function; +} + +/** + * @brief Return the address of the downsampled data buffer + */ +float * AHRS_ADC_GetBuffer() +{ + return downsampled_buffer; +} + +/** + * @brief Set the fir coefficients. Takes as many samples as the + * current filter order plus one (normalization) + * + * @param new_filter Array of adc_oversampling floats plus one for the + * filter coefficients + */ +void AHRS_ADC_SetFIRCoefficients(float * new_filter) +{ + // Less than or equal to get normalization constant + for(int i = 0; i <= adc_config.adc_oversample; i++) + adc_config.fir_coeffs[i] = new_filter[i]; +} + +/** + * @brief Downsample the data for each of the channels then call + * callback function if installed + */ +void AHRS_ADC_downsample_data() +{ + uint16_t chan; + uint16_t sample; + + for (chan = 0; chan < PIOS_ADC_NUM_CHANNELS; chan++) { + downsampled_buffer[chan] = 0; + for (sample = 0; sample < adc_config.adc_oversample; sample++) { + downsampled_buffer[chan] += adc_config.valid_data_buffer[chan + sample * PIOS_ADC_NUM_CHANNELS] * adc_config.fir_coeffs[sample]; + } + downsampled_buffer[chan] /= (float) adc_config.fir_coeffs[adc_config.adc_oversample]; + } + + if(callback_function != NULL) + callback_function(downsampled_buffer); +} + /** * @brief Interrupt for half and full buffer transfer * @@ -199,32 +264,22 @@ uint8_t AHRS_ADC_Config(int32_t adc_oversample) */ void AHRS_ADC_DMA_Handler(void) { - if (ahrs_state == AHRS_IDLE) { - // Ideally this would have a mutex, but I think we can avoid it (and don't have RTOS features) - - if (DMA_GetFlagStatus(DMA1_IT_TC1)) { // whole double buffer filled - valid_data_buffer = - &raw_data_buffer[1 * PIOS_ADC_NUM_CHANNELS * - adc_oversample]; - DMA_ClearFlag(DMA1_IT_TC1); - } - else if (DMA_GetFlagStatus(DMA1_IT_HT1)) { - valid_data_buffer = - &raw_data_buffer[0 * PIOS_ADC_NUM_CHANNELS * - adc_oversample]; - DMA_ClearFlag(DMA1_IT_HT1); - } - else { - // This should not happen, probably due to transfer errors - DMA_ClearFlag(DMA1_FLAG_GL1); - } - - ahrs_state = AHRS_DATA_READY; - } else { - // Track how many times an interrupt occurred before EKF finished processing - ekf_too_slow++; - DMA_ClearFlag(DMA1_IT_GL1); + if (DMA_GetFlagStatus(DMA1_IT_TC1)) { // whole double buffer filled + adc_config.valid_data_buffer = + &raw_data_buffer[1 * PIOS_ADC_NUM_CHANNELS * + adc_config.adc_oversample]; + DMA_ClearFlag(DMA1_IT_TC1); + AHRS_ADC_downsample_data(); + } + else if (DMA_GetFlagStatus(DMA1_IT_HT1)) { + adc_config.valid_data_buffer = + &raw_data_buffer[0 * PIOS_ADC_NUM_CHANNELS * + adc_config.adc_oversample]; + DMA_ClearFlag(DMA1_IT_HT1); + AHRS_ADC_downsample_data(); + } + else { + // This should not happen, probably due to transfer errors + DMA_ClearFlag(DMA1_FLAG_GL1); } - - total_conversion_blocks++; } diff --git a/flight/AHRS/inc/ahrs_adc.h b/flight/AHRS/inc/ahrs_adc.h index b541b1f08..c925a124d 100644 --- a/flight/AHRS/inc/ahrs_adc.h +++ b/flight/AHRS/inc/ahrs_adc.h @@ -37,17 +37,16 @@ #include // Maximum of 50 oversampled points -#define MAX_SAMPLES (8*50*2) +#define MAX_OVERSAMPLING 50 /* cannot have more than 50 samples */ +#define MAX_SAMPLES (PIOS_ADC_NUM_CHANNELS*MAX_OVERSAMPLING*2) +typedef void (*ADCCallback) (float * data); + +// Public API: uint8_t AHRS_ADC_Config(int32_t adc_oversample); void AHRS_ADC_DMA_Handler(void); - -typedef enum { AHRS_IDLE, AHRS_DATA_READY, AHRS_PROCESSING } states; -extern volatile states ahrs_state; -extern volatile int16_t *valid_data_buffer; -//! Counts how many times the EKF wasn't idle when DMA handler called -extern volatile int32_t total_conversion_blocks; -//! Total number of data blocks converted -extern volatile int32_t ekf_too_slow; +void AHRS_ADC_SetCallback(ADCCallback); +void AHRS_ADC_SetFIRCoefficients(float * new_filter); +float * AHRS_ADC_GetBuffer(); #endif diff --git a/flight/PiOS/Boards/STM32103CB_AHRS.h b/flight/PiOS/Boards/STM32103CB_AHRS.h index 0f8510a62..85c432700 100644 --- a/flight/PiOS/Boards/STM32103CB_AHRS.h +++ b/flight/PiOS/Boards/STM32103CB_AHRS.h @@ -222,7 +222,7 @@ TIM8 | | | | /* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */ /* Tconv = 239.5 + 12.5 = 252 cycles = 18�s */ /* (1 / (ADCCLK / CYCLES)) = Sample Time (�S) */ -#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_HIGH +#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW // Currently analog acquistion hard coded at 480 Hz // PCKL2 = HCLK / 16