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

EKF: Enable height source to be selected independent of EV aiding

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-27 13:14:52 +10:00
parent 00bada8f25
commit 1b6c5bbafd
5 changed files with 37 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+32 -8
View File
@@ -323,6 +323,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF baro hgt timeout - reset to GPS\n");
@@ -333,6 +334,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF baro hgt timeout - reset to baro\n");
@@ -372,6 +374,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF gps hgt timeout - reset to baro\n");
@@ -382,6 +385,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF gps hgt timeout - reset to GPS\n");
@@ -414,6 +418,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF rng hgt timeout - reset to baro\n");
@@ -424,6 +429,7 @@ void Ekf::controlHeightSensorTimeouts()
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
// request a reset
reset_height = true;
printf("EKF rng hgt timeout - reset to rng hgt\n");
@@ -448,25 +454,43 @@ void Ekf::controlHeightAiding()
// check for height sensor timeouts and reset and change sensor if necessary
controlHeightSensorTimeouts();
// Control the soure of height measurements for the main filter
if (_control_status.flags.ev_pos) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) {
// Control the source of height measurements for the main filter
// do not switch to a sensor if it is unhealthy or the data is stale
if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) &&
!_baro_hgt_faulty &&
(((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) || _control_status.flags.baro_hgt)) {
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) &&
!_gps_hgt_faulty &&
(((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) || _control_status.flags.gps_hgt)) {
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) &&
!_rng_hgt_faulty &&
(((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) < 2 * RNG_MAX_INTERVAL) || _control_status.flags.rng_hgt)) {
} else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE && !_rng_hgt_faulty) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_EV) &&
(((_imu_sample_delayed.time_us - _ev_sample_delayed.time_us) < 2 * EV_MAX_INTERVAL) || _control_status.flags.ev_hgt)) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true;
}
}