Merge branch 'corvuscorax/OP-1036_fixed-wing-improvements' into next

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;
         }

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 <pios_math.h>
 
@@ -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 {}
+    }
 }
 
 

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
 

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;
     }

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

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

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

shared/uavobjectdefinition/airspeedsettings.xml

@@ -4,7 +4,8 @@
         <field name="SamplePeriod" units="ms" type="uint8" elements="1" defaultvalue="100"/>
         <field name="ZeroPoint" units="raw" type="uint16" elements="1" defaultvalue="0"/>
         <field name="Scale" units="raw" type="float" elements="1" defaultvalue="1.0"/>
-        <field name="AirspeedSensorType" units="" type="enum" elements="1" options="EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002"  defaultvalue="DIYDronesMPXV7002"/>
+        <field name="AirspeedSensorType" units="" type="enum" elements="1" options="EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002,GroundSpeedBasedWindEstimation"  defaultvalue="DIYDronesMPXV7002"/>
+	<field name="GroundSpeedBasedEstimationLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.08" />
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>