diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index b3723fd763..6c5763bab4 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -297,12 +297,21 @@ FixedwingPositionControl::parameters_update() void FixedwingPositionControl::vehicle_control_mode_poll() { - bool updated; + bool updated = false; orb_check(_control_mode_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + const bool was_armed = _control_mode.flag_armed; + + if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) == PX4_OK) { + + // reset state when arming + if (!was_armed && _control_mode.flag_armed) { + reset_takeoff_state(true); + reset_landing_state(); + } + } } } @@ -1870,10 +1879,10 @@ FixedwingPositionControl::run() } void -FixedwingPositionControl::reset_takeoff_state() +FixedwingPositionControl::reset_takeoff_state(bool force) { // only reset takeoff if !armed or just landed - if (!_control_mode.flag_armed || (_was_in_air && _vehicle_land_detected.landed)) { + if (!_control_mode.flag_armed || (_was_in_air && _vehicle_land_detected.landed) || force) { _runway_takeoff.reset(); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 72854c83f9..44a9d53784 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -447,7 +447,7 @@ private: */ void handle_command(); - void reset_takeoff_state(); + void reset_takeoff_state(bool force = false); void reset_landing_state(); /*