fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-09 20:38:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: added option to attempt dead-reckon RTL

Browse Source
This commit is contained in:
oravla5
2025-07-03 16:57:01 +02:00
parent 1ec83cac41
commit 9ca3a03352
4 changed files with 28 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander_params.c
+16
View File
@@ -536,6 +536,22 @@ PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
*/
PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5.f);
/**
* Loss of position autonomous failsafe action
*
* If no autonomous horizontal navigation is possible anymore should the vehicle attempt a dead-reckon
* return to home or shall it attempt to descend blindly and land.
*
* Action to take when autonomous horizontal navigation is lost:
* - "Dead-Reckon RTH" can be preferred to bring the vehicle back to Line of Sight (LOS) range, at which point the pilot can take over control.
* - "Land if possible" blind with potential drift and uncontrolled landing (risk of hitting obstacles)
*
* @group Commander
* @value 0 Land if possible
* @value 1 Dead-Reckoning RTL
*/
PARAM_DEFINE_INT32(COM_POS_FS_ACT, 0);
/**
* Horizontal velocity error threshold.
*
src/modules/commander/failsafe/framework.cpp
+2 -1
View File
@@ -573,7 +573,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
// fallthrough
case Action::DeadReckonRTL:
if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR)) {
if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR)
&& _param_com_pos_fs_act.get() == 1) {
selected_action = Action::DeadReckonRTL;
break;
}
src/modules/commander/failsafe/framework.h
+2 -1
View File
@@ -296,7 +296,8 @@ private:
void *_on_notify_user_arg{nullptr};
DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
(ParamFloat<px4::params::COM_FAIL_ACT_T>) _param_com_fail_act_t
(ParamFloat<px4::params::COM_FAIL_ACT_T>) _param_com_fail_act_t,
(ParamInt<px4::params::COM_POS_FS_ACT>) _param_com_pos_fs_act
);
};
src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp
+8 -6
View File
@@ -97,7 +97,7 @@ void FlightTaskReturnDeadReckoning::_updateState()
} else {
_state = State::ASCENT;
events::send<float, float>(events::ID("dead_reckon_rtl_ascent"), events::Log::Info,
"Ascending to return altitude {1:.2m_v} with bearing {2:.2} deg", _rtl_alt, math::degrees(_bearing_to_home));
"Ascending to return altitude {1:.2m_v} with bearing {2:.2} deg", _rtl_alt, math::degrees(_bearing_to_home));
}
break;
@@ -204,8 +204,8 @@ bool FlightTaskReturnDeadReckoning::_updateBearingToHome()
_distance_flown_estimate = .0f;
_initial_distance_to_home = get_distance_to_next_waypoint(
_start_vehicle_global_position(0), _start_vehicle_global_position(1),
_home_position(0), _home_position(1));
_start_vehicle_global_position(0), _start_vehicle_global_position(1),
_home_position(0), _home_position(1));
}
return !isnanf(_bearing_to_home);
@@ -251,7 +251,8 @@ float FlightTaskReturnDeadReckoning::_computeBearing(const matrix::Vector3d &_gl
void FlightTaskReturnDeadReckoning::_computeReturnParameters()
{
_rtl_alt = math::max((float) _start_vehicle_global_position(2), (float) _home_position(2) + _param_rtl_return_alt.get());
_rtl_alt = math::max((float) _start_vehicle_global_position(2),
(float) _home_position(2) + _param_rtl_return_alt.get());
_rtl_acc = _param_mpc_acc_hor_max.get();
}
@@ -296,12 +297,13 @@ bool FlightTaskReturnDeadReckoning::_isAboveReturnAltitude() const
bool FlightTaskReturnDeadReckoning::_isReturnComplete()
{
bool ret = false;
if (_isGlobalPositionValid()) {
// Close enough to home posititon
_readGlobalPosition(_start_vehicle_global_position);
ret = get_distance_to_next_waypoint(
_start_vehicle_global_position(0), _start_vehicle_global_position(1),
_home_position(0), _home_position(1)) < _param_nav_acc_rad.get();
_start_vehicle_global_position(0), _start_vehicle_global_position(1),
_home_position(0), _home_position(1)) < _param_nav_acc_rad.get();
}
if (!ret) {