From 08e909ac722574e5690fe0ffa4db3bdb59b78294 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 6 Feb 2025 17:10:08 +0300 Subject: [PATCH] publish default control limits once Signed-off-by: RomanBapst --- .../fw_pos_control/FixedwingPositionControl.cpp | 13 +++++++++++++ .../fw_pos_control/FixedwingPositionControl.hpp | 2 ++ 2 files changed, 15 insertions(+) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 1fc32b9e26..fb84080e01 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -2183,6 +2183,19 @@ FixedwingPositionControl::Run() /* only run controller if position changed */ + if (!_control_limits_published_once) { + longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; + setDefaultLongControlLimits(longitudinal_control_limits); + _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); + + lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; + lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + _lateral_ctrl_limits_pub.publish(lateral_limits); + + _control_limits_published_once = true; + + } + if (_local_pos_sub.update(&_local_pos)) { const float control_interval = math::constrain((_local_pos.timestamp - _last_time_position_control_called) * 1e-6f, diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 4ad6ba12f8..fae929418c 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -228,6 +228,8 @@ private: vehicle_local_position_s _local_pos{}; vehicle_status_s _vehicle_status{}; + bool _control_limits_published_once{false}; + Vector2f _lpos_where_backtrans_started; bool _position_setpoint_previous_valid{false};