Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

commander: disarm from safety relax land detector timeout

Browse Source 
- ensure land detector is updated before safety is checked
This commit is contained in:
Daniel Agar
2020-09-24 11:25:45 -04:00
parent 1dbd7df83b
commit 2ccf664e95
2 changed files with 39 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+39 -46
View File
@@ -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);
src/modules/commander/Commander.hpp
-1
View File
@@ -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};