mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
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
This commit is contained in:
peabody124
peabody124
parent
6f1e7b4e41
commit
45e3ed27a7
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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++;
|
||||
}
|
||||
|
||||
@ -37,17 +37,16 @@
|
||||
#include <pios.h>
|
||||
|
||||
// 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
|
||||
|
||||
@ -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<31>s */
|
||||
/* (1 / (ADCCLK / CYCLES)) = Sample Time (<28>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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user