zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

master
Paul Riseborough 9 years ago
parent
00bada8f25
commit
1b6c5bbafd
5 changed files with 37 additions and 16 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      EKF/common.h
  2. 40
      EKF/control.cpp
  3. 6
      EKF/ekf.cpp
  4. 2
      EKF/ekf_helper.cpp
  5. 2
      EKF/vel_pos_fusion.cpp

1
EKF/common.h
Unescape View File

@ -422,6 +422,7 @@ union filter_control_status_u { @@ -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
Unescape View File

@ -323,6 +323,7 @@ void Ekf::controlHeightSensorTimeouts() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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;
}
}

6
EKF/ekf.cpp
Unescape View File

@ -423,12 +423,8 @@ bool Ekf::initialiseFilter(void) @@ -423,12 +423,8 @@ 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;
}
}
if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
@ -447,7 +443,7 @@ bool Ekf::initialiseFilter(void) @@ -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
Unescape View File

@ -204,7 +204,7 @@ void Ekf::resetHeight() @@ -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
Unescape View File

@ -165,7 +165,7 @@ void Ekf::fuseVelPosHeight() @@ -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);

Write Preview
Loading…
Cancel
Save