From 776c7f5d6047feb0a81b44b25684a2da6c874ae2 Mon Sep 17 00:00:00 2001 From: Emmanuel Roussel Date: Thu, 28 Jan 2016 13:26:49 +0100 Subject: [PATCH] Updated commander to take free-fall into account Added free-fall status in vehicle_status topic --- src/modules/commander/commander.cpp | 9 ++++++--- src/modules/land_detector/MulticopterLandDetector.cpp | 2 +- src/modules/land_detector/land_detector_params.c | 2 +- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bccfb3b880..b1c33b74ce 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1119,6 +1119,7 @@ int commander_thread_main(int argc, char *argv[]) bool sensor_fail_tune_played = false; bool arm_tune_played = false; bool was_landed = true; + bool was_falling = false; bool was_armed = false; bool startup_in_hil = false; @@ -1918,10 +1919,11 @@ int commander_thread_main(int argc, char *argv[]) } if ((updated && status_flags.condition_local_altitude_valid) || check_for_disarming) { - if (was_landed != land_detector.landed) { - if (land_detector.landed) { + if ((was_landed != land_detector.landed) || (was_falling != land_detector.freefall)) { + if (land_detector.freefall) { + mavlink_and_console_log_info(&mavlink_log_pub, "FREEFALL DETECTED"); + } else 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"); } @@ -2623,6 +2625,7 @@ int commander_thread_main(int argc, char *argv[]) } was_landed = land_detector.landed; + was_falling = land_detector.freefall; was_armed = armed.armed; /* print new state */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 6b928067fb..c6759d74e5 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -126,7 +126,7 @@ bool MulticopterLandDetector::get_freefall_state() } acc_norm = sqrtf(acc_norm); //norm of specific force - int32_t elapsed_time_ms = (now - _freefallTimer)/1000; + int32_t elapsed_time_ms = (now - _freefallTimer) / 1000; bool freefall = (acc_norm < _params.acc_threshold_m_s2); if (!freefall || _freefallTimer == 0) { //reset timer if uav not falling _freefallTimer = now; diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index dfbeaddc69..2b5b411c73 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -105,7 +105,7 @@ PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 2.0f); * * @group Land Detector */ -PARAM_DEFINE_INT32(LNDMC_FFALL_TRIG, 200); //minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h +PARAM_DEFINE_INT32(LNDMC_FFALL_TRIG, 300); //minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h /** * Fixedwing max horizontal velocity