fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 09:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Adjust home position altitude after GNSS altitude correction (#25003)

Browse Source 
Within the first 2min of a flight, check if the integrated GNSS vertical velocity and the baro
measurements disagree with the reported GNSS altitude, and in that case update the 
set home position altitude to cancel out GNSS altitude drift.

* prevent re-init when reaching negative altitudes

* only allow correction during the first 120 second after takeoff

* add dependency to COM_HOME_EN parameter. reset vel-integral for multiple takeoffs
This commit is contained in:
Marco Hauswirth
2025-06-18 13:26:45 +02:00
committed by GitHub
parent ef252481a8
commit 6604c52c98
4 changed files with 94 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/HomePosition.cpp
+68 -12
View File
@@ -39,12 +39,8 @@
#include <lib/geo/geo.h>
#include "commander_helper.h"
using namespace time_literals;
HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags)
: _failsafe_flags(failsafe_flags)
{
}
HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags): ModuleParams(nullptr),
_failsafe_flags(failsafe_flags) {}
bool HomePosition::hasMovedFromCurrentHomeLocation()
{
@@ -308,6 +304,22 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
_local_position_sub.update();
_global_position_sub.update();
if (_vehicle_air_data_sub.updated()) {
vehicle_air_data_s baro_data;
_vehicle_air_data_sub.copy(&baro_data);
const float baro_alt = baro_data.baro_alt_meter;
if (_last_baro_timestamp != 0) {
const float dt = baro_data.timestamp - _last_baro_timestamp;
_lpf_baro.update(baro_alt, dt);
} else {
_lpf_baro.reset(baro_alt);
}
_last_baro_timestamp = baro_data.timestamp;
}
if (_vehicle_gps_position_sub.updated()) {
sensor_gps_s vehicle_gps_position;
_vehicle_gps_position_sub.copy(&vehicle_gps_position);
@@ -319,12 +331,56 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
_gps_epv = vehicle_gps_position.epv;
const hrt_abstime now = hrt_absolute_time();
const bool time = (now < vehicle_gps_position.timestamp + 1_s);
const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
const bool eph = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
const bool epv = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
const bool evh = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
_gps_position_for_home_valid = time && fix && eph && epv && evh;
const bool time_valid = now < (vehicle_gps_position.timestamp + 1_s);
const bool fix_valid = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
const bool eph_valid = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
const bool epv_valid = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
const bool evh_valid = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
_gps_position_for_home_valid = time_valid && fix_valid && eph_valid && epv_valid && evh_valid;
if (_param_com_home_en.get() && _gps_position_for_home_valid && _last_gps_timestamp != 0 && _last_baro_timestamp != 0
&& _takeoff_time != 0 && now < _takeoff_time + kHomePositionCorrectionTimeWindow) {
const float gps_alt = static_cast<float>(_gps_alt);
if (!PX4_ISFINITE(_gps_vel_integral)) {
_gps_vel_integral = gps_alt; // initialize the gps-vel-integral at same altitude as gps-pos
_baro_gps_static_offset = gps_alt - _lpf_baro.getState();
}
_gps_vel_integral += 1e-6f * (vehicle_gps_position.timestamp - _last_gps_timestamp) * (-vehicle_gps_position.vel_d_m_s);
// correct baro_alt with offset from GPS alt from when the drift integral was initialized
const float baro_alt_corrected = _lpf_baro.getState() + _baro_gps_static_offset;
const float gps_alt_with_home_offset = gps_alt + _home_altitude_offset_applied;
// Apply home altitude correction only if the GPS velocity-integrated altitude and baro altitude
// are more consistent with each other than either is with the GPS altitude (with home offset).
if (fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(baro_alt_corrected - gps_alt_with_home_offset) &&
fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(_gps_vel_integral - gps_alt_with_home_offset)) {
const float offset_new = baro_alt_corrected - gps_alt - _home_altitude_offset_applied;
if (fabsf(offset_new) > kAltitudeDifferenceThreshold) {
home_position_s home = _home_position_pub.get();
home.alt -= offset_new;
home.z += offset_new;
home.timestamp = now;
home.manual_home = false;
home.update_count = _home_position_pub.get().update_count + 1U;
_home_position_pub.update(home);
_home_altitude_offset_applied = baro_alt_corrected - gps_alt; // offset present when home position was last corrected
}
}
} else {
_gps_vel_integral = NAN;
}
_last_gps_timestamp = vehicle_gps_position.timestamp;
}
const vehicle_local_position_s &lpos = _local_position_sub.get();