fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-04 08:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add unit tests and update ekf_helper setEkfGlobalOrigin() method to allow for condition when current position is unitialized.

Browse Source
This commit is contained in:
mcsauder
2021-02-19 08:59:47 -05:00
committed by Daniel Agar
parent 63f64b57c1
commit 164e53bad3
6 changed files with 210 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+4 -2
View File
@@ -688,7 +688,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
bool current_pos_available = false;
double current_lat = static_cast<double>(NAN);
double current_lon = static_cast<double>(NAN);
float current_alt = NAN;
float current_alt = 0.f;
// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (map_projection_initialized(&_pos_ref) && isHorizontalAidingActive()) {
@@ -699,7 +699,6 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
// reinitialize map projection to latitude, longitude, altitude, and reset position
if (map_projection_init_timestamped(&_pos_ref, latitude, longitude, _time_last_imu) == 0) {
if (current_pos_available) {
// reset horizontal position
Vector2f position;
@@ -709,6 +708,9 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
// reset altitude
_gps_alt_ref = altitude;
resetVerticalPositionTo(_gps_alt_ref - current_alt);
} else {
// reset altitude
_gps_alt_ref = altitude;
}
return true;