From aea7bd5b471cd64ddef843fe0c912f6b0e2eb679 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 20 Jul 2016 13:21:18 +0200 Subject: [PATCH] fw_attitude_control: calculate attitude setpoint for STAB mode - attitude setpoint generation for stabilized mode was shifted back to the fw attitude controller. since the fw position controller is polling on global position attitude setpoints were not generated when global position was not published. Signed-off-by: Roman --- .../fw_att_control/fw_att_control_main.cpp | 13 ++++++++++ .../fw_pos_control_l1_main.cpp | 24 +++++-------------- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8c6e70f83b..83f50050fb 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -975,6 +975,19 @@ FixedwingAttitudeControl::task_main() float yaw_manual = 0.0f; float throttle_sp = 0.0f; + // in STABILIZED mode we need to generate the attitude setpoint + // from manual user inputs + if (!_vcontrol_mode.flag_control_climb_rate_enabled) { + _att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad; + _att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max); + _att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad; + _att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max); + _att_sp.yaw_body = 0.0f; + _att_sp.thrust = _manual.z; + int instance; + orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT); + } + roll_sp = _att_sp.roll_body; pitch_sp = _att_sp.pitch_body; yaw_sp = _att_sp.yaw_body; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 98d67fd365..31c6a12877 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1942,7 +1942,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - } else { + } + + if (!_yaw_lock_engaged || fabsf(_manual.y) > HDG_HOLD_MAN_INPUT_THRESH || + fabsf(_manual.r) > HDG_HOLD_MAN_INPUT_THRESH) { _hdg_hold_enabled = false; _yaw_lock_engaged = false; _att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad; @@ -2004,23 +2007,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; - if (_control_mode.flag_control_attitude_enabled) { - /** STABILIZED FLIGHT **/ - _att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad; - _att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max_rad; - _att_sp.yaw_body = 0.0f; - _att_sp.thrust = _manual.z; - - } else { - /** MANUAL FLIGHT **/ - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = 0.0f; - _att_sp.thrust = 0.0f; - - /* do not publish the setpoint */ - setpoint = false; - } + /* do not publish the setpoint */ + setpoint = false; // reset hold altitude _hold_alt = _global_pos.alt;