diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 70b5835a6c..c8a79a7b0c 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -160,7 +160,7 @@ void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const { // clip the radius to be within range radius = math::constrain(radius, _radius_min, _param_mc_orbit_rad_max.get()); - velocity = math::constrain(velocity, -fabsf(_velocity_max), fabsf(_velocity_max)); + velocity = math::constrain(velocity, -fabsf(_param_mpc_xy_vel_max.get()), fabsf(_param_mpc_xy_vel_max.get())); bool exceeds_maximum_acceleration = (velocity * velocity) >= _acceleration_max * radius; diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index af4ddc67be..b23adb113a 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -72,7 +72,6 @@ protected: private: /* TODO: Should be controlled by params */ static constexpr float _radius_min = 1.f; - static constexpr float _velocity_max = 10.f; static constexpr float _acceleration_max = 2.f; static constexpr float _horizontal_acceptance_radius = 2.f; @@ -149,6 +148,7 @@ private: (ParamFloat) _param_mpc_acc_up_max, (ParamFloat) _param_mpc_acc_down_max, (ParamFloat) _param_mpc_z_v_auto_up, - (ParamFloat) _param_mpc_z_v_auto_dn + (ParamFloat) _param_mpc_z_v_auto_dn, + (ParamFloat) _param_mpc_xy_vel_max ) };