diff --git a/flight/modules/Airspeed/revolution/airspeed.c b/flight/modules/Airspeed/airspeed.c similarity index 95% rename from flight/modules/Airspeed/revolution/airspeed.c rename to flight/modules/Airspeed/airspeed.c index c4a66f3fd..9241007d2 100644 --- a/flight/modules/Airspeed/revolution/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -43,6 +43,7 @@ #include "airspeedsensor.h" // object that will be updated by the module #include "baro_airspeed_etasv3.h" #include "baro_airspeed_mpxv.h" +#include "gps_airspeed.h" #include "taskinfo.h" // Private constants @@ -139,6 +140,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters) AirspeedSettingsUpdatedCb(NULL); + gps_airspeedInitialize(); airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; @@ -164,6 +166,9 @@ static void airspeedTask(__attribute__((unused)) void *parameters) baro_airspeedGetETASV3(&airspeedData, &airspeedSettings); break; #endif + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION: + gps_airspeedGet(&airspeedData, &airspeedSettings); + break; default: airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; } diff --git a/flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c b/flight/modules/Airspeed/baro_airspeed_etasv3.c similarity index 100% rename from flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c rename to flight/modules/Airspeed/baro_airspeed_etasv3.c diff --git a/flight/modules/Airspeed/revolution/baro_airspeed_mpxv.c b/flight/modules/Airspeed/baro_airspeed_mpxv.c similarity index 100% rename from flight/modules/Airspeed/revolution/baro_airspeed_mpxv.c rename to flight/modules/Airspeed/baro_airspeed_mpxv.c diff --git a/flight/modules/Airspeed/revolution/gps_airspeed.c b/flight/modules/Airspeed/gps_airspeed.c similarity index 76% rename from flight/modules/Airspeed/revolution/gps_airspeed.c rename to flight/modules/Airspeed/gps_airspeed.c index 7f63f622f..baa31e46f 100644 --- a/flight/modules/Airspeed/revolution/gps_airspeed.c +++ b/flight/modules/Airspeed/gps_airspeed.c @@ -31,9 +31,11 @@ #include "openpilot.h" -#include "gps_airspeed.h" -#include "gpsvelocitysensor.h" +#include "velocitystate.h" #include "attitudestate.h" +#include "airspeedsensor.h" +#include "airspeedsettings.h" +#include "gps_airspeed.h" #include "CoordinateConversions.h" #include @@ -50,6 +52,7 @@ struct GPSGlobals { float gpsVelOld_N; float gpsVelOld_E; float gpsVelOld_D; + float oldAirspeed; }; // Private variables @@ -66,13 +69,16 @@ void gps_airspeedInitialize() gps = (struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals)); // GPS airspeed calculation variables - GPSVelocitySensorData gpsVelData; - GPSVelocitySensorGet(&gpsVelData); + VelocityStateInitialize(); + VelocityStateData gpsVelData; + VelocityStateGet(&gpsVelData); gps->gpsVelOld_N = gpsVelData.North; gps->gpsVelOld_E = gpsVelData.East; gps->gpsVelOld_D = gpsVelData.Down; + gps->oldAirspeed = 0.0f; + AttitudeStateData attData; AttitudeStateGet(&attData); @@ -96,7 +102,7 @@ void gps_airspeedInitialize() * where "f" is the fuselage vector in earth coordinates. * We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|. */ -void gps_airspeedGet(float *v_air_GPS) +void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) { float Rbe[3][3]; @@ -115,8 +121,14 @@ void gps_airspeedGet(float *v_air_GPS) // If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue. if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) { - GPSVelocitySensorData gpsVelData; - GPSVelocitySensorGet(&gpsVelData); + VelocityStateData gpsVelData; + VelocityStateGet(&gpsVelData); + + if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) { + airspeedData->CalibratedAirspeed = 0; + airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + return; // do not calculate if gps velocity is insufficient... + } // Calculate the norm^2 of the difference between the two GPS vectors float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f); @@ -125,7 +137,16 @@ void gps_airspeedGet(float *v_air_GPS) float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f); // Airspeed magnitude is the ratio between the two difference norms - *v_air_GPS = sqrtf(normDiffGPS2 / normDiffAttitude2); + float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2); + if (!IS_REAL(airspeedData->CalibratedAirspeed)) { + airspeedData->CalibratedAirspeed = 0; + airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + } else { + // need a low pass filter to filter out spikes in non coordinated maneuvers + airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed; + gps->oldAirspeed = airspeedData->CalibratedAirspeed; + airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + } // Save old variables for next pass gps->gpsVelOld_N = gpsVelData.North; @@ -135,7 +156,7 @@ void gps_airspeedGet(float *v_air_GPS) gps->RbeCol1_old[0] = Rbe[0][0]; gps->RbeCol1_old[1] = Rbe[0][1]; gps->RbeCol1_old[2] = Rbe[0][2]; - } else {} + } } diff --git a/flight/modules/Airspeed/revolution/inc/airspeed.h b/flight/modules/Airspeed/inc/airspeed.h similarity index 100% rename from flight/modules/Airspeed/revolution/inc/airspeed.h rename to flight/modules/Airspeed/inc/airspeed.h diff --git a/flight/modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h b/flight/modules/Airspeed/inc/baro_airspeed_etasv3.h similarity index 100% rename from flight/modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h rename to flight/modules/Airspeed/inc/baro_airspeed_etasv3.h diff --git a/flight/modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h b/flight/modules/Airspeed/inc/baro_airspeed_mpxv.h similarity index 100% rename from flight/modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h rename to flight/modules/Airspeed/inc/baro_airspeed_mpxv.h diff --git a/flight/modules/Airspeed/revolution/inc/gps_airspeed.h b/flight/modules/Airspeed/inc/gps_airspeed.h similarity index 93% rename from flight/modules/Airspeed/revolution/inc/gps_airspeed.h rename to flight/modules/Airspeed/inc/gps_airspeed.h index 6e0903b8d..4b5f08ad6 100644 --- a/flight/modules/Airspeed/revolution/inc/gps_airspeed.h +++ b/flight/modules/Airspeed/inc/gps_airspeed.h @@ -32,7 +32,7 @@ #define GPS_AIRSPEED_H void gps_airspeedInitialize(); -void gps_airspeedGet(float *v_air_GPS); +void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings); #endif // GPS_AIRSPEED_H diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 46b59723b..239dd0911 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -328,7 +328,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) } if (!this->inited) { - return 1; + return 3; } float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; @@ -436,7 +436,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->work.updated = 0; if (this->init_stage < 0) { - return 2; + return 1; } else { return 0; } diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 95eb42bca..07aed3b52 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -32,7 +32,7 @@ MODULES += Sensors #MODULES += Attitude/revolution MODULES += StateEstimation # use instead of Attitude MODULES += Altitude/revolution -MODULES += Airspeed/revolution +MODULES += Airspeed MODULES += AltitudeHold MODULES += Stabilization MODULES += VtolPathFollower diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index c2fe5e312..73dc7acd6 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -32,7 +32,7 @@ MODULES += Sensors MODULES += Attitude/revolution #MODULES += StateEstimation # use instead of Attitude MODULES += Altitude/revolution -MODULES += Airspeed/revolution +MODULES += Airspeed MODULES += AltitudeHold MODULES += Stabilization MODULES += ManualControl diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index badf65db1..76524166e 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -39,7 +39,8 @@ MODULES += CameraStab MODULES += Telemetry MODULES += FirmwareIAP MODULES += StateEstimation -MODULES += Sensors/simulated/Sensors +#MODULES += Sensors/simulated/Sensors +MODULES += Airspeed #MODULES += OveroSync # Paths diff --git a/shared/uavobjectdefinition/airspeedsettings.xml b/shared/uavobjectdefinition/airspeedsettings.xml index a208ff692..3ed136b16 100644 --- a/shared/uavobjectdefinition/airspeedsettings.xml +++ b/shared/uavobjectdefinition/airspeedsettings.xml @@ -4,7 +4,8 @@ - + +