diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 6e9695bbf..38c704639 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -39,7 +39,9 @@ #include "insgps.h" #include "CoordinateConversions.h" -#define MAX_OVERSAMPLING 50 +#define MAX_OVERSAMPLING 50 /* cannot have more than 50 samples */ +#define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */ + // For debugging the raw sensors //#define DUMP_RAW //#define DUMP_FRIENDLY @@ -195,7 +197,9 @@ int main() { float gyro[3], accel[3]; float vel[3] = { 0, 0, 0 }; + uint8_t gps_dirty = 1; gps_data.quality = -1; + uint32_t last_gps_time = 0; ahrs_algorithm = AHRSSETTINGS_ALGORITHM_SIMPLE; @@ -223,10 +227,6 @@ int main() PIOS_HMC5843_ReadID(mag_data.id); #endif - /* SPI link to master */ -// PIOS_SPI_Init(); - -// lfsm_init(); reset_values(); INSGPSInit(); @@ -360,16 +360,31 @@ for all data to be up to date before doing anything*/ INSCovariancePrediction(1 / (float)EKF_RATE); if (gps_data.updated && ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) { - // Compute velocity from Heading and groundspeed - vel[0] = - gps_data.groundspeed * - cos(gps_data.heading * M_PI / 180); - vel[1] = - gps_data.groundspeed * - sin(gps_data.heading * M_PI / 180); + uint32_t this_gps_time = timer_count(); + float gps_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 + // update + if (this_gps_time < last_gps_time) + gps_delay = ((0xFFFF - last_gps_time) - this_gps_time) / timer_rate(); + else + gps_delay = (this_gps_time - last_gps_time) / timer_rate(); + last_gps_time = this_gps_time; + + gps_dirty = gps_delay > INSGPS_GPS_TIMEOUT; + // Compute velocity from Heading and groundspeed + vel[0] = gps_data.groundspeed * + cos(gps_data.heading * M_PI / 180); + vel[1] = gps_data.groundspeed * + sin(gps_data.heading * M_PI / 180); + vel[2] = 0; + + INSSetPosVelVar(0.004); - if (mag_data.updated) { + + if (mag_data.updated && !gps_dirty) { //TOOD: add check for altitude updates FullCorrection(mag_data.scaled.axis, gps_data.NED, @@ -377,11 +392,13 @@ for all data to be up to date before doing anything*/ altitude_data. altitude); mag_data.updated = 0; - } else { + } else if (!gps_dirty) { GpsBaroCorrection(gps_data.NED, vel, altitude_data. altitude); + } else { // GPS hasn't updated for a bit + INSPosVelReset(gps_data.NED,vel); } gps_data.updated = false; @@ -830,14 +847,13 @@ void gps_callback(AhrsObjHandle obj) HomeLocationData home; HomeLocationGet(&home); - if(ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) { - return; - } - - if(pos.Status != GPSPOSITION_STATUS_FIX3D) //FIXME: Will this work? the old ahrs_comms does it differently. + if((ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || + (pos.Status != GPSPOSITION_STATUS_FIX3D) || + (pos.Satellites < 7) || + (pos.PDOP < 3.5)) { gps_data.quality = 0; - gps_data.updated = true; + gps_data.updated = false; return; } @@ -848,8 +864,8 @@ void gps_callback(AhrsObjHandle obj) gps_data.heading = pos.Heading; gps_data.groundspeed = pos.Groundspeed; - gps_data.quality = 1; - gps_data.updated = true; + gps_data.quality = 1; /* currently unused */ + gps_data.updated = true; } void altitude_callback(AhrsObjHandle obj) diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h index 0e2277a5f..84f3ce201 100644 --- a/flight/AHRS/inc/insgps.h +++ b/flight/AHRS/inc/insgps.h @@ -59,6 +59,7 @@ void INSSetAccelVar(float accel_var[3]); void INSSetGyroVar(float gyro_var[3]); void INSSetMagNorth(float B[3]); void INSSetMagVar(float scaled_mag_var[3]); +void INSPosVelReset(float pos[3], float vel[3]); void MagCorrection(float mag_data[3]); void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt); diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c index 1bcefa104..af28d08e0 100644 --- a/flight/AHRS/insgps.c +++ b/flight/AHRS/insgps.c @@ -76,6 +76,12 @@ void INSGPSInit() //pretty much just a place holder for now Be[1] = 0; Be[2] = 0; // local magnetic unit vector + for (int i = 0; i < NUMX; i++) { + for (int j = 0; j < NUMX; j++) { + P[i][j] = 0; // zero all terms + } + } + P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2) P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2 P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance @@ -98,6 +104,26 @@ void INSGPSInit() //pretty much just a place holder for now R[9] = .05; // High freq altimeter noise variance (m^2) } +void INSPosVelReset(float pos[3], float vel[3]) +{ + for (int i = 0; i < 6; i++) { + for(int j = i; j < NUMX; j++) { + P[i][j] = 0; // zero the first 6 rows and columns + P[j][i] = 0; + } + } + + P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2) + P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2 + + X[0] = pos[0]; + X[1] = pos[1]; + X[2] = pos[2]; + X[3] = vel[0]; + X[4] = vel[1]; + X[5] = vel[2]; +} + void INSSetPosVelVar(float PosVar) { R[0] = PosVar;