aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: Address safety issues and ensure motors disarm during gcs failsafe

Browse Source
This commit is contained in:
jaxxzer 2016-02-11 23:03:57 -05:00 committed by Andrew Tridgell
parent eb76fc7a3c
commit 0e2f0321f9
3 changed files with 6 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduSub/GCS_Mavlink.cpp
View File

@ -1431,8 +1431,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (sub.init_arm_motors(true)) {
result = MAV_RESULT_ACCEPTED;
}
} else if (is_zero(packet.param1) && (sub.ap.land_complete || is_equal(packet.param2,21196.0f))) {
} else if (is_zero(packet.param1)) {
// force disarming by setting param2 = 21196 is deprecated
// see COMMAND_LONG DO_FLIGHTTERMINATION
sub.init_disarm_motors();
result = MAV_RESULT_ACCEPTED;
} else {

2
ArduSub/config.h
View File

@ -214,7 +214,7 @@
# define FS_GCS DISABLED
#endif
#ifndef FS_GCS_TIMEOUT_MS
# define FS_GCS_TIMEOUT_MS 5000 // gcs failsafe triggers after 5 seconds with no GCS heartbeat
# define FS_GCS_TIMEOUT_MS 2500 // gcs failsafe triggers after 5 seconds with no GCS heartbeat
#endif
#ifndef GNDEFFECT_COMPENSATION

51
ArduSub/events.cpp
View File

@ -171,7 +171,7 @@ void Sub::failsafe_gcs_check()
// return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs and we are not in guided mode
// this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
if ((!failsafe.gcs)&&(g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!failsafe.rc_override_active && control_mode != GUIDED))) {
if ((!failsafe.gcs) && (g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!failsafe.rc_override_active && control_mode != GUIDED))) {
return;
}
@ -203,53 +203,8 @@ void Sub::failsafe_gcs_check()
hal.rcin->clear_overrides();
failsafe.rc_override_active = false;
// This is how to handle a failsafe.
// use the throttle failsafe setting to decide what to do
switch(control_mode) {
case STABILIZE:
case ACRO:
case SPORT:
// if throttle is zero disarm motors
if (ap.throttle_zero || ap.land_complete) {
init_disarm_motors();
}else if(home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We have no GPS or are very close to home so we will land
set_mode_land_with_pause();
}
break;
case AUTO:
// if mission has not started AND vehicle is landed, disarm motors
if (!ap.auto_armed && ap.land_complete) {
init_disarm_motors();
// if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
if (home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We are very close to home so we will land
set_mode_land_with_pause();
}
}
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break;
default:
// used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
// if landed disarm
if (ap.land_complete) {
init_disarm_motors();
} else if (home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We have no GPS or are very close to home so we will land
set_mode_land_with_pause();
}
break;
}
// Disarm the motors, pilot has lost all contact/control over vehicle.
init_disarm_motors();
}
// failsafe_gcs_off_event - actions to take when GCS contact is restored