diff options
Diffstat (limited to 'src/modules/fw_pos_control_l1')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 52 |
1 files changed, 44 insertions, 8 deletions
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 ed6d8792c..774758ef4 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 @@ -176,6 +176,8 @@ private: bool launch_detected; bool usePreTakeoffThrust; + bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) + /* Landingslope object */ Landingslope landingslope; @@ -344,6 +346,16 @@ private: * Main sensor collection task. */ void task_main() __attribute__((noreturn)); + + /* + * Reset takeoff state + */ + int reset_takeoff_state(); + + /* + * Reset landing state + */ + int reset_landing_state(); }; namespace l1_control @@ -389,6 +401,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), launch_detected(false), + last_manual(false), usePreTakeoffThrust(false), flare_curve_alt_last(0.0f), launchDetector(), @@ -1022,19 +1035,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // mission is active _loiter_hold = false; - /* reset land state */ + /* reset landing state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { - land_noreturn_horizontal = false; - land_noreturn_vertical = false; - land_stayonground = false; - land_motor_lim = false; - land_onslope = false; + reset_landing_state(); } /* reset takeoff/launch state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { - launch_detected = false; - usePreTakeoffThrust = false; + reset_takeoff_state(); } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1131,6 +1139,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; + + /* reset landing and takeoff state */ + if (!last_manual) { + reset_landing_state(); + reset_takeoff_state(); + } } if (usePreTakeoffThrust) { @@ -1141,6 +1155,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _att_sp.pitch_body = _tecs.get_pitch_demand(); + if (_control_mode.flag_control_position_enabled) { + last_manual = false; + } else { + last_manual = true; + } + return setpoint; } @@ -1291,6 +1311,22 @@ FixedwingPositionControl::task_main() _exit(0); } +int FixedwingPositionControl::reset_takeoff_state() +{ + launch_detected = false; + usePreTakeoffThrust = false; + launchDetector.reset(); +} + +int FixedwingPositionControl::reset_landing_state() +{ + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; + land_motor_lim = false; + land_onslope = false; +} + int FixedwingPositionControl::start() { |