Browse Source

AP_NavEKF3: getLLH fix when no GPS available

zr-v5.1
Randy Mackay 5 years ago
parent
commit
91a79543c9
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@ -345,6 +345,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const @@ -345,6 +345,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
return true;
} else {
// if no GPS fix, provide last known position before entering the mode
loc.lat = EKF_origin.lat;
loc.lng = EKF_origin.lng;
loc.offset(lastKnownPositionNE.x, lastKnownPositionNE.y);
return false;
}

Loading…
Cancel
Save