flight/libraries/insgps_helper.c

   1 #include "ins.h"
   2 #include "pios.h"
   3 #include "ahrs_spi_comm.h"
   4 #include "insgps.h"
   5 #include "CoordinateConversions.h"
   6
   7 #define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */
   8 #define INSGPS_GPS_MINSAT  6   /* 2 seconds triggers reinit of position */
   9 #define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates    */
  10 /* If GPS is more than this distance on any dimension then wait a few updates */
  11 /* and reinitialize there */
  12 #define INSGPS_GPS_FAR     10
  13
  14 // ! Contains the data from the mag sensor chip
  15 extern struct mag_sensor mag_data;
  16
  17 // ! Contains the data from the accelerometer
  18 extern struct accel_sensor accel_data;
  19
  20 // ! Contains the data from the gyro
  21 extern struct gyro_sensor gyro_data;
  22
  23 // ! Conains the current estimate of the attitude
  24 extern struct attitude_solution attitude_data;
  25
  26 // ! Contains data from the altitude sensor
  27 extern struct altitude_sensor altitude_data;
  28
  29 // ! Contains data from the GPS (via the SPI link)
  30 extern struct gps_sensor gps_data;
  31
  32 // ! Offset correction of barometric alt, to match gps data
  33 static float baro_offset = 0;
  34
  35 extern void send_calibration(void);
  36 extern void send_attitude(void);
  37 extern void send_velocity(void);
  38 extern void send_position(void);
  39 extern volatile int8_t ahrs_algorithm;
  40 extern void get_accel_gyro_data();
  41 extern void get_mag_data();
  42
  43 /* INS functions */
  44 /**
  45  * @brief Update the EKF when in outdoor mode.  The primary difference is using the GPS values.
  46  */
  47 uint32_t total_far_count = 0;
  48 uint32_t relocated = 0;
  49 void ins_outdoor_update()
  50 {
  51     static uint32_t ins_last_time;
  52     float gyro[3], accel[3], vel[3];
  53     float dT;
  54     uint16_t sensors;
  55     static uint32_t gps_far_count = 0;
  56
  57     dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6;
  58     ins_last_time = PIOS_DELAY_GetRaw();
  59
  60     // This should only happen at start up or at mode switches
  61     if (dT > 0.01) {
  62         dT = 0.01;
  63     }
  64
  65     // format data for INS algo
  66     gyro[0]  = gyro_data.filtered.x;
  67     gyro[1]  = gyro_data.filtered.y;
  68     gyro[2]  = gyro_data.filtered.z;
  69     accel[0] = accel_data.filtered.x,
  70     accel[1] = accel_data.filtered.y,
  71     accel[2] = accel_data.filtered.z,
  72
  73     INSStatePrediction(gyro, accel, dT);
  74     attitude_data.quaternion.q1 = Nav.q[0];
  75     attitude_data.quaternion.q2 = Nav.q[1];
  76     attitude_data.quaternion.q3 = Nav.q[2];
  77     attitude_data.quaternion.q4 = Nav.q[3];
  78     send_attitude(); // get message out quickly
  79     INSCovariancePrediction(dT);
  80
  81     PositionActualData positionActual;
  82     PositionActualGet(&positionActual);
  83     positionActual.North = Nav.Pos[0];
  84     positionActual.East  = Nav.Pos[1];
  85     positionActual.Down  = Nav.Pos[2];
  86     PositionActualSet(&positionActual);
  87
  88     VelocityActualData velocityActual;
  89     VelocityActualGet(&velocityActual);
  90     velocityActual.North = Nav.Vel[0];
  91     velocityActual.East  = Nav.Vel[1];
  92     velocityActual.Down  = Nav.Vel[2];
  93     VelocityActualSet(&velocityActual);
  94
  95     sensors = 0;
  96
  97     if (gps_data.updated) {
  98         vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
  99         vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
 100         vel[2] = 0;
 101
 102         if (abs(gps_data.NED[0] - Nav.Pos[0]) > INSGPS_GPS_FAR ||
 103             abs(gps_data.NED[1] - Nav.Pos[1]) > INSGPS_GPS_FAR ||
 104             abs(gps_data.NED[2] - Nav.Pos[2]) > INSGPS_GPS_FAR ||
 105             abs(vel[0] - Nav.Vel[0]) > INSGPS_GPS_FAR ||
 106             abs(vel[1] - Nav.Vel[1]) > INSGPS_GPS_FAR) {
 107             gps_far_count++;
 108             total_far_count++;
 109             gps_data.updated = false;
 110
 111             if (gps_far_count > 30) {
 112                 INSPosVelReset(gps_data.NED, vel);
 113                 relocated++;
 114             }
 115         } else {
 116             sensors |= HORIZ_SENSORS | POS_SENSORS;
 117
 118             /*
 119              * When using gps need to make sure that barometer is brought into NED frame
 120              * we should try and see if the altitude from the home location is good enough
 121              * to use for the offset but for now starting with this conservative filter
 122              */
 123             if (fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) {
 124                 baro_offset = gps_data.NED[2] + altitude_data.altitude;
 125             } else {
 126                 /* IIR filter with 100 second or so tau to keep them crudely in the same frame */
 127                 baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
 128             }
 129             gps_data.updated = false;
 130         }
 131     }
 132
 133     if (mag_data.updated) {
 134         sensors |= MAG_SENSORS;
 135         mag_data.updated = false;
 136     }
 137
 138     if (altitude_data.updated) {
 139         sensors |= BARO_SENSOR;
 140         altitude_data.updated = false;
 141     }
 142
 143     /*
 144      * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
 145      * although probably should occur within INS itself
 146      */
 147     INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors);
 148
 149     if (fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) {
 150         float zeros[3] = { 0, 0, 0 };
 151         INSSetGyroBias(zeros);
 152     }
 153 }
 154
 155 /**
 156  * @brief Update the EKF when in indoor mode
 157  */
 158 void ins_indoor_update()
 159 {
 160     static uint32_t updated_without_gps = 0;
 161
 162     float gyro[3], accel[3];
 163     float zeros[3]   = { 0, 0, 0 };
 164     static uint32_t ins_last_time = 0;
 165     uint16_t sensors = 0;
 166     float dT;
 167
 168     dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6;
 169     ins_last_time = PIOS_DELAY_GetRaw();
 170
 171     // This should only happen at start up or at mode switches
 172     if (dT > 0.01) {
 173         dT = 0.01;
 174     }
 175
 176     // format data for INS algo
 177     gyro[0]  = gyro_data.filtered.x;
 178     gyro[1]  = gyro_data.filtered.y;
 179     gyro[2]  = gyro_data.filtered.z;
 180     accel[0] = accel_data.filtered.x,
 181     accel[1] = accel_data.filtered.y,
 182     accel[2] = accel_data.filtered.z,
 183
 184     INSStatePrediction(gyro, accel, dT);
 185     attitude_data.quaternion.q1 = Nav.q[0];
 186     attitude_data.quaternion.q2 = Nav.q[1];
 187     attitude_data.quaternion.q3 = Nav.q[2];
 188     attitude_data.quaternion.q4 = Nav.q[3];
 189     send_attitude(); // get message out quickly
 190     INSCovariancePrediction(dT);
 191
 192     /* Indoors, update with zero position and velocity and high covariance */
 193     sensors = HORIZ_SENSORS | VERT_SENSORS;
 194
 195     if (mag_data.updated && (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
 196         sensors |= MAG_SENSORS;
 197         mag_data.updated = false;
 198     }
 199
 200     if (altitude_data.updated) {
 201         sensors |= BARO_SENSOR;
 202         altitude_data.updated = false;
 203     }
 204
 205     if (gps_data.updated) {
 206         PositionActualData positionActual;
 207         PositionActualGet(&positionActual);
 208         positionActual.North = gps_data.NED[0];
 209         positionActual.East  = gps_data.NED[1];
 210         positionActual.Down  = Nav.Pos[2];
 211         PositionActualSet(&positionActual);
 212
 213         VelocityActualData velocityActual;
 214         VelocityActualGet(&velocityActual);
 215         velocityActual.North = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
 216         velocityActual.East  = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
 217         velocityActual.Down  = Nav.Vel[2];
 218         VelocityActualSet(&velocityActual);
 219
 220         updated_without_gps  = 0;
 221         gps_data.updated     = false;
 222     } else {
 223         PositionActualData positionActual;
 224         PositionActualGet(&positionActual);
 225
 226         VelocityActualData velocityActual;
 227         VelocityActualGet(&velocityActual);
 228
 229         positionActual.Down = Nav.Pos[2];
 230         velocityActual.Down = Nav.Vel[2];
 231
 232         if (updated_without_gps > 500) {
 233             // After 2-3 seconds without a GPS update set velocity estimate to NAN
 234             positionActual.North = NAN;
 235             positionActual.East  = NAN;
 236             velocityActual.North = NAN;
 237             velocityActual.East  = NAN;
 238         } else {
 239             updated_without_gps++;
 240         }
 241
 242         PositionActualSet(&positionActual);
 243         VelocityActualSet(&velocityActual);
 244     }
 245
 246     /*
 247      * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
 248      * although probably should occur within INS itself
 249      */
 250     INSCorrection(mag_data.scaled.axis, zeros, zeros, altitude_data.altitude, sensors);
 251
 252     if (fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) {
 253         float zeros[3] = { 0, 0, 0 };
 254         INSSetGyroBias(zeros);
 255     }
 256 }
 257
 258 /**
 259  * @brief Initialize the EKF assuming stationary
 260  */
 261 bool inited = false;
 262 float init_q[4];
 263 void ins_init_algorithm()
 264 {
 265     inited = true;
 266     float Rbe[3][3], q[4], accels[3], rpy[3], mag;
 267     float ge[3] = { 0, 0, -9.81 }, zeros[3] = { 0, 0, 0 }, Pdiag[16] = { 25, 25, 25, 5, 5, 5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-4, 1e-4, 1e-4 };
 268     bool using_mags, using_gps;
 269
 270     INSGPSInit();
 271
 272     HomeLocationData home;
 273     HomeLocationGet(&home);
 274
 275     accels[0]   = accel_data.filtered.x;
 276     accels[1]   = accel_data.filtered.y;
 277     accels[2]   = accel_data.filtered.z;
 278
 279     using_mags  = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR);
 280     using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */
 281
 282     using_gps   = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality >= INSGPS_GPS_MINSAT);
 283
 284     /* Block till a data update */
 285     get_accel_gyro_data();
 286
 287     /* Ensure we get mag data in a timely manner */
 288     uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec
 289     while (using_mags && !mag_data.updated && fail_count--) {
 290         get_mag_data();
 291         get_accel_gyro_data();
 292         AhrsPoll();
 293         PIOS_DELAY_WaituS(2000);
 294     }
 295     using_mags &= mag_data.updated;
 296
 297     if (using_mags) {
 298         RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
 299         R2Quaternion(Rbe, q);
 300         if (using_gps) {
 301             INSSetState(gps_data.NED, zeros, q, zeros, zeros);
 302         } else {
 303             INSSetState(zeros, zeros, q, zeros, zeros);
 304         }
 305     } else {
 306         // assume yaw = 0
 307         mag    = VectorMagnitude(accels);
 308         rpy[1] = asinf(-accels[0] / mag);
 309         rpy[0] = atan2(accels[1] / mag, accels[2] / mag);
 310         rpy[2] = 0;
 311         RPY2Quaternion(rpy, init_q);
 312         if (using_gps) {
 313             INSSetState(gps_data.NED, zeros, init_q, zeros, zeros);
 314         } else {
 315             for (uint32_t i = 0; i < 5; i++) {
 316                 INSSetState(zeros, zeros, init_q, zeros, zeros);
 317                 ins_indoor_update();
 318             }
 319         }
 320     }
 321
 322     INSResetP(Pdiag);
 323
 324     // TODO: include initial estimate of gyro bias?
 325 }