fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Johan Jansen
2015-03-08 12:13:56 +01:00
parent 6aba6a4f88
commit 807e02b4a0
3 changed files with 29 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+3
View File
@@ -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);
}
}
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+14 -7
View File
@@ -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;
}
src/modules/ekf_att_pos_estimator/estimator_22states.h
+12 -2
View File
@@ -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();