diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp index a73d60e7fc..0ae3d35bb6 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -31,13 +31,6 @@ * ****************************************************************************/ -/** - * @file FlightTaskManualPosition.hpp - * - * Flight task for manual position controlled mode. - * - */ - #pragma once #include "FlightTaskManualAltitudeSmoothVel.hpp" @@ -53,7 +46,7 @@ public: bool activate(const trajectory_setpoint_s &last_setpoint) override; bool update() override; -private: +protected: void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index 17712ff090..4df57ca256 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -73,10 +73,12 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt) { - // maximum commanded acceleration and velocity - Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get()); + // maximum commanded velocity can be constrained dynamically const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint); Vector2f velocity_scale(velocity_sc, velocity_sc); + // maximum commanded acceleration is scaled down with velocity + const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get()); + Vector2f acceleration_scale(acceleration_sc, acceleration_sc); acceleration_scale *= 2.f; // because of drag the average acceleration is half