diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 7bd0f11fa..f861c953c 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -77,6 +77,9 @@ int32_t configuration_check() RevoSettingsFusionAlgorithmGet(&revoFusion); bool navCapableFusion; switch (revoFusion) { + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYGPSOUTDOOR: + navCapableFusion = (GetCurrentFrameType() == FRAME_TYPE_FIXED_WING) ? true : false; + break; case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR: case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13: case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF: diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 1bad774d0..0e67fb99a 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -64,13 +64,16 @@ struct data { HomeLocationData homeLocation; bool first_run; bool useMag; + bool useVelHeading; float currentAccel[3]; float currentMag[3]; + float currentVel[3]; float accels_filtered[3]; float grot_filtered[3]; float gyroBias[3]; bool accelUpdated; bool magUpdated; + bool velUpdated; float accel_alpha; bool accel_filter_enabled; float rollPitchBiasRate; @@ -90,9 +93,10 @@ static FlightStatusData flightStatus; static int32_t initwithmag(stateFilter *self); static int32_t initwithoutmag(stateFilter *self); +static int32_t initwithvelheading(stateFilter *self); static int32_t maininit(stateFilter *self); static filterResult filter(stateFilter *self, stateEstimation *state); -static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]); +static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float velheading[2], float attitude[4]); static void flightStatusUpdatedCb(UAVObjEvent *ev); @@ -118,6 +122,15 @@ int32_t filterCFInitialize(stateFilter *handle) return STACK_REQUIRED; } +int32_t filterCFHInitialize(stateFilter *handle) +{ + globalInit(); + handle->init = &initwithvelheading; + handle->filter = &filter; + handle->localdata = pios_malloc(sizeof(struct data)); + return STACK_REQUIRED; +} + int32_t filterCFMInitialize(stateFilter *handle) { globalInit(); @@ -132,6 +145,16 @@ static int32_t initwithmag(stateFilter *self) struct data *this = (struct data *)self->localdata; this->useMag = 1; + this->useVelHeading = 0; + return maininit(self); +} + +static int32_t initwithvelheading(stateFilter *self) +{ + struct data *this = (struct data *)self->localdata; + + this->useMag = 0; + this->useVelHeading = 1; return maininit(self); } @@ -140,6 +163,7 @@ static int32_t initwithoutmag(stateFilter *self) struct data *this = (struct data *)self->localdata; this->useMag = 0; + this->useVelHeading = 0; return maininit(self); } @@ -184,6 +208,12 @@ static filterResult filter(stateFilter *self, stateEstimation *state) this->currentMag[1] = state->mag[1]; this->currentMag[2] = state->mag[2]; } + if (IS_SET(state->updated, SENSORUPDATES_vel)) { + this->velUpdated = 1; + this->currentVel[0] = state->vel[0]; + this->currentVel[1] = state->vel[1]; + this->currentVel[2] = state->vel[2]; + } if (IS_SET(state->updated, SENSORUPDATES_accel)) { this->accelUpdated = 1; this->currentAccel[0] = state->accel[0]; @@ -193,7 +223,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) if (IS_SET(state->updated, SENSORUPDATES_gyro)) { if (this->accelUpdated) { float attitude[4]; - result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude); + result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, this->currentVel, attitude); if (result == FILTERRESULT_OK) { state->attitude[0] = attitude[0]; state->attitude[1] = attitude[1]; @@ -203,6 +233,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) } this->accelUpdated = 0; this->magUpdated = 0; + this->velUpdated = 0; } } return result; @@ -222,7 +253,7 @@ static inline void apply_accel_filter(const struct data *this, const float *raw, } } -static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]) +static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float vel[3], float attitude[4]) { float dT; @@ -396,7 +427,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float accel_err[2] /= (accel_mag * grot_mag); float mag_err[3] = { 0.0f }; - if (this->magUpdated && this->useMag) { + if (this->magUpdated && this->useMag && !this->useVelHeading) { // Rotate gravity to body frame and cross with accels float brot[3]; float Rbe[3][3]; @@ -421,6 +452,31 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float } else { CrossProduct((const float *)mag, (const float *)brot, mag_err); } + } else if (this->velUpdated && this->useVelHeading) { + float speed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1]); + if (speed > 3.0f) { // ~11kmh + float rpy[3]; + + Quaternion2RPY(attitude, rpy); + float heading = RAD2DEG(atan2f(-vel[1], -vel[0])) + 180.0f; + + // find shortest way + float modulo = fmodf((heading - rpy[2]) + 180.0f, 360.0f); + if (modulo < 0) { + modulo += 180.0f; + } else { + modulo -= 180.0f; + } + /* From dRonin: The 0.008 is chosen to put things in a somewhat similar scale + * to the cross product. A 45 degree heading error will become + * a 7 deg/sec correction, so it neither takes forever to + * correct nor does a second of bad heading data screw us up + * too badly. + */ + mag_err[2] = modulo * 0.008f; + } else { + mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; + } } else { mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; } @@ -433,7 +489,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate; this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate; - if (this->useMag) { + if (this->useMag || this->useVelHeading) { this->gyroBias[2] -= mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate; } else { this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate; @@ -443,7 +499,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float // Correct rates based on proportional coefficient gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT; gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT; - if (this->useMag) { + if (this->useMag || this->useVelHeading) { gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * this->attitudeSettings.MagKp / dT; } else { gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT; diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index ad71c7c18..50d454b0a 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -95,6 +95,7 @@ int32_t filterAirInitialize(stateFilter *handle); int32_t filterStationaryInitialize(stateFilter *handle); int32_t filterLLAInitialize(stateFilter *handle); int32_t filterCFInitialize(stateFilter *handle); +int32_t filterCFHInitialize(stateFilter *handle); int32_t filterCFMInitialize(stateFilter *handle); int32_t filterEKF13iInitialize(stateFilter *handle); int32_t filterEKF13Initialize(stateFilter *handle); diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index a8fbd8665..f041693af 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -154,6 +154,7 @@ static stateFilter airFilter; static stateFilter stationaryFilter; static stateFilter llaFilter; static stateFilter cfFilter; +static stateFilter cfhFilter; static stateFilter cfmFilter; static stateFilter ekf13iFilter; static stateFilter ekf13Filter; @@ -184,6 +185,22 @@ static const filterPipeline *cfQueue = &(filterPipeline) { } } }; +static const filterPipeline *cfgpsQueue = &(filterPipeline) { + .filter = &airFilter, + .next = &(filterPipeline) { + .filter = &llaFilter, + .next = &(filterPipeline) { + .filter = &baroiFilter, + .next = &(filterPipeline) { + .filter = &altitudeFilter, + .next = &(filterPipeline) { + .filter = &cfhFilter, + .next = NULL, + } + } + } + } +}; static const filterPipeline *cfmiQueue = &(filterPipeline) { .filter = &magFilter, .next = &(filterPipeline) { @@ -366,6 +383,7 @@ int32_t StateEstimationInitialize(void) stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter)); stack_required = maxint32_t(stack_required, filterLLAInitialize(&llaFilter)); stack_required = maxint32_t(stack_required, filterCFInitialize(&cfFilter)); + stack_required = maxint32_t(stack_required, filterCFHInitialize(&cfhFilter)); stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter)); stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter)); stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter)); @@ -443,6 +461,11 @@ static void StateEstimationCb(void) // reinit Mag alarm AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_UNINITIALISED); break; + case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYGPSOUTDOOR: + newFilterChain = cfgpsQueue; + // reinit Mag alarm + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_UNINITIALISED); + break; case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG: newFilterChain = cfmiQueue; break; diff --git a/ground/gcs/src/share/qml/js/uav.js b/ground/gcs/src/share/qml/js/uav.js index 95660ae8a..840bb85b6 100644 --- a/ground/gcs/src/share/qml/js/uav.js +++ b/ground/gcs/src/share/qml/js/uav.js @@ -287,7 +287,7 @@ function gpsStatus() { } function fusionAlgorithm() { - var fusionAlgorithmText = ["None", "Basic (No Nav)", "CompMag", "Comp+Mag+GPS", "EKFIndoor", "GPSNav (INS)", "GPSNav (INS+CF)", "Testing (INS Indoor+CF)"]; + var fusionAlgorithmText = ["None", "Basic (No Nav)", "Comp+GPS", "CompMag", "Comp+Mag+GPS", "EKFIndoor", "GPSNav (INS)", "GPSNav (INS+CF)", "Testing (INS Indoor+CF)", "Acro"]; if (fusionAlgorithmText.length != RevoSettings.RevoSettingsConstants.FusionAlgorithmCount) { console.log("uav.js: fusionAlgorithm() do not match revoSettings.fusionAlgorithm uavo"); diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index bb258e20b..c42aab1cf 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -2,7 +2,7 @@ Settings for the revo to control the algorithm and what is updated