diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c index f5a4ee60d..3ed89b99d 100644 --- a/flight/modules/StateEstimation/filterair.c +++ b/flight/modules/StateEstimation/filterair.c @@ -74,11 +74,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) struct data *this = (struct data *)self->localdata; // take static pressure altitude estimation for - if (ISSET(state->updated, SENSORUPDATES_baro)) { + if (IS_SET(state->updated, SENSORUPDATES_baro)) { this->altitude = state->baro[0]; } // calculate true airspeed estimation - if (ISSET(state->updated, SENSORUPDATES_airspeed)) { + if (IS_SET(state->updated, SENSORUPDATES_airspeed)) { state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude); } diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index b8b8f14be..0d8e83f67 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -79,20 +79,20 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->first_run) { // Initialize to current altitude reading at initial location - if (ISSET(state->updated, SENSORUPDATES_baro)) { + if (IS_SET(state->updated, SENSORUPDATES_baro)) { this->first_run = 0; this->baroOffset = state->baro[0]; } } else { // Track barometric altitude offset with a low pass filter // based on GPS altitude if available - if (ISSET(state->updated, SENSORUPDATES_pos)) { + if (IS_SET(state->updated, SENSORUPDATES_pos)) { this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset + (1.0f - BARO_OFFSET_LOWPASS_ALPHA) * (-state->pos[2] - state->baro[0]); } // calculate bias corrected altitude - if (ISSET(state->updated, SENSORUPDATES_baro)) { + if (IS_SET(state->updated, SENSORUPDATES_baro)) { state->baro[0] -= this->baroOffset; } } diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 27ebbcce9..32ccc1a72 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -158,19 +158,19 @@ static int32_t filter(stateFilter *self, stateEstimation *state) int32_t result = 0; - if (ISSET(state->updated, SENSORUPDATES_mag)) { + if (IS_SET(state->updated, SENSORUPDATES_mag)) { this->magUpdated = 1; this->currentMag[0] = state->mag[0]; this->currentMag[1] = state->mag[1]; this->currentMag[2] = state->mag[2]; } - if (ISSET(state->updated, SENSORUPDATES_accel)) { + if (IS_SET(state->updated, SENSORUPDATES_accel)) { this->accelUpdated = 1; this->currentAccel[0] = state->accel[0]; this->currentAccel[1] = state->accel[1]; this->currentAccel[2] = state->accel[2]; } - if (ISSET(state->updated, SENSORUPDATES_gyro)) { + if (IS_SET(state->updated, SENSORUPDATES_gyro)) { if (this->accelUpdated) { float attitude[4]; result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude); diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 34fd3204d..bbbb870af 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -193,7 +193,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // Get most recent data #define UPDATE(shortname, num) \ - if (ISSET(state->updated, SENSORUPDATES_##shortname)) { \ + if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \ uint8_t t; \ for (t = 0; t < num; t++) { \ this->work.shortname[t] = state->shortname[t]; \ @@ -210,11 +210,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // check whether mandatory updates are present accels must have been supplied already, // and gyros must be supplied just now for a prediction step to take place // ("gyros last" rule for multi object synchronization) - if (!(ISSET(this->work.updated, SENSORUPDATES_accel) && ISSET(state->updated, SENSORUPDATES_gyro))) { - UNSET(state->updated, SENSORUPDATES_pos); - UNSET(state->updated, SENSORUPDATES_vel); - UNSET(state->updated, SENSORUPDATES_attitude); - UNSET(state->updated, SENSORUPDATES_gyro); + if (!(IS_SET(this->work.updated, SENSORUPDATES_accel) && IS_SET(state->updated, SENSORUPDATES_gyro))) { + UNSET_MASK(state->updated, SENSORUPDATES_pos); + UNSET_MASK(state->updated, SENSORUPDATES_vel); + UNSET_MASK(state->updated, SENSORUPDATES_attitude); + UNSET_MASK(state->updated, SENSORUPDATES_gyro); return 0; } @@ -226,10 +226,10 @@ static int32_t filter(stateFilter *self, stateEstimation *state) (gpsData.PDOP > 4.0f) || (gpsData.Latitude == 0 && gpsData.Longitude == 0) || (this->homeLocation.Set != HOMELOCATION_SET_TRUE)) { - UNSET(state->updated, SENSORUPDATES_pos); - UNSET(state->updated, SENSORUPDATES_vel); - UNSET(this->work.updated, SENSORUPDATES_pos); - UNSET(this->work.updated, SENSORUPDATES_vel); + UNSET_MASK(state->updated, SENSORUPDATES_pos); + UNSET_MASK(state->updated, SENSORUPDATES_vel); + UNSET_MASK(this->work.updated, SENSORUPDATES_pos); + UNSET_MASK(this->work.updated, SENSORUPDATES_vel); } } @@ -243,7 +243,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) dT = 0.001f; } - if (!this->inited && ISSET(this->work.updated, SENSORUPDATES_mag) && ISSET(this->work.updated, SENSORUPDATES_baro) && ISSET(this->work.updated, SENSORUPDATES_pos)) { + if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) { // Don't initialize until all sensors are read if (this->init_stage == 0) { // Reset the INS algorithm @@ -362,11 +362,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // Advance the covariance estimate INSCovariancePrediction(dT); - if (ISSET(this->work.updated, SENSORUPDATES_mag)) { + if (IS_SET(this->work.updated, SENSORUPDATES_mag)) { sensors |= MAG_SENSORS; } - if (ISSET(this->work.updated, SENSORUPDATES_baro)) { + if (IS_SET(this->work.updated, SENSORUPDATES_baro)) { sensors |= BARO_SENSOR; } @@ -392,15 +392,15 @@ static int32_t filter(stateFilter *self, stateEstimation *state) ); } - if (ISSET(this->work.updated, SENSORUPDATES_pos)) { + if (IS_SET(this->work.updated, SENSORUPDATES_pos)) { sensors |= POS_SENSORS; } - if (ISSET(this->work.updated, SENSORUPDATES_vel)) { + if (IS_SET(this->work.updated, SENSORUPDATES_vel)) { sensors |= HORIZ_SENSORS | VERT_SENSORS; } - if (ISSET(this->work.updated, SENSORUPDATES_airspeed) && ((!ISSET(this->work.updated, SENSORUPDATES_vel) && !ISSET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) { + if (IS_SET(this->work.updated, SENSORUPDATES_airspeed) && ((!IS_SET(this->work.updated, SENSORUPDATES_vel) && !IS_SET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) { // HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance sensors |= HORIZ_SENSORS | VERT_SENSORS; INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index ceec76747..8c62cf418 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -79,7 +79,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; - if (ISSET(state->updated, SENSORUPDATES_mag)) { + if (IS_SET(state->updated, SENSORUPDATES_mag)) { if (this->revoCalibration.MagBiasNullingRate > 0) { magOffsetEstimation(this, state->mag); } diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index a334d64ac..9b954874a 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -55,10 +55,6 @@ typedef struct { sensorUpdates updated; } stateEstimation; -#define ISSET(bitfield, bit) ((bitfield) & (bit) ? 1 : 0) -#define UNSET(bitfield, bit) (bitfield) &= ~(bit) - - typedef struct stateFilterStruct { int32_t (*init)(struct stateFilterStruct *self); int32_t (*filter)(struct stateFilterStruct *self, stateEstimation *state); diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 82a4dad6c..3b2a413e6 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include @@ -57,7 +57,7 @@ #define STACK_SIZE_BYTES 256 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR #define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL -#define TIMEOUT_MS 100 +#define TIMEOUT_MS 10 // Private types @@ -181,8 +181,6 @@ static const filterPipeline *ekf16Queue = &(filterPipeline) { static void settingsUpdatedCb(UAVObjEvent *objEv); static void sensorUpdatedCb(UAVObjEvent *objEv); static void StateEstimationCb(void); -static void getNED(GPSPositionSensorData *gpsPosition, float *NED); -static bool sane(float value); static inline int32_t maxint32_t(int32_t a, int32_t b) { @@ -205,7 +203,7 @@ int32_t StateEstimationInitialize(void) MagSensorInitialize(); BaroSensorInitialize(); AirspeedSensorInitialize(); - GPSPositionSensorInitialize(); + PositionSensorInitialize(); GPSVelocitySensorInitialize(); GyroStateInitialize(); @@ -223,7 +221,7 @@ int32_t StateEstimationInitialize(void) MagSensorConnectCallback(&sensorUpdatedCb); BaroSensorConnectCallback(&sensorUpdatedCb); AirspeedSensorConnectCallback(&sensorUpdatedCb); - GPSPositionSensorConnectCallback(&sensorUpdatedCb); + PositionSensorConnectCallback(&sensorUpdatedCb); GPSVelocitySensorConnectCallback(&sensorUpdatedCb); uint32_t stack_required = STACK_SIZE_BYTES; @@ -271,6 +269,7 @@ static void StateEstimationCb(void) static uint16_t alarmcounter = 0; static filterPipeline *current; static stateEstimation states; + static uint32_t last_time; switch (runState) { case RUNSTATE_LOAD: @@ -279,7 +278,11 @@ static void StateEstimationCb(void) // set alarm to warning if called through timeout if (updatedSensors == 0) { - alarm = 1; + if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) { + alarm = 1; + } + } else { + last_time = PIOS_DELAY_GetRaw(); } // check if a new filter chain should be initialized @@ -338,48 +341,38 @@ static void StateEstimationCb(void) // most sensors get only rudimentary sanity checks #define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \ - if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ sensorname##Get(&s); \ - if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \ + if (ISREAL(s.a1) && ISREAL(s.a2) && ISREAL(s.a3)) { \ states.shortname[0] = s.a1; \ states.shortname[1] = s.a2; \ states.shortname[2] = s.a3; \ } \ else { \ - UNSET(states.updated, SENSORUPDATES_##shortname); \ + UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } SANITYCHECK3(GyroSensor, gyro, x, y, z); SANITYCHECK3(AccelSensor, accel, x, y, z); SANITYCHECK3(MagSensor, mag, x, y, z); + SANITYCHECK3(PositionSensor, pos, North, East, Down); SANITYCHECK3(GPSVelocitySensor, vel, North, East, Down); #define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \ - if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ sensorname##Get(&s); \ - if (sane(s.a1) && EXTRACHECK) { \ + if (ISREAL(s.a1) && EXTRACHECK) { \ states.shortname[0] = s.a1; \ } \ else { \ - UNSET(states.updated, SENSORUPDATES_##shortname); \ + UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } SANITYCHECK1(BaroSensor, baro, Altitude, 1); SANITYCHECK1(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter - // GPS is a tiny bit more tricky as GPSPositionSensor is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons - if (ISSET(states.updated, SENSORUPDATES_pos)) { - GPSPositionSensorData s; - GPSPositionSensorGet(&s); - if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) { - getNED(&s, states.pos); - } else { - UNSET(states.updated, SENSORUPDATES_pos); - } - } - // at this point sensor state is stored in "states" with some rudimentary filtering applied // apply all filters in the current filter chain @@ -411,7 +404,7 @@ static void StateEstimationCb(void) // the final output of filters is saved in state variables #define STORE3(statename, shortname, a1, a2, a3) \ - if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ statename##Data s; \ statename##Get(&s); \ s.a1 = states.shortname[0]; \ @@ -425,7 +418,7 @@ static void StateEstimationCb(void) STORE3(PositionState, pos, North, East, Down); STORE3(VelocityState, vel, North, East, Down); #define STORE2(statename, shortname, a1, a2) \ - if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \ + if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ statename##Data s; \ statename##Get(&s); \ s.a1 = states.shortname[0]; \ @@ -434,7 +427,7 @@ static void StateEstimationCb(void) } STORE2(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed); // attitude nees manual conversion from quaternion to euler - if (ISSET(states.updated, SENSORUPDATES_attitude)) { \ + if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \ AttitudeStateData s; AttitudeStateGet(&s); s.q1 = states.attitude[0]; @@ -490,7 +483,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { HomeLocationGet((HomeLocationData *)&homeLocation); - if (sane(homeLocation.Latitude) && sane(homeLocation.Longitude) && sane(homeLocation.Altitude) && sane(homeLocation.Be[0]) && sane(homeLocation.Be[1]) && sane(homeLocation.Be[2])) { + if (ISREAL(homeLocation.Latitude) && ISREAL(homeLocation.Longitude) && ISREAL(homeLocation.Altitude) && ISREAL(homeLocation.Be[0]) && ISREAL(homeLocation.Be[1]) && ISREAL(homeLocation.Be[2])) { // Compute matrix to convert deltaLLA to NED float lat, alt; lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); @@ -529,7 +522,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev) updatedSensors |= SENSORUPDATES_mag; } - if (ev->obj == GPSPositionSensorHandle()) { + if (ev->obj == PositionSensorHandle()) { updatedSensors |= SENSORUPDATES_pos; } @@ -548,39 +541,6 @@ static void sensorUpdatedCb(UAVObjEvent *ev) DelayedCallbackDispatch(stateEstimationCallback); } -/** - * @brief Convert the GPS LLA position into NED coordinates - * @note this method uses a taylor expansion around the home coordinates - * to convert to NED which allows it to be done with all floating - * calculations - * @param[in] Current GPS coordinates - * @param[out] NED frame coordinates - * @returns 0 for success, -1 for failure - */ -static void getNED(GPSPositionSensorData *gpsPosition, float *NED) -{ - float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), - DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), - (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) }; - - NED[0] = LLA2NEDM[0] * dL[0]; - NED[1] = LLA2NEDM[1] * dL[1]; - NED[2] = LLA2NEDM[2] * dL[2]; -} - -/** - * @brief sanity check for float values - * @note makes sure a float value is safe for further processing, ie not NAN and not INF - * @param[in] float value - * @returns true for safe and false for unsafe - */ -static bool sane(float value) -{ - if (isnan(value) || isinf(value)) { - return false; - } - return true; -} /** * @} diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index 47c8d798b..8eb823d4b 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -46,11 +46,19 @@ #define M_EULER_F 0.57721566490153286060651209008f /* Euler constant */ // Conversion macro -#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F)) -#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f)) +#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F)) +#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f)) // Useful math macros -#define MAX(a, b) ((a) > (b) ? (a) : (b)) -#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + +#define ISREAL(f) (!isnan(f) && !isinf(f)) + +// Bitfield access + +#define IS_SET(field, mask) (((field) & (mask)) == (mask)) +#define SET_MASK(field, mask) (field) |= (mask) +#define UNSET_MASK(field, mask) (field) &= ~(mask) #endif // PIOS_MATH_H diff --git a/shared/uavobjectdefinition/positionsensor.xml b/shared/uavobjectdefinition/positionsensor.xml new file mode 100644 index 000000000..34ec55a24 --- /dev/null +++ b/shared/uavobjectdefinition/positionsensor.xml @@ -0,0 +1,12 @@ + + + Contains the position measured relative to @ref HomeLocation + + + + + + + + +