mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-02 10:24:11 +01:00
OP-156 AHRS: Run the accel data through a fifo so it can output raw data faster
than the ekf runs and also be more tolerant of timing jitter git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2191 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
45e3ed27a7
commit
4e8c6588b6
@ -38,6 +38,8 @@
|
||||
#include "ahrs_spi_comm.h"
|
||||
#include "insgps.h"
|
||||
#include "CoordinateConversions.h"
|
||||
#include <stdbool.h>
|
||||
#include "fifo_buffer.h"
|
||||
|
||||
#define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */
|
||||
#define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */
|
||||
@ -48,7 +50,7 @@
|
||||
// For debugging the raw sensors
|
||||
//#define DUMP_RAW
|
||||
//#define DUMP_FRIENDLY
|
||||
#define DUMP_EKF
|
||||
//#define DUMP_EKF
|
||||
|
||||
volatile int8_t ahrs_algorithm;
|
||||
|
||||
@ -59,6 +61,7 @@ void simple_update();
|
||||
|
||||
/* Data accessors */
|
||||
void adc_callback(float *);
|
||||
bool get_accel_gyro_data();
|
||||
void process_mag_data();
|
||||
void reset_values();
|
||||
void calibrate_sensors(void);
|
||||
@ -99,16 +102,19 @@ struct altitude_sensor altitude_data;
|
||||
struct gps_sensor gps_data;
|
||||
|
||||
//! The oversampling rate, ekf is 2k / this
|
||||
static uint8_t adc_oversampling = 30;
|
||||
static uint8_t adc_oversampling = 15;
|
||||
|
||||
//! 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;
|
||||
|
||||
//! Buffer to allow ADC to run a bit faster than EKF
|
||||
uint8_t adc_fifo_buf[sizeof(float) * 6 * 4] __attribute__ ((aligned(4))); // align to 32-bit to try and provide speed improvement
|
||||
t_fifo_buffer adc_fifo_buffer;
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
@ -456,6 +462,9 @@ int main()
|
||||
AHRS_ADC_Config(adc_oversampling);
|
||||
AHRS_ADC_SetCallback(adc_callback);
|
||||
|
||||
/* ADC buffer */
|
||||
fifoBuf_init(&adc_fifo_buffer, adc_fifo_buf, sizeof(adc_fifo_buf));
|
||||
|
||||
/* Setup the Accelerometer FS (Full-Scale) GPIO */
|
||||
PIOS_GPIO_Enable(0);
|
||||
SET_ACCEL_6G;
|
||||
@ -471,9 +480,7 @@ int main()
|
||||
|
||||
reset_values();
|
||||
|
||||
ahrs_state = AHRS_IDLE;
|
||||
AhrsInitComms();
|
||||
ahrs_state = AHRS_IDLE;
|
||||
while(!AhrsLinkReady()) {
|
||||
AhrsPoll();
|
||||
}
|
||||
@ -528,8 +535,8 @@ for all data to be up to date before doing anything*/
|
||||
running_counts = counter_val - last_counter_idle_end;
|
||||
last_counter_idle_start = counter_val;
|
||||
|
||||
while (ahrs_state != AHRS_DATA_READY);
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
// This function blocks till data avilable
|
||||
get_accel_gyro_data();
|
||||
|
||||
counter_val = timer_count();
|
||||
idle_counts = counter_val - last_counter_idle_start;
|
||||
@ -556,13 +563,36 @@ for all data to be up to date before doing anything*/
|
||||
ins_indoor_update();
|
||||
break;
|
||||
}
|
||||
ahrs_state = AHRS_IDLE;
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the accel and gyro data from whichever source when available
|
||||
*
|
||||
* This function will act as the HAL for the new INS sensors
|
||||
*/
|
||||
bool get_accel_gyro_data()
|
||||
{
|
||||
float accel[6];
|
||||
float gyro[6];
|
||||
while(fifoBuf_getUsed(&adc_fifo_buffer) < (sizeof(accel) + sizeof(gyro)));
|
||||
|
||||
fifoBuf_getData(&adc_fifo_buffer, &accel[0], sizeof(float) * 3);
|
||||
fifoBuf_getData(&adc_fifo_buffer, &gyro[0], sizeof(float) * 3);
|
||||
fifoBuf_getData(&adc_fifo_buffer, &accel[3], sizeof(float) * 3);
|
||||
fifoBuf_getData(&adc_fifo_buffer, &gyro[3], sizeof(float) * 3);
|
||||
|
||||
accel_data.filtered.x = (accel[0] + accel[3]) / 2;
|
||||
accel_data.filtered.y = (accel[1] + accel[4]) / 2;
|
||||
accel_data.filtered.z = (accel[2] + accel[5]) / 2;
|
||||
gyro_data.filtered.x = (gyro[0] + gyro[3]) / 2;
|
||||
gyro_data.filtered.y = (gyro[1] + gyro[4]) / 2;
|
||||
gyro_data.filtered.z = (gyro[2] + gyro[5]) / 2;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Downsample the analog data
|
||||
* @return none
|
||||
@ -577,15 +607,23 @@ for all data to be up to date before doing anything*/
|
||||
*/
|
||||
void adc_callback(float * downsampled_data)
|
||||
{
|
||||
float accel[3], gyro[3];
|
||||
// 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];
|
||||
accel[0] = (downsampled_data[2] * accel_data.calibration.scale[0]) + accel_data.calibration.bias[0];
|
||||
accel[1] = (downsampled_data[0] * accel_data.calibration.scale[1]) + accel_data.calibration.bias[1];
|
||||
accel[2] = (downsampled_data[4] * accel_data.calibration.scale[2]) + accel_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];
|
||||
gyro[0] = (downsampled_data[1] * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
|
||||
gyro[1] = (downsampled_data[3] * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
|
||||
gyro[2] = (downsampled_data[5] * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
|
||||
|
||||
if(fifoBuf_getFree(&adc_fifo_buffer) >= (sizeof(accel) + sizeof(gyro))) {
|
||||
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &accel[0], sizeof(accel));
|
||||
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &gyro[0], sizeof(gyro));
|
||||
} else {
|
||||
ekf_too_slow++;
|
||||
}
|
||||
|
||||
AttitudeRawData raw;
|
||||
|
||||
@ -598,13 +636,13 @@ void adc_callback(float * downsampled_data)
|
||||
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.gyros_filtered[0] = gyro[0] * 180 / M_PI;
|
||||
raw.gyros_filtered[1] = gyro[1] * 180 / M_PI;
|
||||
raw.gyros_filtered[2] = gyro[2] * 180 / M_PI;
|
||||
|
||||
raw.accels_filtered[0] = accel_data.filtered.x;
|
||||
raw.accels_filtered[1] = accel_data.filtered.y;
|
||||
raw.accels_filtered[2] = accel_data.filtered.z;
|
||||
raw.accels_filtered[0] = accel[0];
|
||||
raw.accels_filtered[1] = accel[1];
|
||||
raw.accels_filtered[2] = accel[2];
|
||||
|
||||
raw.magnetometers[0] = mag_data.scaled.axis[0];
|
||||
raw.magnetometers[1] = mag_data.scaled.axis[1];
|
||||
@ -622,14 +660,6 @@ void adc_callback(float * downsampled_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)
|
||||
@ -691,8 +721,8 @@ void calibrate_sensors()
|
||||
|
||||
|
||||
for (i = 0, j = 0; i < NBIAS; i++) {
|
||||
while (ahrs_state != AHRS_DATA_READY) ;
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
|
||||
get_accel_gyro_data();
|
||||
|
||||
gyro_bias[0] += gyro_data.filtered.x / NBIAS;
|
||||
gyro_bias[1] += gyro_data.filtered.y / NBIAS;
|
||||
@ -700,7 +730,6 @@ void calibrate_sensors()
|
||||
accel_bias[0] += accel_data.filtered.x / NBIAS;
|
||||
accel_bias[1] += accel_data.filtered.y / NBIAS;
|
||||
accel_bias[2] += accel_data.filtered.z / NBIAS;
|
||||
ahrs_state = AHRS_IDLE;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5843_NewDataAvailable()) {
|
||||
@ -731,8 +760,7 @@ void calibrate_sensors()
|
||||
accel_data.calibration.variance[2] = 0;
|
||||
|
||||
for (i = 0, j = 0; j < NVAR; j++) {
|
||||
while (ahrs_state != AHRS_DATA_READY) ;
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
get_accel_gyro_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;
|
||||
@ -740,7 +768,7 @@ void calibrate_sensors()
|
||||
accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
|
||||
accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
|
||||
accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
|
||||
ahrs_state = AHRS_IDLE;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5843_NewDataAvailable()) {
|
||||
j ++;
|
||||
|
||||
@ -92,8 +92,8 @@ int32_t StabilizationInitialize()
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
|
||||
// Listen for updates.
|
||||
AttitudeActualConnectQueue(queue);
|
||||
// AttitudeRawConnectQueue(queue);
|
||||
// AttitudeActualConnectQueue(queue);
|
||||
AttitudeRawConnectQueue(queue);
|
||||
|
||||
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
|
||||
SettingsUpdatedCb(StabilizationSettingsHandle());
|
||||
|
||||
Loading…
Reference in New Issue
Block a user