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

EKF: publish ekf solution status summary data

Browse Source
This commit is contained in:
Paul Riseborough
2016-10-06 09:02:29 +02:00
committed by Lorenz Meier
parent 007e73379f
commit 352750e5d1
4 changed files with 44 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+17
View File
@@ -445,4 +445,21 @@ union filter_control_status_u {
uint16_t value;
};
union ekf_solution_status {
struct {
uint16_t attitude : 1; // 0 - True if the attitude estimate is good
uint16_t velocity_horiz : 1; // 1 - True if the horizontal velocity estimate is good
uint16_t velocity_vert : 1; // 2 - True if the vertical velocity estimate is good
uint16_t pos_horiz_rel : 1; // 3 - True if the horizontal position (relative) estimate is good
uint16_t pos_horiz_abs : 1; // 4 - True if the horizontal position (absolute) estimate is good
uint16_t pos_vert_abs : 1; // 5 - True if the vertical position (absolute) estimate is good
uint16_t pos_vert_agl : 1; // 6 - True if the vertical position (above ground) estimate is good
uint16_t const_pos_mode : 1; // 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
uint16_t gps_glitch : 1; // 10 - True if the EKF has detected a GPS glitch
} flags;
uint16_t value;
};
}
EKF/ekf.h
+3
View File
@@ -169,6 +169,9 @@ public:
// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl);
// return a bitmask integer that describes which state estimates can be used for flight control
void get_ekf_soln_status(uint16_t *status);
private:
static const uint8_t _k_num_states = 24;
EKF/ekf_helper.cpp
+22
View File
@@ -731,6 +731,28 @@ void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, f
*hagl = sqrt(_terr_test_ratio);
}
// return a bitmask integer that describes which state estimates are valid
void Ekf::get_ekf_soln_status(uint16_t *status)
{
ekf_solution_status soln_status;
soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
float dummy_var;
soln_status.flags.pos_vert_agl = get_terrain_vert_pos(&dummy_var);
soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_vert_abs;
bool gps_vel_innov_bad = (_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f);
bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f);
bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f);
soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
*status = soln_status.value;
}
// fuse measurement
void Ekf::fuse(float *K, float innovation)
{
EKF/estimator_interface.h
+2
View File
@@ -249,6 +249,8 @@ public:
// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
virtual void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl) = 0;
// return a bitmask integer that describes which state estimates can be used for flight control
virtual void get_ekf_soln_status(uint16_t *status) = 0;
protected: