rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

OP-1273 attitude estimation uses airspeedSensor.TrueAirspeed when available (e.g. from MS4525DO; available when TrueAirspeed >=0 ) instead of using an altitude correction IAS2TAS(alt). When not available it falls back to IAS2TAS(alt)

Browse Source
This commit is contained in:
Andres 2014-03-26 09:15:56 +01:00
parent 4dff86dc9c
commit 9d30673d18
3 changed files with 20 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
flight/modules/Attitude/revolution/attitude.c
View File

@ -650,7 +650,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
// we have airspeed available // we have airspeed available
airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed;
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down); airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed;
AirspeedStateSet(&airspeed); AirspeedStateSet(&airspeed);
} }
} }
@ -1078,7 +1078,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
AirspeedStateGet(&airspeed); AirspeedStateGet(&airspeed);
airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - Nav.Pos[2]); airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed * IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed;
AirspeedStateSet(&airspeed); AirspeedStateSet(&airspeed);
if (!gps_vel_updated && !gps_updated) { if (!gps_vel_updated && !gps_updated) {

2
flight/modules/StateEstimation/filterair.c
View File

@ -78,7 +78,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->altitude = state->baro[0]; this->altitude = state->baro[0];
} }
// calculate true airspeed estimation // calculate true airspeed estimation
if (IS_SET(state->updated, SENSORUPDATES_airspeed)) { if (IS_SET(state->updated, SENSORUPDATES_airspeed) && (state->airspeed[1]<0.f) ) {
state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude); state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude);
} }

18
flight/modules/StateEstimation/stateestimation.c
View File

@ -85,6 +85,18 @@
UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
} \ } \
} }
#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, a2, EXTRACHECK) \
if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
sensorname##Get(&s); \
if (IS_REAL(s.a1) && IS_REAL(s.a2) && EXTRACHECK) { \
states.shortname[0] = s.a1; \
states.shortname[1] = s.a2; \
} \
else { \
UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \
} \
}
// local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_SAVE before the check of alarms! // local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_SAVE before the check of alarms!
#define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(statename, shortname, a1, a2, a3) \ #define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(statename, shortname, a1, a2, a3) \
@ -379,8 +391,10 @@ static void StateEstimationCb(void)
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now // FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(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, set to zero for now
// GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
// at this point sensor state is stored in "states" with some rudimentary filtering applied // at this point sensor state is stored in "states" with some rudimentary filtering applied