From 5347c12c8a85823775d8b7b7b73d9b3f0d04d980 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Mon, 18 Nov 2024 10:19:11 +0100 Subject: [PATCH] use interpolate func, change naming --- .../FlightTaskManualAcceleration.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index 28b48815eea6..13e3fc10933c 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -75,16 +75,15 @@ bool FlightTaskManualAcceleration::update() if (max_hagl_ratio > factor_threshold) { max_hagl_ratio = math::min(max_hagl_ratio, 1.f); - float flow_vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get()); + float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get()); - flow_vxy_max = flow_vxy_max + (max_hagl_ratio - factor_threshold) * (min_vel - flow_vxy_max) / - (min_vel - factor_threshold); + vxy_max = interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel); const float current_vel_constraint = _stick_acceleration_xy.getVelocityConstraint(); - if (abs(current_vel_constraint - flow_vxy_max) > 0.1f) { - // adjust velocity constraint with a lag because good tracking is required for the drag estimation - const float v_limit = math::constrain(flow_vxy_max, current_vel_constraint - _deltatime * _param_mpc_acc_hor.get(), + if (fabsf(current_vel_constraint - vxy_max) > 0.1f) { + // gradually adjust velocity constraint because good tracking is required for the drag estimation + const float v_limit = math::constrain(vxy_max, current_vel_constraint - _deltatime * _param_mpc_acc_hor.get(), current_vel_constraint + _deltatime * _param_mpc_acc_hor.get()); _stick_acceleration_xy.setVelocityConstraint(v_limit); }