AttPosEKF: Use multiplatform land detector (was custom FixedWing only)

4 files changed, 46 insertions, 21 deletions

diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index 07db924b2..228ffa853 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -49,6 +49,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
@@ -141,6 +142,7 @@ private:
     int     _manual_control_sub;        /**< notification of manual control updates */
     int     _mission_sub;
     int     _home_sub;          /**< home position as defined by commander / user */
+    int     _landDetectorSub;
 
     orb_advert_t    _att_pub;           /**< vehicle attitude */
     orb_advert_t    _global_pos_pub;        /**< global position */
@@ -160,6 +162,7 @@ private:
     struct vehicle_gps_position_s       _gps;           /**< GPS position */
     struct wind_estimate_s              _wind;          /**< wind estimate */
     struct range_finder_report          _distance;      /**< distance estimate */
+    struct vehicle_land_detected_s      _landDetector;
 
     struct gyro_scale               _gyro_offsets[3];
     struct accel_scale              _accel_offsets[3];
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 511492b5a..5e5208e78 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -121,6 +121,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_manual_control_sub(-1),
 	_mission_sub(-1),
 	_home_sub(-1),
+	_landDetectorSub(-1),
 
 	/* publications */
 	_att_pub(-1),
@@ -141,6 +142,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_gps({}),
 	_wind({}),
 	_distance{},
+	_landDetector{},
 
 	_gyro_offsets({}),
 	_accel_offsets({}),
@@ -496,6 +498,7 @@ void AttitudePositionEstimatorEKF::task_main()
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_sub = orb_subscribe(ORB_ID(home_position));
+	_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
@@ -649,6 +652,9 @@ void AttitudePositionEstimatorEKF::task_main()
 				} 
 				else if (_ekf->statesInitialised) {
 
+					// Check if on ground - status is used by covariance prediction
+					_ekf->setOnGround(_landDetector.landed);
+
 					// We're apparently initialized in this case now
 					// check (and reset the filter as needed)
 					int check = check_filter_state();
@@ -904,9 +910,6 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 	// store the predicted states for subsequent use by measurement fusion
 	_ekf->StoreStates(IMUmsec);
 
-	// Check if on ground - status is used by covariance prediction
-	_ekf->OnGroundCheck();
-
 	// sum delta angles and time used by covariance prediction
 	_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
 	_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
@@ -1086,7 +1089,7 @@ void AttitudePositionEstimatorEKF::print_status()
 	}
 	printf("states: %s %s %s %s %s %s %s %s %s %s\n",
 		       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
-		       (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
+		       (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE",
 		       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
 		       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
 		       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
@@ -1218,6 +1221,13 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
 
+	//Update Land Detector
+	bool newLandData;
+	orb_check(_landDetectorSub, &newLandData);
+	if(newLandData) {
+		orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector);
+	}
+
 	//Update AirSpeed
 	orb_check(_airspeed_sub, &_newAdsData);
 	if (_newAdsData) {
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index f8c6510a2..700979eda 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -153,7 +153,6 @@ AttPosEKF::AttPosEKF() :
     inhibitGndState(true),
     inhibitScaleState(true),
 
-    onGround(true),
     staticMode(true),
     useGPS(false),
     useAirspeed(true),
@@ -183,7 +182,9 @@ AttPosEKF::AttPosEKF() :
     auxFlowInnovGate(0.0f),
     rngInnovGate(0.0f),
     minFlowRng(0.0f),
-    moCompR_LOS(0.0f)
+    moCompR_LOS(0.0f),
+
+    _onGround(true)    
 {
 
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
@@ -442,7 +443,7 @@ void AttPosEKF::CovariancePrediction(float dt)
     daxCov = sq(dt*gyroProcessNoise);
     dayCov = sq(dt*gyroProcessNoise);
     dazCov = sq(dt*gyroProcessNoise);
-    if (onGround) dazCov = dazCov * sq(yawVarScale);
+    if (_onGround) dazCov = dazCov * sq(yawVarScale);
     accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f);
     dvxCov = sq(dt*accelProcessNoise);
     dvyCov = sq(dt*accelProcessNoise);
@@ -1132,6 +1133,7 @@ void AttPosEKF::FuseVelposNED()
                 current_ekf_state.velHealth = false;
             }
         }
+
         if (fusePosData)
         {
             // test horizontal position measurements
@@ -1163,6 +1165,7 @@ void AttPosEKF::FuseVelposNED()
                 current_ekf_state.posHealth = false;
             }
         }
+
         // test height measurements
         if (fuseHgtData)
         {
@@ -1608,7 +1611,7 @@ void AttPosEKF::FuseMagnetometer()
                     KH[i][j] = Kfusion[i] * H_MAG[j];
                 }
                 for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f;
-                if (!onGround)
+                if (!_onGround)
                 {
                     for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++)
                     {
@@ -1632,7 +1635,7 @@ void AttPosEKF::FuseMagnetometer()
                     {
                         KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
                     }
-                    if (!onGround)
+                    if (!_onGround)
                     {
                         for (uint8_t k = 16; k < EKF_STATE_ESTIMATES; k++)
                         {
@@ -2527,37 +2530,41 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d
     hgt = hgtRef - posNED[2];
 }
 
-void AttPosEKF::OnGroundCheck()
+void AttPosEKF::setOnGround(const bool isLanded)
 {
-    onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
+    _onGround = isLanded;
 
     if (staticMode) {
         staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
     }
     // don't update wind states if there is no airspeed measurement
-    if (onGround || !useAirspeed) {
+    if (_onGround || !useAirspeed) {
         inhibitWindStates = true;
     } else {
         inhibitWindStates =false;
     }
+
     // don't update magnetic field states if on ground or not using compass
-    if (onGround || !useCompass) {
+    if (_onGround || !useCompass) {
         inhibitMagStates = true;
     } else {
         inhibitMagStates = false;
     }
+
     // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS
-    if ((onGround || !useGPS) && !useRangeFinder) {
+    if ((_onGround || !useGPS) && !useRangeFinder) {
         inhibitGndState = true;
     } else {
         inhibitGndState = false;
     }
+
     // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable
-    if ((onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) {
+    if ((_onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) {
         inhibitGndState = true;
     } else {
         inhibitGndState = false;
     }
+
     // Don't update focal length offset state if there is no range finder or optical flow sensor
     // we need both sensors to do this estimation
     if (!useRangeFinder || !useOpticalFlow) {
@@ -2967,9 +2974,6 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
 
     int ret = 0;
 
-    // Check if we're on ground - this also sets static mode.
-    OnGroundCheck();
-
     // Reset the filter if the states went NaN
     if (StatesNaN()) {
         ekf_debug("re-initializing dynamic");
@@ -3279,7 +3283,7 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err)
         current_ekf_state.states[i] = states[i];
     }
     current_ekf_state.n_states = EKF_STATE_ESTIMATES;
-    current_ekf_state.onGround = onGround;
+    current_ekf_state.onGround = _onGround;
     current_ekf_state.staticMode = staticMode;
     current_ekf_state.useCompass = useCompass;
     current_ekf_state.useAirspeed = useAirspeed;
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index 19ef52145..8e820bfd9 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -212,7 +212,6 @@ public:
     bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
     bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
 
-    bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
     bool staticMode;    ///< boolean true if no position feedback is fused
     bool useGPS; // boolean true if GPS data is being used
     bool useAirspeed;    ///< boolean true if airspeed data is being used
@@ -319,7 +318,11 @@ public:
 
     static inline float sq(float valIn) {return valIn * valIn;}
 
-    void OnGroundCheck();
+    /**
+    * @brief
+    *   Tell the EKF if the vehicle has landed
+    **/
+    void setOnGround(const bool isLanded);
 
     void CovarianceInit();
 
@@ -396,6 +399,11 @@ protected:
 
     void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
 
+    void ResetStoredStates();
+    
+private:
+    bool _onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
+
 };
 
 uint32_t millis();