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

EKF: Enable use of range finder for primary height source

Browse Source
This commit is contained in:
Paul Riseborough
2016-03-13 18:44:34 +11:00
parent 699ec17cc3
commit 370f643f42
4 changed files with 41 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+4 -2
View File
@@ -87,7 +87,7 @@ bool Ekf::resetPosition()
// Reset height state using the last height measurement
void Ekf::resetHeight()
{
if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
if (_control_status.flags.rng_hgt) {
rangeSample range_newest = _range_buffer.get_newest();
if (_time_last_imu - range_newest.time_us < 200000) {
@@ -96,7 +96,7 @@ void Ekf::resetHeight()
} else {
// TODO: reset to last known range based estimate
}
} else {
} else if (_control_status.flags.baro_hgt) {
// initialize vertical position with newest baro measurement
baroSample baro_newest = _baro_buffer.get_newest();
@@ -106,6 +106,8 @@ void Ekf::resetHeight()
} else {
// TODO: reset to last known baro based estimate
}
} else {
// TODO: reset to GPS height
}
}