AttPosEKF: Direct to EKF whether the vehicle is flying like a FixedWing or not

3 files changed, 29 insertions, 9 deletions

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 aad3aa43d..3bb395a87 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
@@ -354,6 +354,9 @@ void AttitudePositionEstimatorEKF::vehicle_status_poll()
 	if (vstatus_updated) {
 
 		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
+
+		//Tell EKF that the vehicle is a fixed wing or multi-rotor
+		_ekf->setIsFixedWing(!_vstatus.is_rotary_wing);
 	}
 }
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index d8e3116b9..a22253dd2 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -185,7 +185,8 @@ AttPosEKF::AttPosEKF() :
     minFlowRng(0.0f),
     moCompR_LOS(0.0f),
 
-    _onGround(true)    
+    _isFixedWing(false),
+    _onGround(true)
 {
 
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
@@ -196,6 +197,7 @@ AttPosEKF::AttPosEKF() :
 
 AttPosEKF::~AttPosEKF()
 {
+    //dtor
 }
 
 void AttPosEKF::InitialiseParameters()
@@ -2003,12 +2005,12 @@ void AttPosEKF::FuseOptFlow()
                 K_LOS[1][15] = 0.0f;
             }
             if (inhibitMagStates) {
-            K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]);
-            K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]);
-            K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]);
-            K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]);
-            K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]);
-            K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]);
+                K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]);
+                K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]);
+                K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]);
+                K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]);
+                K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]);
+                K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]);
             } else {
                 for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) {
                     K_LOS[1][i] = 0.0f;
@@ -3317,3 +3319,8 @@ void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
     memcpy(last_error, &last_ekf_error, sizeof(*last_error));
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
 }
+
+void AttPosEKF::setIsFixedWing(const bool fixedWing)
+{
+    _isFixedWing = fixedWing;
+}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index e15ded977..8dd1d74e9 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -372,6 +372,16 @@ public:
 
     void InitializeDynamic(float (&initvelNED)[3], float declination);
 
+    /**
+    * @brief
+    *   Tells the EKF wheter the vehicle is a fixed wing frame or not.
+    *   This changes the way the EKF fuses position or attitude calulations
+    *   by making some assumptions on movement.
+    * @param fixedWing
+    *   true if the vehicle moves like a Fixed Wing, false otherwise
+    **/
+    void setIsFixedWing(const bool fixedWing);
+
 protected:
 
     /**
@@ -401,8 +411,8 @@ protected:
     void ResetStoredStates();
     
 private:
-    bool _onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
-
+    bool _isFixedWing;               ///< True if the vehicle is a fixed-wing frame type
+    bool _onGround;                  ///< boolean true when the flight vehicle is on the ground (not flying)
 };
 
 uint32_t millis();