diff options
Diffstat (limited to 'src/modules')
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(); |