rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

AHRS: Separated out code for debugging output from the main code and cleaned up

Browse Source 
how various sensors are used in the INS update.  Also added slow filter to
track offset between GPS and baro in outdoor mode.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2154 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-11-26 15:57:04 +00:00 committed by peabody124
parent 444dd5c311
commit 44069e64bc
3 changed files with 236 additions and 201 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

432
flight/AHRS/ahrs.c
View File

@ -167,6 +167,7 @@ void simple_update();
/* Function Prototypes */ /* Function Prototypes */
void downsample_data(void); void downsample_data(void);
void process_mag_data();
void calibrate_sensors(void); void calibrate_sensors(void);
void reset_values(); void reset_values();
void send_calibration(void); void send_calibration(void);
@ -204,14 +205,16 @@ static uint8_t adc_oversampling = 20;
*/ */
/* INS functions */ /* INS functions */
/** /**
* @brief Update the EKF when in outdoor mode. The primary difference is using the GPS values. * @brief Update the EKF when in outdoor mode. The primary difference is using the GPS values.
*/ */
void ins_outdoor_update() void ins_outdoor_update()
{ {
float gyro[3], accel[3], vel[3]; float gyro[3], accel[3], vel[3];
static uint8_t gps_dirty = 1;
static uint32_t last_gps_time = 0; static uint32_t last_gps_time = 0;
static float baro_offset = 0;
uint16_t sensors;
// format data for INS algo // format data for INS algo
gyro[0] = gyro_data.filtered.x; gyro[0] = gyro_data.filtered.x;
@ -231,45 +234,61 @@ void ins_outdoor_update()
send_position(); send_position();
INSCovariancePrediction(1 / (float)EKF_RATE); INSCovariancePrediction(1 / (float)EKF_RATE);
sensors = 0;
if (gps_data.updated) { if (gps_data.updated) {
uint32_t this_gps_time = timer_count(); uint32_t this_gps_time = timer_count();
float gps_delay; float gps_delay;
// Detect if greater than certain time since last gps update and if so vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);
// reset EKF to that position since probably drifted too far for safe vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
// update vel[2] = 0;
/*
* Detect if greater than certain time since last gps update and if so
* reset EKF to that position since probably drifted too far for safe
* update
*/
if (this_gps_time < last_gps_time) if (this_gps_time < last_gps_time)
gps_delay = ((0xFFFF - last_gps_time) - this_gps_time) / timer_rate(); gps_delay = ((0xFFFF - last_gps_time) - this_gps_time) / timer_rate();
else else
gps_delay = (this_gps_time - last_gps_time) / timer_rate(); gps_delay = (this_gps_time - last_gps_time) / timer_rate();
last_gps_time = this_gps_time; last_gps_time = this_gps_time;
gps_dirty = gps_delay > INSGPS_GPS_TIMEOUT; if(gps_delay > INSGPS_GPS_TIMEOUT)
INSPosVelReset(gps_data.NED,vel); // position stale, reset
// Compute velocity from Heading and groundspeed else {
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180); sensors |= HORIZ_SENSORS | POS_SENSORS;
vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180); }
vel[2] = 0;
/*
if (mag_data.updated && !gps_dirty) { * When using gps need to make sure that barometer is brought into NED frame
//TOOD: add check for altitude updates * we should try and see if the altitude from the home location is good enough
//FullCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude); * to use for the offset but for now starting with this conservative filter
GpsMagCorrection(mag_data.scaled.axis, gps_data.NED, vel); */
mag_data.updated = 0; if(fabs(gps_data.NED[2] + altitude_data.altitude) > 10) {
} else if (!gps_dirty) { baro_offset = gps_data.NED[2] + altitude_data.altitude;
//GpsBaroCorrection(gps_data.NED, vel, altitude_data.altitude); } else {
GpsBaroCorrection(gps_data.NED, vel, -gps_data.NED[2]); /* IIR filter with 100 second or so tau to keep them crudely in the same frame */
} else { // GPS hasn't updated for a bit baro_offset = baro_offset * 0.99 + (gps_data.NED[2] + altitude_data.altitude) * 0.01;
INSPosVelReset(gps_data.NED,vel);
} }
gps_data.updated = false; gps_data.updated = false;
} else if (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR
&& mag_data.updated == 1) {
MagCorrection(mag_data.scaled.axis); // only trust mags if outdoors
mag_data.updated = 0;
} }
if(mag_data.updated) {
sensors |= MAG_SENSORS;
mag_data.updated = false;
}
if(altitude_data.updated) {
sensors |= BARO_SENSOR;
altitude_data.updated = false;
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself
*/
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, -altitude_data.altitude - baro_offset, sensors);
} }
/** /**
@ -278,8 +297,8 @@ void ins_outdoor_update()
void ins_indoor_update() void ins_indoor_update()
{ {
float gyro[3], accel[3], vel[3]; float gyro[3], accel[3], vel[3];
static uint8_t indoor_dirty = 1;
static uint32_t last_indoor_time = 0; static uint32_t last_indoor_time = 0;
uint16_t sensors = 0;
// format data for INS algo // format data for INS algo
gyro[0] = gyro_data.filtered.x; gyro[0] = gyro_data.filtered.x;
@ -299,7 +318,7 @@ void ins_indoor_update()
send_position(); send_position();
INSCovariancePrediction(1 / (float)EKF_RATE); INSCovariancePrediction(1 / (float)EKF_RATE);
// Indoors, update with zero position and velocity and high covariance /* Indoors, update with zero position and velocity and high covariance */
vel[0] = 0; vel[0] = 0;
vel[1] = 0; vel[1] = 0;
vel[2] = 0; vel[2] = 0;
@ -307,28 +326,86 @@ void ins_indoor_update()
uint32_t this_indoor_time = timer_count(); uint32_t this_indoor_time = timer_count();
float indoor_delay; float indoor_delay;
// Detect if greater than certain time since last gps update and if so /*
// reset EKF to that position since probably drifted too far for safe * Detect if greater than certain time since last gps update and if so
// update * reset EKF to that position since probably drifted too far for safe
* update
*/
if (this_indoor_time < last_indoor_time) if (this_indoor_time < last_indoor_time)
indoor_delay = ((0xFFFF - last_indoor_time) - this_indoor_time) / timer_rate(); indoor_delay = ((0xFFFF - last_indoor_time) - this_indoor_time) / timer_rate();
else else
indoor_delay = (this_indoor_time - last_indoor_time) / timer_rate(); indoor_delay = (this_indoor_time - last_indoor_time) / timer_rate();
last_indoor_time = this_indoor_time; last_indoor_time = this_indoor_time;
if(indoor_delay > INSGPS_GPS_TIMEOUT)
INSPosVelReset(vel,vel);
else
sensors = HORIZ_SENSORS | VERT_SENSORS;
indoor_dirty = indoor_delay > INSGPS_GPS_TIMEOUT; if(mag_data.updated) {
sensors |= MAG_SENSORS;
if(indoor_dirty) mag_data.updated = false;
INSPosVelReset(vel,vel);
if((mag_data.updated == 1) && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
MagVelBaroCorrection(mag_data.scaled.axis,vel,altitude_data.altitude); // only trust mags if outdoors
mag_data.updated = 0;
} else {
VelBaroCorrection(vel, altitude_data.altitude);
} }
if(altitude_data.updated) {
sensors |= BARO_SENSOR;
altitude_data.updated = false;
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself
*/
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, -altitude_data.altitude, sensors | HORIZ_SENSORS | VERT_SENSORS);
} }
/**
* @brief Initialize the EKF assuming stationary
*/
void ins_init_algorithm()
{
float Rbe[3][3], q[4], accels[3], rpy[3], mag;
float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[13]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5};
bool using_mags, using_gps;
INSGPSInit();
HomeLocationData home;
HomeLocationGet(&home);
accels[0]=accel_data.filtered.x;
accels[1]=accel_data.filtered.y;
accels[2]=accel_data.filtered.z;
using_mags = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR);
using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */
using_gps = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality != 0);
if (using_mags){
RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
R2Quaternion(Rbe,q);
if (using_gps)
INSSetState(gps_data.NED, zeros, q, zeros);
else
INSSetState(zeros, zeros, q, zeros);
}
else{
// assume yaw = 0
mag = VectorMagnitude(accels);
rpy[1] = asinf(-accels[0]/mag);
rpy[0] = atan2(accels[1]/mag,accels[2]/mag);
rpy[2] = 0;
RPY2Quaternion(rpy,q);
if (using_gps)
INSSetState(gps_data.NED, zeros, q, zeros);
else
INSSetState(zeros, zeros, q, zeros);
}
INSResetP(Pdiag);
// TODO: include initial estimate of gyro bias?
}
/** /**
* @brief Simple update using just mag and accel. Yaw biased and big attitude changes. * @brief Simple update using just mag and accel. Yaw biased and big attitude changes.
@ -357,6 +434,74 @@ void simple_update() {
send_attitude(); send_attitude();
} }
/**
* @brief Output all the important inputs and states of the ekf through serial port
*/
#ifdef DUMP_EKF
void print_ekf_binary()
{
uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 };
// Dump raw buffer
PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header (1:16)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number (17:20)
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3); // accel data (21:32)
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3); // gyro data (33:44)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1); // mag update (45)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4); // mag data (46:57)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:85)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * NUMX); // X (86:137)
for(uint8_t i = 0; i < NUMX; i++)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &(P[i][i]), 4); // diag(P) (138:189)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); // BaroAlt (190:193)
}
#else
void print_ekf_binary() {}
#endif
/**
* @brief Debugging function to output all the ADC samples
*/
void print_ahrs_raw()
{
int result;
static int previous_conversion = 0;
uint8_t framing[16] =
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15 };
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
if (total_conversion_blocks != previous_conversion + 1)
PIOS_LED_On(LED1); // not keeping up
else
PIOS_LED_Off(LED1);
previous_conversion = total_conversion_blocks;
downsample_data();
ahrs_state = AHRS_IDLE;;
// Dump raw buffer
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & valid_data_buffer[0],
adc_oversampling *
PIOS_ADC_NUM_PINS *
sizeof(valid_data_buffer[0]));
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
}
/** /**
* @brief AHRS Main function * @brief AHRS Main function
*/ */
@ -384,6 +529,7 @@ int main()
/* Setup the Accelerometer FS (Full-Scale) GPIO */ /* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable(0); PIOS_GPIO_Enable(0);
SET_ACCEL_6G; SET_ACCEL_6G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/* Magnetic sensor system */ /* Magnetic sensor system */
PIOS_I2C_Init(); PIOS_I2C_Init();
@ -394,7 +540,6 @@ int main()
#endif #endif
reset_values(); reset_values();
INSGPSInit();
ahrs_state = AHRS_IDLE; ahrs_state = AHRS_IDLE;
AhrsInitComms(); AhrsInitComms();
@ -426,39 +571,9 @@ for all data to be up to date before doing anything*/
fir_coeffs[adc_oversampling] = adc_oversampling; fir_coeffs[adc_oversampling] = adc_oversampling;
#ifdef DUMP_RAW #ifdef DUMP_RAW
int previous_conversion;
while (1) { while (1) {
AhrsPoll(); AhrsPoll();
int result; print_ahrs_raw();
uint8_t framing[16] =
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15 };
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
if (total_conversion_blocks != previous_conversion + 1)
PIOS_LED_On(LED1); // not keeping up
else
PIOS_LED_Off(LED1);
previous_conversion = total_conversion_blocks;
downsample_data();
ahrs_state = AHRS_IDLE;;
// Dump raw buffer
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & valid_data_buffer[0],
adc_oversampling *
ADC_CONTINUOUS_CHANNELS *
sizeof(valid_data_buffer[0]));
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
} }
#endif #endif
@ -489,84 +604,29 @@ for all data to be up to date before doing anything*/
if ((total_conversion_blocks % 100) == 0) if ((total_conversion_blocks % 100) == 0)
PIOS_LED_Toggle(LED1); PIOS_LED_Toggle(LED1);
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
// Get magnetic readings
// For now don't use mags until the magnetic field is set AND until 5 seconds
// after initialization otherwise it seems to have problems
// TODO: Follow up this initialization issue
HomeLocationData home;
HomeLocationGet(&home);
if (PIOS_HMC5843_NewDataAvailable() &&
(home.Set = HOMELOCATION_SET_TRUE) &&
((home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0)) &&
((float) timer_count() / timer_rate() > 5)) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
// Swap the axis here to acount for orientation of mag chip (notice 0 and 1 swapped in raw)
mag_data.scaled.axis[0] = (mag_data.raw.axis[1] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[0] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
// Only use if magnetic length reasonable
float Blen = sqrt(pow(mag_data.scaled.axis[0],2) + pow(mag_data.scaled.axis[1],2) + pow(mag_data.scaled.axis[2],2));
if((Blen < INSGPS_MAGLEN * (1 + INSGPS_MAGTOL)) && (Blen > INSGPS_MAGLEN * (1 - INSGPS_MAGTOL)))
mag_data.updated = 1;
}
#endif
// Delay for valid data // Delay for valid data
counter_val = timer_count(); counter_val = timer_count();
running_counts = counter_val - last_counter_idle_end; running_counts = counter_val - last_counter_idle_end;
last_counter_idle_start = counter_val; last_counter_idle_start = counter_val;
while (ahrs_state != AHRS_DATA_READY) ; while (ahrs_state != AHRS_DATA_READY);
ahrs_state = AHRS_PROCESSING;
counter_val = timer_count(); counter_val = timer_count();
idle_counts = counter_val - last_counter_idle_start; idle_counts = counter_val - last_counter_idle_start;
last_counter_idle_end = counter_val; last_counter_idle_end = counter_val;
ahrs_state = AHRS_PROCESSING;
downsample_data(); downsample_data();
process_mag_data();
#ifdef DUMP_EKF
uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 };
// Dump raw buffer
PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header (1:16)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number (17:20)
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3); // accel data (21:32) print_ekf_binary();
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3); // gyro data (33:44)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1); // mag update (45)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4); // mag data (46:57)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:85)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * NUMX); // X (86:137)
for(uint8_t i = 0; i < NUMX; i++)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &(P[i][i]), 4); // diag(P) (138:189)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); // BaroAlt (190:193)
#endif
//*****************************************
//*****************************************
//*****************************************
//*****************************************
/* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */
if (ahrs_algorithm != last_ahrs_algorithm) if (ahrs_algorithm != last_ahrs_algorithm)
InitAlgorithm(); ins_init_algorithm();
last_ahrs_algorithm = ahrs_algorithm; last_ahrs_algorithm = ahrs_algorithm;
//*****************************************
//*****************************************
//*****************************************
//*****************************************
/******************** INS ALGORITHM **************************/
switch(ahrs_algorithm) { switch(ahrs_algorithm) {
case AHRSSETTINGS_ALGORITHM_SIMPLE: case AHRSSETTINGS_ALGORITHM_SIMPLE:
simple_update(); simple_update();
@ -581,28 +641,6 @@ for all data to be up to date before doing anything*/
} }
ahrs_state = AHRS_IDLE; ahrs_state = AHRS_IDLE;
#ifdef DUMP_FRIENDLY
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "b: %d\r\n",
total_conversion_blocks);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"a: %d %d %d\r\n",
(int16_t) (accel_data.filtered.x * 1000),
(int16_t) (accel_data.filtered.y * 1000),
(int16_t) (accel_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "g: %d %d %d\r\n",
(int16_t) (gyro_data.filtered.x * 1000),
(int16_t) (gyro_data.filtered.y * 1000),
(int16_t) (gyro_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"m: %d %d %d\r\n",
mag_data.raw.axis[0],
mag_data.raw.axis[1],
mag_data.raw.axis[2]);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,
"q: %d %d %d %d\r\n",
(int16_t) (Nav.q[0] * 1000),
(int16_t) (Nav.q[1] * 1000),
(int16_t) (Nav.q[2] * 1000),
(int16_t) (Nav.q[3] * 1000));
#endif
} }
return 0; return 0;
@ -693,6 +731,44 @@ void downsample_data()
AttitudeRawSet(&raw); AttitudeRawSet(&raw);
} }
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/**
* @brief Get the mag data from the I2C sensor and load into structure
* @return none
*
* This function also considers if the home location is set and has a valid
* magnetic field before updating the mag data to prevent data being used that
* cannot be interpreted. In addition the mag data is not used for the first
* five seconds to allow the filter to start to converge
*/
void process_mag_data()
{
// Get magnetic readings
// For now don't use mags until the magnetic field is set AND until 5 seconds
// after initialization otherwise it seems to have problems
// TODO: Follow up this initialization issue
HomeLocationData home;
HomeLocationGet(&home);
if (PIOS_HMC5843_NewDataAvailable() &&
(home.Set = HOMELOCATION_SET_TRUE) &&
((home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0)) &&
((float) timer_count() / timer_rate() > 5)) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
// Swap the axis here to acount for orientation of mag chip (notice 0 and 1 swapped in raw)
mag_data.scaled.axis[0] = (mag_data.raw.axis[1] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[0] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
// Only use if magnetic length reasonable
float Blen = sqrt(pow(mag_data.scaled.axis[0],2) + pow(mag_data.scaled.axis[1],2) + pow(mag_data.scaled.axis[2],2));
if((Blen < INSGPS_MAGLEN * (1 + INSGPS_MAGTOL)) && (Blen > INSGPS_MAGLEN * (1 - INSGPS_MAGTOL)))
mag_data.updated = 1;
}
}
#else
void process_mag_data() { }
#endif
/** /**
* @brief Assumes board is not moving computes biases and variances of sensors * @brief Assumes board is not moving computes biases and variances of sensors
* @returns None * @returns None
@ -998,49 +1074,7 @@ void homelocation_callback(AhrsObjHandle obj)
} }
/** /**
* @} * @}
*/ */
void InitAlgorithm()
{
float Rbe[3][3], q[4], accels[3], rpy[3], mag;
float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[13]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5};
bool using_mags, using_gps;
HomeLocationData home;
HomeLocationGet(&home);
accels[0]=accel_data.filtered.x;
accels[1]=accel_data.filtered.y;
accels[2]=accel_data.filtered.z;
using_mags = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR);
using_gps = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR);
if (using_mags){
RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
R2Quaternion(Rbe,q);
if (using_gps)
INSSetState(gps_data.NED, zeros, q, zeros);
else
INSSetState(zeros, zeros, q, zeros);
}
else{
// assume yaw = 0
mag = VectorMagnitude(accels);
rpy[1] = asinf(-accels[0]/mag);
rpy[0] = atan2(accels[1]/mag,accels[2]/mag);
rpy[2] = 0;
RPY2Quaternion(rpy,q);
if (using_gps)
INSSetState(gps_data.NED, zeros, q, zeros);
else
INSSetState(zeros, zeros, q, zeros);
}
INSResetP(Pdiag);
// TODO: include initial estimate of gyro bias?
}

3
flight/AHRS/inc/insgps.h
View File

@ -32,6 +32,8 @@
#ifndef INSGPS_H_ #ifndef INSGPS_H_
#define INSGPS_H_ #define INSGPS_H_
#include "stdint.h"
/** /**
* @addtogroup Constants * @addtogroup Constants
* @{ * @{
@ -52,6 +54,7 @@
void INSGPSInit(); void INSGPSInit();
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT); void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
void INSCovariancePrediction(float dT); void INSCovariancePrediction(float dT);
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
void INSResetP(float PDiag[13]); void INSResetP(float PDiag[13]);
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3]); void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3]);

2
flight/AHRS/insgps.c
View File

@ -46,8 +46,6 @@
#endif #endif
// Private functions // Private functions
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt, uint16_t SensorsUsed);
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]); float Q[NUMW], float dT, float P[NUMX][NUMX]);
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],