From 914417f5807344cbbdfd08745c769ecd413ba8b1 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Sun, 11 Aug 2019 20:35:11 +0200 Subject: [PATCH] Enable offboard local position setpoints for fixed wing position control --- .../FixedwingPositionControl.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 4ad9625c74..8e782e3bc6 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -803,7 +803,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _tecs.reset_state(); } - if (_control_mode.flag_control_auto_enabled && pos_sp_curr.valid) { + if ((_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled) && pos_sp_curr.valid) { /* AUTONOMOUS FLIGHT */ _control_mode_current = FW_POSCTRL_MODE_AUTO; @@ -1726,6 +1726,18 @@ FixedwingPositionControl::Run() Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon); Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e); + //Convert Local setpoints to global setpoints + if (_control_mode.flag_control_offboard_enabled) { + if (!globallocalconverter_initialized()) { + globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_alt, _local_pos.ref_timestamp); + + } else { + globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z, + &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt); + } + } + /* * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. @@ -1746,7 +1758,7 @@ FixedwingPositionControl::Run() q.copyTo(_att_sp.q_d); _att_sp.q_d_valid = true; - if (!_control_mode.flag_control_offboard_enabled || + if (_control_mode.flag_control_offboard_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) { @@ -1905,6 +1917,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee /* tell TECS to update its state, but let it know when it cannot actually control the plane */ bool in_air_alt_control = (!_vehicle_land_detected.landed && (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_offboard_enabled || _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_altitude_enabled));