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

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/common.h
View File

@ -422,6 +422,7 @@ union filter_control_status_u {
uint16_t gps_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference
uint16_t ev_pos : 1; // 12 - true when local position data from external vision is being fused
uint16_t ev_yaw : 1; // 13 - true when yaw data from external vision measurements is being fused
uint16_t ev_hgt : 1; // 14 - true when height data from external vision measurements is being fused
} flags;
uint16_t value;
};

40
EKF/control.cpp
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;
}
}

8
EKF/ekf.cpp
View File

@ -423,11 +423,7 @@ bool Ekf::initialiseFilter(void)
// set the default height source from the adjustable parameter
if (_hgt_counter == 0) {
if (_params.fusion_mode & MASK_USE_EVPOS) {
_primary_hgt_source = VDIST_SENSOR_EV;
} else {
_primary_hgt_source = _params.vdist_sensor_type;
}
_primary_hgt_source = _params.vdist_sensor_type;
}
if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
@ -447,7 +443,7 @@ bool Ekf::initialiseFilter(void)
}
} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || _primary_hgt_source == VDIST_SENSOR_EV) {
// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
// if the user parameter specifies use of GPS or external vision (EV) for height we use baro height initially and switch to GPS or EV
// later when it passes checks.
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if (_hgt_counter == 0 && _baro_sample_delayed.time_us != 0) {

2
EKF/ekf_helper.cpp
View File

@ -204,7 +204,7 @@ void Ekf::resetHeight()
// TODO: reset to last known gps based estimate
}
} else if (_control_status.flags.ev_pos) {
} else if (_control_status.flags.ev_hgt) {
// initialize vertical position with newest measurement
extVisionSample ev_newest = _ext_vision_buffer.get_newest();

2
EKF/vel_pos_fusion.cpp
View File

@ -165,7 +165,7 @@ void Ekf::fuseVelPosHeight()
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
} else if (_control_status.flags.ev_pos) {
} else if (_control_status.flags.ev_hgt) {
fuse_map[5] = true;
// calculate the innovation assuming the external vision observaton is in local NED frame
_vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);