diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index b5983ac742..cac5e60530 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1530,6 +1530,41 @@ Commander::run() } } + /* Update land detector */ + if (_land_detector_sub.updated()) { + const bool was_landed = _land_detector.landed; + _land_detector_sub.copy(&_land_detector); + + // Only take actions if armed + if (armed.armed) { + if (!was_landed && _land_detector.landed) { + mavlink_log_info(&mavlink_log_pub, "Landing detected"); + + } else if (was_landed && !_land_detector.landed) { + mavlink_log_info(&mavlink_log_pub, "Takeoff detected"); + _time_at_takeoff = hrt_absolute_time(); + _have_taken_off_since_arming = true; + + // Set all position and velocity test probation durations to takeoff value + // This is a larger value to give the vehicle time to complete a failsafe landing + // if faulty sensors cause loss of navigation shortly after takeoff. + _gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s; + _lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s; + _lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s; + + // automatically set or update home position + if (!_home_pub.get().manual_home) { + // set the home position when taking off, but only if we were previously disarmed + // and at least 500 ms from commander start spent to avoid setting home on in-air restart + if (_should_set_home_on_takeoff && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { + _should_set_home_on_takeoff = false; + set_home_position(); + } + } + } + } + } + /* update safety topic */ if (_safety_sub.updated()) { const bool previous_safety_off = _safety.safety_off; @@ -1540,12 +1575,9 @@ Commander::run() bool safety_disarm_allowed = (status.hil_state == vehicle_status_s::HIL_STATE_OFF); - // if land detector is available then prevent disarming via safety button if not landed - if (hrt_elapsed_time(&_land_detector.timestamp) < 1_s) { - - bool maybe_landing = (_land_detector.landed || _land_detector.maybe_landed); - - if (!maybe_landing) { + // prevent disarming via safety button if not landed + if (hrt_elapsed_time(&_land_detector.timestamp) < 10_s) { + if (!_land_detector.landed) { safety_disarm_allowed = false; } } @@ -1628,32 +1660,6 @@ Commander::run() estimator_check(status_flags); - /* Update land detector */ - if (_land_detector_sub.updated()) { - _was_landed = _land_detector.landed; - _land_detector_sub.copy(&_land_detector); - - // Only take actions if armed - if (armed.armed) { - if (_was_landed != _land_detector.landed) { - if (_land_detector.landed) { - mavlink_log_info(&mavlink_log_pub, "Landing detected"); - - } else { - mavlink_log_info(&mavlink_log_pub, "Takeoff detected"); - _have_taken_off_since_arming = true; - - // Set all position and velocity test probation durations to takeoff value - // This is a larger value to give the vehicle time to complete a failsafe landing - // if faulty sensors cause loss of navigation shortly after takeoff. - _gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s; - _lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s; - _lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s; - } - } - } - } - // Auto disarm when landed or kill switch engaged if (armed.armed) { @@ -2278,14 +2284,6 @@ Commander::run() if (!_home_pub.get().manual_home) { const vehicle_local_position_s &local_position = _local_position_sub.get(); - // set the home position when taking off, but only if we were previously disarmed - // and at least 500 ms from commander start spent to avoid setting home on in-air restart - if (_should_set_home_on_takeoff && _was_landed && !_land_detector.landed && - (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { - _should_set_home_on_takeoff = false; - set_home_position(); - } - if (!armed.armed) { if (status_flags.condition_home_position_valid) { if (_land_detector.landed && local_position.xy_valid && local_position.z_valid) { @@ -4137,15 +4135,10 @@ void Commander::estimator_check(const vehicle_status_flags_s &vstatus_flags) if (run_quality_checks && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - // reset flags and timer - _time_at_takeoff = hrt_absolute_time(); + // reset flags _nav_test_failed = false; _nav_test_passed = false; - } else if (_land_detector.landed) { - // record time of takeoff - _time_at_takeoff = hrt_absolute_time(); - } else { // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed const bool sufficient_time = (hrt_elapsed_time(&_time_at_takeoff) > 30_s); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 59953bbcac..f99d88fd85 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -365,7 +365,6 @@ private: bool _status_changed{true}; bool _arm_tune_played{false}; - bool _was_landed{true}; bool _was_armed{false}; bool _failsafe_old{false}; ///< check which state machines for changes, clear "changed" flag bool _have_taken_off_since_arming{false};