dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Updated commander to take free-fall into account 

Added free-fall status in vehicle_status topic
This commit is contained in:
Emmanuel Roussel 2016-01-28 13:26:49 +01:00 committed by Roman
parent 98d51179ad
commit 776c7f5d60
3 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/commander/commander.cpp
View File

@ -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 */

2
src/modules/land_detector/MulticopterLandDetector.cpp
View File

@ -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;

2
src/modules/land_detector/land_detector_params.c
View File

@ -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