UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-27 00:48:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

HomePosition: only set home position using GNSS if bit 0 in EKF2_GPS_CTRL is active

Browse Source 
If the vehicle position is not set by GNSS, then the home position should not be either.
This commit is contained in:
mahima-yoga
2026-01-30 10:23:04 +01:00
committed by Mahima Yoga
parent f8c1e8b81f
commit 4e59a060a8
2 changed files with 20 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/commander/HomePosition.cpp
View File

@@ -40,7 +40,9 @@
#include "commander_helper.h" #include "commander_helper.h"
HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags): ModuleParams(nullptr), HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags): ModuleParams(nullptr),
_failsafe_flags(failsafe_flags) {} _failsafe_flags(failsafe_flags),
_param_ekf2_gps_ctrl_handle(param_find("EKF2_GPS_CTRL"))
{}
bool HomePosition::hasMovedFromCurrentHomeLocation() bool HomePosition::hasMovedFromCurrentHomeLocation()
{ {
@@ -299,6 +301,19 @@ void HomePosition::setHomePosValid()
_valid = true; _valid = true;
} }
bool HomePosition::isGpsHorizontalFusionEnabled()
{
// If parameter doesn't exist, allow GPS usage
if (_param_ekf2_gps_ctrl_handle == PARAM_INVALID) {
return true;
}
// Check if bit 0 (horizontal position fusion) is set
int32_t ekf2_gps_ctrl = 0;
param_get(_param_ekf2_gps_ctrl_handle, &ekf2_gps_ctrl);
return (ekf2_gps_ctrl & 1);
}
void HomePosition::updateHomePositionYaw(float yaw) void HomePosition::updateHomePositionYaw(float yaw)
{ {
home_position_s home = _home_position_pub.get(); home_position_s home = _home_position_pub.get();
@@ -348,7 +363,8 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
const bool epv_valid = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV; const bool epv_valid = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
const bool evh_valid = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH; 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; _gps_position_for_home_valid = time_valid && fix_valid && eph_valid && epv_valid && evh_valid
&& isGpsHorizontalFusionEnabled();
if (_param_com_home_en.get() && _gps_position_for_home_valid && _last_gps_timestamp != 0 && _last_baro_timestamp != 0 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) { && _takeoff_time != 0 && now < _takeoff_time + kHomePositionCorrectionTimeWindow) {

2
src/modules/commander/HomePosition.hpp
View File

@@ -74,6 +74,7 @@ public:
private: private:
bool hasMovedFromCurrentHomeLocation(); bool hasMovedFromCurrentHomeLocation();
bool isGpsHorizontalFusionEnabled();
void setHomePosValid(); void setHomePosValid();
void updateHomePositionYaw(float yaw); void updateHomePositionYaw(float yaw);
@@ -113,4 +114,5 @@ private:
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamBool<px4::params::COM_HOME_EN>) _param_com_home_en (ParamBool<px4::params::COM_HOME_EN>) _param_com_home_en
) )
param_t _param_ekf2_gps_ctrl_handle{PARAM_INVALID};
}; };