diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp index 8679573800..7b1c24f1dd 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp @@ -62,35 +62,35 @@ bool FlightTaskManualAccelerationSlow::update() if (_velocity_limits_received_before) { // message received once since mode was started if (PX4_ISFINITE(_velocity_limits.horizontal_velocity)) { - velocity_horizontal = fmaxf(_velocity_limits.horizontal_velocity, _param_posslow_min_hvel.get()); + velocity_horizontal = fmaxf(_velocity_limits.horizontal_velocity, _param_mc_slow_min_hvel.get()); velocity_horizontal_limited = true; } if (PX4_ISFINITE(_velocity_limits.vertical_velocity)) { - velocity_up = velocity_down = fmaxf(_velocity_limits.vertical_velocity, _param_posslow_min_vvel.get()); + velocity_up = velocity_down = fmaxf(_velocity_limits.vertical_velocity, _param_mc_slow_min_vvel.get()); velocity_vertical_limited = true; } if (PX4_ISFINITE(_velocity_limits.yaw_rate)) { - yaw_rate = fmaxf(_velocity_limits.yaw_rate, math::radians(_param_posslow_min_yawr.get())); + yaw_rate = fmaxf(_velocity_limits.yaw_rate, math::radians(_param_mc_slow_min_yawr.get())); yaw_rate_limited = true; } } // Remote knob commanded limits - if (_param_posslow_map_hvel.get() != 0) { - const float min_horizontal_velocity_scale = _param_posslow_min_hvel.get() / fmaxf(velocity_horizontal, FLT_EPSILON); - const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_posslow_map_hvel.get()); + if (_param_mc_slow_map_hvel.get() != 0) { + const float min_horizontal_velocity_scale = _param_mc_slow_min_hvel.get() / fmaxf(velocity_horizontal, FLT_EPSILON); + const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_hvel.get()); const float aux_based_scale = math::interpolate(aux_input, -1.f, 1.f, min_horizontal_velocity_scale, 1.f); velocity_horizontal *= aux_based_scale; velocity_horizontal_limited = true; } - if (_param_posslow_map_vvel.get() != 0) { - const float min_up_speed_scale = _param_posslow_min_vvel.get() / fmaxf(velocity_up, FLT_EPSILON); - const float min_down_speed_scale = _param_posslow_min_vvel.get() / fmaxf(velocity_down, FLT_EPSILON); - const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_posslow_map_vvel.get()); + if (_param_mc_slow_map_vvel.get() != 0) { + const float min_up_speed_scale = _param_mc_slow_min_vvel.get() / fmaxf(velocity_up, FLT_EPSILON); + const float min_down_speed_scale = _param_mc_slow_min_vvel.get() / fmaxf(velocity_down, FLT_EPSILON); + const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_vvel.get()); const float up_aux_based_scale = math::interpolate(aux_input, -1.f, 1.f, min_up_speed_scale, 1.f); const float down_aux_based_scale = @@ -100,9 +100,9 @@ bool FlightTaskManualAccelerationSlow::update() velocity_vertical_limited = true; } - if (_param_posslow_map_yawr.get() != 0) { - const float min_yaw_rate_scale = math::radians(_param_posslow_min_yawr.get()) / fmaxf(yaw_rate, FLT_EPSILON); - const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_posslow_map_yawr.get()); + if (_param_mc_slow_map_yawr.get() != 0) { + const float min_yaw_rate_scale = math::radians(_param_mc_slow_min_yawr.get()) / fmaxf(yaw_rate, FLT_EPSILON); + const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_yawr.get()); const float aux_based_scale = math::interpolate(aux_input, -1.f, 1.f, min_yaw_rate_scale, 1.f); yaw_rate *= aux_based_scale; @@ -111,15 +111,15 @@ bool FlightTaskManualAccelerationSlow::update() // No input from remote and MAVLink -> use default slow mode limits if (!velocity_horizontal_limited) { - velocity_horizontal = _param_posslow_def_hvel.get(); + velocity_horizontal = _param_mc_slow_def_hvel.get(); } if (!velocity_vertical_limited) { - velocity_up = velocity_down = _param_posslow_def_vvel.get(); + velocity_up = velocity_down = _param_mc_slow_def_vvel.get(); } if (!yaw_rate_limited) { - yaw_rate = math::radians(_param_posslow_def_yawr.get()); + yaw_rate = math::radians(_param_mc_slow_def_yawr.get()); } // Interface to set resulting velocity limits diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp index 8a9c541a37..b54d9b765d 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp @@ -71,15 +71,15 @@ private: velocity_limits_s _velocity_limits{}; DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration, - (ParamInt) _param_posslow_map_hvel, - (ParamInt) _param_posslow_map_vvel, - (ParamInt) _param_posslow_map_yawr, - (ParamFloat) _param_posslow_min_hvel, - (ParamFloat) _param_posslow_min_vvel, - (ParamFloat) _param_posslow_min_yawr, - (ParamFloat) _param_posslow_def_hvel, - (ParamFloat) _param_posslow_def_vvel, - (ParamFloat) _param_posslow_def_yawr, + (ParamInt) _param_mc_slow_map_hvel, + (ParamInt) _param_mc_slow_map_vvel, + (ParamInt) _param_mc_slow_map_yawr, + (ParamFloat) _param_mc_slow_min_hvel, + (ParamFloat) _param_mc_slow_min_vvel, + (ParamFloat) _param_mc_slow_min_yawr, + (ParamFloat) _param_mc_slow_def_hvel, + (ParamFloat) _param_mc_slow_def_vvel, + (ParamFloat) _param_mc_slow_def_yawr, (ParamFloat) _param_mpc_vel_manual, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_z_vel_max_dn, diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c index b2456f1df8..4c643d87d8 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c @@ -43,7 +43,7 @@ * @value 6 AUX6 * @group Multicopter Position Slow Mode */ -PARAM_DEFINE_INT32(POSSLOW_MAP_HVEL, 0); +PARAM_DEFINE_INT32(MC_SLOW_MAP_HVEL, 0); /** * Manual input mapped to scale vertical velocity in position slow mode @@ -57,7 +57,7 @@ PARAM_DEFINE_INT32(POSSLOW_MAP_HVEL, 0); * @value 6 AUX6 * @group Multicopter Position Slow Mode */ -PARAM_DEFINE_INT32(POSSLOW_MAP_VVEL, 0); +PARAM_DEFINE_INT32(MC_SLOW_MAP_VVEL, 0); /** * Manual input mapped to scale yaw rate in position slow mode @@ -71,7 +71,7 @@ PARAM_DEFINE_INT32(POSSLOW_MAP_VVEL, 0); * @value 6 AUX6 * @group Multicopter Position Slow Mode */ -PARAM_DEFINE_INT32(POSSLOW_MAP_YAWR, 0); +PARAM_DEFINE_INT32(MC_SLOW_MAP_YAWR, 0); /** * Horizontal velocity lower limit @@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(POSSLOW_MAP_YAWR, 0); * @decimal 2 * @group Multicopter Position Slow Mode */ -PARAM_DEFINE_FLOAT(POSSLOW_MIN_HVEL, .3f); +PARAM_DEFINE_FLOAT(MC_SLOW_MIN_HVEL, .3f); /** * Vertical velocity lower limit @@ -97,7 +97,7 @@ PARAM_DEFINE_FLOAT(POSSLOW_MIN_HVEL, .3f); * @decimal 2 * @group Multicopter Position Slow Mode */ -PARAM_DEFINE_FLOAT(POSSLOW_MIN_VVEL, .3f); +PARAM_DEFINE_FLOAT(MC_SLOW_MIN_VVEL, .3f); /** * Yaw rate lower limit @@ -110,7 +110,7 @@ PARAM_DEFINE_FLOAT(POSSLOW_MIN_VVEL, .3f); * @decimal 0 * @group Multicopter Position Slow Mode */ -PARAM_DEFINE_FLOAT(POSSLOW_MIN_YAWR, 3.f); +PARAM_DEFINE_FLOAT(MC_SLOW_MIN_YAWR, 3.f); /** * Default horizontal velocity limit @@ -125,7 +125,7 @@ PARAM_DEFINE_FLOAT(POSSLOW_MIN_YAWR, 3.f); * @decimal 2 * @group Multicopter Position Slow Mode */ -PARAM_DEFINE_FLOAT(POSSLOW_DEF_HVEL, 3.f); +PARAM_DEFINE_FLOAT(MC_SLOW_DEF_HVEL, 3.f); /** * Default vertical velocity limit @@ -140,7 +140,7 @@ PARAM_DEFINE_FLOAT(POSSLOW_DEF_HVEL, 3.f); * @decimal 2 * @group Multicopter Position Slow Mode */ -PARAM_DEFINE_FLOAT(POSSLOW_DEF_VVEL, 1.f); +PARAM_DEFINE_FLOAT(MC_SLOW_DEF_VVEL, 1.f); /** * Default yaw rate limit @@ -155,4 +155,4 @@ PARAM_DEFINE_FLOAT(POSSLOW_DEF_VVEL, 1.f); * @decimal 0 * @group Multicopter Position Slow Mode */ -PARAM_DEFINE_FLOAT(POSSLOW_DEF_YAWR, 45.f); +PARAM_DEFINE_FLOAT(MC_SLOW_DEF_YAWR, 45.f);