diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index 973471ea4e..ed855c74aa 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -72,7 +72,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get()); Vector2f velocity_scale(_param_mpc_vel_manual.get(), _param_mpc_vel_manual.get()); - acceleration_scale *= 2.f; // because of drag the average aceleration is half + acceleration_scale *= 2.f; // because of drag the average acceleration is half // Map stick input to acceleration Sticks::limitStickUnitLengthXY(stick_xy); @@ -144,9 +144,13 @@ Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const flo void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration) { + // fetch the tilt limit which is lower than the maximum during takeoff + takeoff_status_s takeoff_status{}; + _takeoff_status_sub.copy(&takeoff_status); + // Check if acceleration would exceed the tilt limit const float acc = acceleration.length(); - const float acc_tilt_max = tanf(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()) * CONSTANTS_ONE_G; + const float acc_tilt_max = tanf(takeoff_status.tilt_limit) * CONSTANTS_ONE_G; if (acc > acc_tilt_max) { acceleration *= acc_tilt_max / acc; diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index a9eda763c0..5f011acd9b 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -40,9 +40,10 @@ #pragma once #include -#include // TODO add this include to AlphaFilter since it's used there #include #include +#include +#include #include "SlewRate.hpp" @@ -66,6 +67,8 @@ private: void applyTiltLimit(matrix::Vector2f &acceleration); void lockPosition(const matrix::Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt); + uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; + SlewRate _acceleration_slew_rate_x; SlewRate _acceleration_slew_rate_y; AlphaFilter _brake_boost_filter;