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

EKF : allow init without external vision measurements (#464)

This commit is contained in:
Kabir Mohammed 2018-07-11 21:36:47 -04:00 committed by Paul Riseborough
parent a0ed08967e
commit e1661a92c3
2 changed files with 6 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/control.cpp
View File

@ -175,6 +175,11 @@ void Ekf::controlExternalVisionFusion()
_control_status.flags.ev_pos = true; _control_status.flags.ev_pos = true;
ECL_INFO("EKF commencing external vision position fusion"); ECL_INFO("EKF commencing external vision position fusion");
if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
// Reset transformation between EV and EKF navigation frames when starting fusion
resetExtVisRotMat();
}
// reset the position if we are not already aiding using GPS, else use a relative position // reset the position if we are not already aiding using GPS, else use a relative position
// method for fusing the position data // method for fusing the position data
if (_control_status.flags.gps) { if (_control_status.flags.gps) {

10
EKF/ekf.cpp
View File

@ -214,9 +214,8 @@ bool Ekf::initialiseFilter()
// check to see if we have enough measurements and return false if not // check to see if we have enough measurements and return false if not
bool hgt_count_fail = _hgt_counter <= 2u * _obs_buffer_length; bool hgt_count_fail = _hgt_counter <= 2u * _obs_buffer_length;
bool mag_count_fail = _mag_counter <= 2u * _obs_buffer_length; bool mag_count_fail = _mag_counter <= 2u * _obs_buffer_length;
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2u * _obs_buffer_length);
if (hgt_count_fail || mag_count_fail || ev_count_fail) { if (hgt_count_fail || mag_count_fail) {
return false; return false;
} else { } else {
@ -260,13 +259,6 @@ bool Ekf::initialiseFilter()
// calculate the initial magnetic field and yaw alignment // calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(mag_init); _control_status.flags.yaw_align = resetMagHeading(mag_init);
// initialise the rotation from EV to EKF navigation frame if required
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS)
&& !(_params.fusion_mode & MASK_USE_EVYAW)) {
resetExtVisRotMat();
}
if (_control_status.flags.rng_hgt) { if (_control_status.flags.rng_hgt) {
// if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup // if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup
// so it can be used as a backup ad set the initial height using the range finder // so it can be used as a backup ad set the initial height using the range finder