diff --git a/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp index 231631496e..82be530455 100644 --- a/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp +++ b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp @@ -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; diff --git a/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp index 1693aeccae..3309574208 100644 --- a/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp +++ b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp @@ -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 _slew_rate_acceleration_x{0.0f}; /**< Slew rate for x-acceleration setpoint */ SlewRate _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 _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 */