From f019e3ab0e78e83792ff3db6ded3cd88bce83d2c Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 1 Oct 2024 11:14:07 +0200 Subject: [PATCH] Expanded Vehicleconstraints to xy for emergency braking --- src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 2 +- src/modules/mc_pos_control/GotoControl/GotoControl.cpp | 1 + src/modules/mc_pos_control/MulticopterPositionControl.hpp | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index fe34719c98c1..4ab6e4a61a1a 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -816,7 +816,7 @@ void FlightTaskAuto::_updateTrajConstraints() // correction required by the altitude/vertical position controller _constraints.speed_down = 1.2f * _param_mpc_z_v_auto_dn.get(); _constraints.speed_up = 1.2f * _param_mpc_xy_vel_max.get(); - _constraints.speed_xy = 1.2f * _param_mpc_xy_vel_max.get(); + _constraints.speed_xy = 1.2f * _param_mpc_xy_vel_max.get();; if (_is_emergency_braking_active) { // When initializing with large velocity, allow 1g of diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp index 2453997dcb15..a3fc4f44bf44 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp @@ -128,6 +128,7 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const .timestamp = goto_setpoint.timestamp, .speed_up = NAN, .speed_down = NAN, + .speed_xy = NAN, .want_takeoff = false }; _vehicle_constraints_pub.publish(vehicle_constraints); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index f7cd9bbbe09a..cc03bc2623ee 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -119,6 +119,7 @@ class MulticopterPositionControl : public ModuleBase .timestamp = 0, .speed_up = NAN, .speed_down = NAN, + .speed_xy = NAN, .want_takeoff = false, };