forked from Archive/PX4-Autopilot
EKF : allow init without external vision measurements (#464)
This commit is contained in:
parent
a0ed08967e
commit
e1661a92c3
|
@ -175,6 +175,11 @@ void Ekf::controlExternalVisionFusion()
|
|||
_control_status.flags.ev_pos = true;
|
||||
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
|
||||
// method for fusing the position data
|
||||
if (_control_status.flags.gps) {
|
||||
|
|
10
EKF/ekf.cpp
10
EKF/ekf.cpp
|
@ -214,9 +214,8 @@ bool Ekf::initialiseFilter()
|
|||
// check to see if we have enough measurements and return false if not
|
||||
bool hgt_count_fail = _hgt_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;
|
||||
|
||||
} else {
|
||||
|
@ -260,13 +259,6 @@ bool Ekf::initialiseFilter()
|
|||
// calculate the initial magnetic field and yaw alignment
|
||||
_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 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
|
||||
|
|
Loading…
Reference in New Issue