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

ekf: added methods for setting control height flags

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2017-02-28 10:42:52 +01:00 committed by ChristophTobler
parent d6decb3c84
commit a1e67396f4
2 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
EKF/ekf.h
View File

@ -507,6 +507,18 @@ private:
return var * var;
}
// set control flags to use baro height
void setControlBaroHeight();
// set control flags to use range height
void setControlRangeHeight();
// set control flags to use GPS height
void setControlGPSHeight();
// set control flags to use external vision height
void setControlEVHeight();
// zero the specified range of rows in the state covariance matrix
void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);

36
EKF/ekf_helper.cpp
View File

@ -1162,3 +1162,39 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
}
}
void Ekf::setControlBaroHeight()
{
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
}
void Ekf::setControlRangeHeight()
{
_control_status.flags.rng_hgt = true;
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.ev_hgt = false;
}
void Ekf::setControlGPSHeight()
{
_control_status.flags.gps_hgt = true;
_control_status.flags.baro_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
}
void Ekf::setControlEVHeight()
{
_control_status.flags.ev_hgt = true;
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
}