From bdec85fdd05fb9d0737451e85c92c45754a6d74d Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Sat, 1 Jan 2022 19:48:01 +0100 Subject: [PATCH] Disable local vehicle setpoints while in transition This commit adds disabling vehicle setpoints while in transition --- .../FixedwingPositionControl.cpp | 19 ++++++++++++++++--- .../FixedwingPositionControl.hpp | 2 +- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index c5c734dd45..efe3383267 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -864,7 +864,9 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c _att_sp.pitch_body = get_tecs_pitch(); } - publishLocalPositionSetpoint(current_sp); + if (!_vehicle_status.in_transition_to_fw) { + publishLocalPositionSetpoint(current_sp); + } } void @@ -2322,18 +2324,29 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo tecs_status_publish(); } -void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s current_waypoint) +void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint) { if (_global_local_proj_ref.isInitialized()) { Vector2f current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon); vehicle_local_position_setpoint_s local_position_setpoint{}; + local_position_setpoint.timestamp = hrt_absolute_time(); local_position_setpoint.x = current_setpoint(0); local_position_setpoint.y = current_setpoint(1); local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt; + local_position_setpoint.yaw = NAN; + local_position_setpoint.yawspeed = NAN; + local_position_setpoint.vx = NAN; + local_position_setpoint.vy = NAN; + local_position_setpoint.vz = NAN; + local_position_setpoint.acceleration[0] = NAN; + local_position_setpoint.acceleration[1] = NAN; + local_position_setpoint.acceleration[2] = NAN; + local_position_setpoint.jerk[0] = NAN; + local_position_setpoint.jerk[1] = NAN; + local_position_setpoint.jerk[2] = NAN; local_position_setpoint.thrust[0] = _att_sp.thrust_body[0]; local_position_setpoint.thrust[1] = _att_sp.thrust_body[1]; local_position_setpoint.thrust[2] = _att_sp.thrust_body[2]; - local_position_setpoint.timestamp = hrt_absolute_time(); _local_pos_sp_pub.publish(local_position_setpoint); } } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index c2bdc04e85..6befe703f7 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -286,7 +286,7 @@ private: void status_publish(); void landing_status_publish(); void tecs_status_publish(); - void publishLocalPositionSetpoint(const position_setpoint_s current_waypoint); + void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint); void abort_landing(bool abort);