Browse Source

Commander: Do not switch land detection state when not armed

This is important to have the probation times set up correctly and to silence land detected messages for systems that are not actually flying and just on the bench.
sbg
Lorenz Meier 7 years ago
parent
commit
3b71c70583
  1. 41
      src/modules/commander/commander.cpp

41
src/modules/commander/commander.cpp

@ -1,6 +1,5 @@ @@ -1,6 +1,5 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
@ -2170,30 +2169,32 @@ Commander::run() @@ -2170,30 +2169,32 @@ Commander::run()
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
if (was_landed != land_detector.landed) {
if (land_detector.landed) {
mavlink_and_console_log_info(&mavlink_log_pub, "Landing detected");
} else {
mavlink_and_console_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 = posctl_nav_loss_prob;
gvel_probation_time_us = posctl_nav_loss_prob;
lpos_probation_time_us = posctl_nav_loss_prob;
lvel_probation_time_us = posctl_nav_loss_prob;
// Only take actions if armed
if (armed.armed) {
if (was_landed != land_detector.landed) {
if (land_detector.landed) {
mavlink_and_console_log_info(&mavlink_log_pub, "Landing detected");
} else {
mavlink_and_console_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 = posctl_nav_loss_prob;
gvel_probation_time_us = posctl_nav_loss_prob;
lpos_probation_time_us = posctl_nav_loss_prob;
lvel_probation_time_us = posctl_nav_loss_prob;
}
}
}
if (was_falling != land_detector.freefall) {
if (land_detector.freefall) {
mavlink_and_console_log_info(&mavlink_log_pub, "Freefall detected");
if (was_falling != land_detector.freefall) {
if (land_detector.freefall) {
mavlink_and_console_log_info(&mavlink_log_pub, "Freefall detected");
}
}
}
was_landed = land_detector.landed;
was_falling = land_detector.freefall;
}

Loading…
Cancel
Save