fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
View File

@ -113,7 +113,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
} else {
// 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();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::active;

3
src/modules/ekf2/EKF/ekf.cpp
View File

@ -280,8 +280,7 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
uint64_t timestamp_observation)
{
if (!_pos_ref.isInitialized()) {
ECL_WARN("unable to reset global position, position reference not initialized");
return false;
return setLatLonOriginFromCurrentPos(lat_deg, lon_deg, accuracy);
}
Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);