dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: Improve error reporting 

Add missing velocity accuracy reporting
Add missing dead reckoning reporting
This commit is contained in:
priseborough 2017-03-08 10:54:19 +11:00 committed by Lorenz Meier
parent 9efb1a59f2
commit fb6e050b06
1 changed files with 7 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/ekf2/ekf2_main.cpp
View File

@ -844,12 +844,9 @@ void Ekf2::task_main()
lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate
lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found
// TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
Vector3f pos_var, vel_var;
_ekf.get_pos_var(pos_var);
_ekf.get_vel_var(vel_var);
lpos.eph = sqrtf(pos_var(0) + pos_var(1));
lpos.epv = sqrtf(pos_var(2));
bool dead_reckoning;
_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv, &dead_reckoning);
_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv, &dead_reckoning);
// get state reset information of position and velocity
_ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
@ -893,8 +890,9 @@ void Ekf2::task_main()
global_pos.yaw = euler.psi(); // Yaw in radians -PI..+PI.
global_pos.eph = sqrtf(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally
global_pos.epv = sqrtf(pos_var(2)); // Standard deviation of position vertically
_ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv, &global_pos.dead_reckoning);
global_pos.evh = lpos.evh;
global_pos.evv = lpos.evv;
if (lpos.dist_bottom_valid) {
global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84
@ -905,8 +903,7 @@ void Ekf2::task_main()
global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
}
// TODO use innovatun consistency check timouts to set this
global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning
global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m)