fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 01:17:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3056 from PX4/auto_disarm

Browse Source 
Commander: Disarm after 5 seconds on the ground.
This commit is contained in:
Roman Bapst
2015-10-29 23:19:34 +01:00
parent 8d59d08ad6 a71164ea11
commit 69dd80b42d
2 changed files with 38 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+25 -1
View File
@@ -183,6 +183,7 @@ static hrt_abstime commander_boot_timestamp = 0;
static unsigned int leds_counter;
/* To remember when last notification was sent */
static uint64_t last_print_mode_reject_time = 0;
static uint64_t _inair_last_time = 0;
static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f;
@@ -920,6 +921,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_eph = param_find("COM_HOME_H_T");
param_t _param_epv = param_find("COM_HOME_V_T");
param_t _param_geofence_action = param_find("GF_ACTION");
param_t _param_disarm_land = param_find("COM_DISARM_LAND");
// const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
// main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
@@ -1269,6 +1271,8 @@ int commander_thread_main(int argc, char *argv[])
int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */
int32_t disarm_when_landed = 0;
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false;
bool main_state_changed = false;
@@ -1348,6 +1352,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
param_get(_param_ef_time_thres, &ef_time_thres);
param_get(_param_geofence_action, &geofence_action);
param_get(_param_disarm_land, &disarm_when_landed);
/* Autostart id */
param_get(_param_autostart_id, &autostart_id);
@@ -1622,12 +1627,13 @@ int commander_thread_main(int argc, char *argv[])
&(status.condition_local_altitude_valid), &status_changed);
/* Update land detector */
static bool check_for_disarming = false;
orb_check(land_detector_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
}
if (updated && status.condition_local_altitude_valid) {
if ((updated && status.condition_local_altitude_valid) || check_for_disarming) {
if (status.condition_landed != land_detector.landed) {
status.condition_landed = land_detector.landed;
status_changed = true;
@@ -1639,6 +1645,24 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED");
}
}
if (disarm_when_landed > 0) {
if (land_detector.landed) {
if (!check_for_disarming && _inair_last_time > 0) {
_inair_last_time = land_detector.timestamp;
check_for_disarming = true;
}
if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) {
mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING");
arm_disarm(false, mavlink_fd, "auto disarm on land");
_inair_last_time = 0;
check_for_disarming = false;
}
} else {
_inair_last_time = land_detector.timestamp;
}
}
}
/* update battery status */
src/modules/commander/commander_params.c
+13
View File
@@ -274,3 +274,16 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1);
* @max 2
*/
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
/**
* Time-out for auto disarm after landing
*
* A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be
* automatically disarmed in case a landing situation has been detected during this period.
* A value of zero means that automatic disarming is disabled.
*
* @group Commander
* @min 0
*/
PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);