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

control: inhibit EV yaw from restarting when GPS aid wants to start

This commit is contained in:
bresch 2021-05-07 16:14:49 +02:00 committed by Paul Riseborough
parent a685987082
commit 48722a2a92
3 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
EKF/control.cpp
View File

@ -190,6 +190,10 @@ void Ekf::controlExternalVisionFusion()
// Check for new external vision data
if (_ev_data_ready) {
if (_inhibit_ev_yaw_use) {
stopEvYawFusion();
}
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
// needs to be calculated and the observations rotated into the EKF frame of reference
if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
@ -215,7 +219,7 @@ void Ekf::controlExternalVisionFusion()
}
// external vision yaw aiding selection logic
if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
// don't start using EV data unless data is arriving frequently
if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
startEvYawFusion();
@ -521,13 +525,16 @@ void Ekf::controlGpsFusion()
// If the heading is not aligned, reset the yaw and magnetic field states
// Do not use external vision for yaw if using GPS because yaw needs to be
// defined relative to an NED reference frame
const bool want_to_reset_mag_heading = !_control_status.flags.yaw_align ||
_control_status.flags.ev_yaw ||
_mag_inhibit_yaw_reset_req;
if (!_control_status.flags.yaw_align
|| _control_status.flags.ev_yaw
|| _mag_inhibit_yaw_reset_req
|| _mag_yaw_reset_req) {
if (want_to_reset_mag_heading) {
_mag_yaw_reset_req = true;
_control_status.flags.ev_yaw = false;
// Stop the vision for yaw fusion and do not allow it to start again
stopEvYawFusion();
_inhibit_ev_yaw_use = true;
} else {
// If the heading is valid start using gps aiding

1
EKF/ekf.h
View File

@ -341,6 +341,7 @@ private:
Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m)
bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use
Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused

4
EKF/ekf_helper.cpp
View File

@ -1458,6 +1458,10 @@ void Ekf::stopGpsFusion()
stopGpsPosFusion();
stopGpsVelFusion();
stopGpsYawFusion();
// We do not need to know the true North anymore
// EV yaw can start again
_inhibit_ev_yaw_use = false;;
}
void Ekf::stopGpsPosFusion()