fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-14 01:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

flight_task DR RTL: added scaling factors for heading and horizontal motion

Browse Source
This commit is contained in:
oravla5
2025-07-23 10:21:13 +02:00
parent 4294b148aa
commit eebc930a9d
2 changed files with 8 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp
+5 -5
View File
@@ -265,18 +265,18 @@ void FlightTaskReturnDeadReckoning::_updateSubscriptions()
bool FlightTaskReturnDeadReckoning::_initializeSmoothers()
{
// Initialize the heading smoother
_heading_smoothing.setMaxHeadingRate(_param_mpc_yawrauto_max.get());
_heading_smoothing.setMaxHeadingAccel(_param_mpc_yawrauto_acc.get());
_heading_smoothing.setMaxHeadingRate(_heading_smoothing_scaling_factor * _param_mpc_yawrauto_max.get());
_heading_smoothing.setMaxHeadingAccel(_heading_smoothing_scaling_factor * _param_mpc_yawrauto_acc.get());
_heading_smoothing.reset(_sub_vehicle_local_position.get().heading, 0.0f);
// Initialize the position smoother
_slew_rate_velocity_z.setSlewRate(_param_mpc_acc_up_max.get());
_slew_rate_velocity_z.setSlewRate(_velocity_z_rate_scaling_factor * _param_mpc_acc_up_max.get());
_slew_rate_velocity_z.setForcedValue((float) _sub_vehicle_local_position.get().vz);
// Initialize the acceleration smoothers
_slew_rate_acceleration_x.setSlewRate(_param_mpc_jerk_auto.get());
_slew_rate_acceleration_x.setSlewRate(_jerk_scaling_factor * _param_mpc_jerk_auto.get());
_slew_rate_acceleration_x.setForcedValue(_sub_vehicle_local_position.get().ax);
_slew_rate_acceleration_y.setSlewRate(_param_mpc_jerk_auto.get());
_slew_rate_acceleration_y.setSlewRate(_jerk_scaling_factor * _param_mpc_jerk_auto.get());
_slew_rate_acceleration_y.setForcedValue(_sub_vehicle_local_position.get().ay);
return true;
src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp
+3 -1
View File
@@ -136,12 +136,14 @@ private:
float _initial_distance_to_home{0.0f}; /**< Initial distance to home position */
float _distance_flown_estimate{0.0f};
float _heading_smoothing_scaling_factor{0.5f}; /**< Scaling factor applied to heading smoothing limits */
HeadingSmoothing _heading_smoothing; /**< Smoother for heading */
float _jerk_scaling_factor{0.2f}; /**< Scaling factor applied to acceleration slew rate (jerk) */
SlewRate<float> _slew_rate_acceleration_x{0.0f}; /**< Slew rate for x-acceleration setpoint */
SlewRate<float> _slew_rate_acceleration_y{0.0f}; /**< Slew rate for y-acceleration setpoint */
float _velocity_z_rate_scaling_factor{0.4f}; /**< Scaling factor applied to the vertical velocity slew rate */
SlewRate<float> _slew_rate_velocity_z{0.0f}; /**< Slew rate for vertical velocity */
float _rtl_alt{0.0f}; /**< Return altitude */
float _rtl_acc{0.0f}; /**< Return acceleration */