From ec3c3c0030649368d3d53ce0624751f511cf370b Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Thu, 30 Mar 2023 14:10:07 +0200 Subject: [PATCH] Add velocity control as a fw state F F --- .../FixedwingPositionControl.cpp | 19 +++++++++++-------- .../FixedwingPositionControl.hpp | 1 + 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 594ec3db0a..e295ea7598 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -706,6 +706,12 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, const _skipping_takeoff_detection = false; + if (_control_mode.flag_control_offboard_enabled && !_control_mode.flag_control_position_enabled + && _control_mode.flag_control_velocity_enabled) { + return FW_POSCTRL_MODE_AUTO_VELOCITY; + } + + if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) || _control_mode.flag_control_offboard_enabled) && _position_setpoint_current_valid) { @@ -889,10 +895,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp); break; - case position_setpoint_s::SETPOINT_TYPE_VELOCITY: - control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp); - break; - case position_setpoint_s::SETPOINT_TYPE_LOITER: control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next); break; @@ -976,10 +978,6 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp { uint8_t position_sp_type = pos_sp_curr.type; - if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) { - return position_setpoint_s::SETPOINT_TYPE_VELOCITY; - } - Vector2d curr_wp{0, 0}; /* current waypoint (the one currently heading for) */ @@ -2347,6 +2345,11 @@ FixedwingPositionControl::Run() break; } + case FW_POSCTRL_MODE_AUTO_VELOCITY: { + control_auto_velocity(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current); + break; + } + case FW_POSCTRL_MODE_MANUAL_POSITION: { control_manual_position(control_interval, curr_pos, ground_speed); break; diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp index f8fcb07713..5ab1da9799 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -174,6 +174,7 @@ enum FW_POSCTRL_MODE { FW_POSCTRL_MODE_AUTO_TAKEOFF, FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT, FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR, + FW_POSCTRL_MODE_AUTO_VELOCITY, FW_POSCTRL_MODE_MANUAL_POSITION, FW_POSCTRL_MODE_MANUAL_ALTITUDE, FW_POSCTRL_MODE_OTHER