Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

ekf2: initialize origin from corrent position when possible

Browse Source
This commit is contained in:
bresch
2024-08-21 16:32:29 +02:00
committed by Mathieu Bresciani
parent af752536b9
commit 130fefb1e7
2 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
+2 -1
View File
@@ -113,7 +113,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
} else { } else {
// Try to initialize using measurement // Try to initialize using measurement
if (ekf.setEkfGlobalOrigin(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, sample.epv)) { if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
sample.epv)) {
ekf.enableControlStatusAuxGpos(); ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter; _reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::active; _state = State::active;
src/modules/ekf2/EKF/ekf.cpp
+1 -2
View File
@@ -280,8 +280,7 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
uint64_t timestamp_observation) uint64_t timestamp_observation)
{ {
if (!_pos_ref.isInitialized()) { if (!_pos_ref.isInitialized()) {
ECL_WARN("unable to reset global position, position reference not initialized"); return setLatLonOriginFromCurrentPos(lat_deg, lon_deg, accuracy);
return false;
} }
Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg); Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);