fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 07:33:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: fix convoluted auto disarm

Browse Source
This commit is contained in:
Julian Oes
2016-06-07 12:34:54 +02:00
committed by Lorenz Meier
parent 294c05d607
commit ba7d35d9d0
1 changed files with 6 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+6 -13
View File
@@ -1950,15 +1950,12 @@ int commander_thread_main(int argc, char *argv[])
&(status_flags.condition_local_altitude_valid), &status_changed); &(status_flags.condition_local_altitude_valid), &status_changed);
/* Update land detector */ /* Update land detector */
static bool check_for_disarming = false;
orb_check(land_detector_sub, &updated); orb_check(land_detector_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
}
if ((updated && status_flags.condition_local_altitude_valid) || check_for_disarming) {
if (was_landed != land_detector.landed) { if (was_landed != land_detector.landed) {
if (land_detector.landed && armed.armed) { if (land_detector.landed) {
mavlink_and_console_log_info(&mavlink_log_pub, "LANDING DETECTED"); mavlink_and_console_log_info(&mavlink_log_pub, "LANDING DETECTED");
} else { } else {
mavlink_and_console_log_info(&mavlink_log_pub, "TAKEOFF DETECTED"); mavlink_and_console_log_info(&mavlink_log_pub, "TAKEOFF DETECTED");
@@ -1973,20 +1970,18 @@ int commander_thread_main(int argc, char *argv[])
if (disarm_when_landed > 0) { if (disarm_when_landed > 0) {
if (land_detector.landed) { if (land_detector.landed) {
if (!check_for_disarming && _inair_last_time > 0) { if (_inair_last_time > 0 &&
_inair_last_time = land_detector.timestamp; (hrt_elapsed_time(&_inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) {
check_for_disarming = true;
}
if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) {
arm_disarm(false, &mavlink_log_pub, "auto disarm on land"); arm_disarm(false, &mavlink_log_pub, "auto disarm on land");
_inair_last_time = 0; _inair_last_time = 0;
check_for_disarming = false;
} }
} else { } else {
_inair_last_time = land_detector.timestamp; _inair_last_time = land_detector.timestamp;
} }
} }
was_landed = land_detector.landed;
was_falling = land_detector.freefall;
} }
if (!rtl_on) { if (!rtl_on) {
@@ -2678,8 +2673,6 @@ int commander_thread_main(int argc, char *argv[])
commander_set_home_position(home_pub, _home, local_position, global_position, attitude); commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
} }
was_landed = land_detector.landed;
was_falling = land_detector.freefall;
was_armed = armed.armed; was_armed = armed.armed;
/* print new state */ /* print new state */