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

Copter: set notify event when failsafe changes flight mode

Browse Source
This commit is contained in:
Randy Mackay 2014-12-15 12:48:54 +09:00
parent 1c8ab375e2
commit 562f3e7382
2 changed files with 38 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/control_land.pde
View File

@ -241,10 +241,14 @@ static void land_do_not_use_GPS()
} }
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts // set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
static void set_mode_land_with_pause() static void set_mode_land_with_pause()
{ {
set_mode(LAND); set_mode(LAND);
land_pause = true; land_pause = true;
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
} }
// landing_with_GPS - returns true if vehicle is landing using GPS // landing_with_GPS - returns true if vehicle is landing using GPS

62
ArduCopter/events.pde
View File

@ -25,9 +25,8 @@ static void failsafe_radio_on_event()
// if far from home then RTL // if far from home then RTL
}else if(home_distance > wp_nav.get_wp_radius()) { }else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { // switch to RTL or if that fails, LAND
set_mode_land_with_pause(); set_mode_RTL_or_land_with_pause();
}
// We have no GPS or are very close to home so we will land // We have no GPS or are very close to home so we will land
}else{ }else{
@ -47,9 +46,8 @@ static void failsafe_radio_on_event()
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_RTL do RTL // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_RTL do RTL
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) { } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
if(home_distance > wp_nav.get_wp_radius()) { if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { // switch to RTL or if that fails, LAND
set_mode_land_with_pause(); set_mode_RTL_or_land_with_pause();
}
}else{ }else{
// We are very close to home so we will land // We are very close to home so we will land
set_mode_land_with_pause(); set_mode_land_with_pause();
@ -76,10 +74,8 @@ static void failsafe_radio_on_event()
// if far from home then RTL // if far from home then RTL
}else if(home_distance > wp_nav.get_wp_radius()) { }else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)){ // switch to RTL or if that fails, LAND
// if RTL fails because of no GPS, then LAND set_mode_RTL_or_land_with_pause();
set_mode_land_with_pause();
}
}else{ }else{
// We have no GPS or are very close to home so we will land // We have no GPS or are very close to home so we will land
set_mode_land_with_pause(); set_mode_land_with_pause();
@ -120,9 +116,8 @@ static void failsafe_battery_event(void)
}else{ }else{
// set mode to RTL or LAND // set mode to RTL or LAND
if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) { if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { // switch to RTL or if that fails, LAND
set_mode_land_with_pause(); set_mode_RTL_or_land_with_pause();
}
}else{ }else{
set_mode_land_with_pause(); set_mode_land_with_pause();
} }
@ -135,9 +130,8 @@ static void failsafe_battery_event(void)
// set mode to RTL or LAND // set mode to RTL or LAND
} else if (home_distance > wp_nav.get_wp_radius()) { } else if (home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { // switch to RTL or if that fails, LAND
set_mode_land_with_pause(); set_mode_RTL_or_land_with_pause();
}
} else { } else {
set_mode_land_with_pause(); set_mode_land_with_pause();
} }
@ -150,9 +144,8 @@ static void failsafe_battery_event(void)
// set mode to RTL or LAND // set mode to RTL or LAND
} else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) { } else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { // switch to RTL or if that fails, LAND
set_mode_land_with_pause(); set_mode_RTL_or_land_with_pause();
}
} else { } else {
set_mode_land_with_pause(); set_mode_land_with_pause();
} }
@ -212,6 +205,8 @@ static void failsafe_gps_check()
if (mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) { if (mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
if (g.failsafe_gps_enabled == FS_GPS_ALTHOLD && !failsafe.radio) { if (g.failsafe_gps_enabled == FS_GPS_ALTHOLD && !failsafe.radio) {
set_mode(ALT_HOLD); set_mode(ALT_HOLD);
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
}else{ }else{
set_mode_land_with_pause(); set_mode_land_with_pause();
} }
@ -277,9 +272,8 @@ static void failsafe_gcs_check()
if (ap.throttle_zero) { if (ap.throttle_zero) {
init_disarm_motors(); init_disarm_motors();
}else if(home_distance > wp_nav.get_wp_radius()) { }else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { // switch to RTL or if that fails, LAND
set_mode_land_with_pause(); set_mode_RTL_or_land_with_pause();
}
}else{ }else{
// We have no GPS or are very close to home so we will land // We have no GPS or are very close to home so we will land
set_mode_land_with_pause(); set_mode_land_with_pause();
@ -289,9 +283,8 @@ static void failsafe_gcs_check()
// if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission // if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) { if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
if (home_distance > wp_nav.get_wp_radius()) { if (home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { // switch to RTL or if that fails, LAND
set_mode_land_with_pause(); set_mode_RTL_or_land_with_pause();
}
}else{ }else{
// We are very close to home so we will land // We are very close to home so we will land
set_mode_land_with_pause(); set_mode_land_with_pause();
@ -301,9 +294,8 @@ static void failsafe_gcs_check()
break; break;
default: default:
if(home_distance > wp_nav.get_wp_radius()) { if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { // switch to RTL or if that fails, LAND
set_mode_land_with_pause(); set_mode_RTL_or_land_with_pause();
}
}else{ }else{
// We have no GPS or are very close to home so we will land // We have no GPS or are very close to home so we will land
set_mode_land_with_pause(); set_mode_land_with_pause();
@ -319,6 +311,20 @@ static void failsafe_gcs_off_event(void)
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED); Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
} }
// set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
static void set_mode_RTL_or_land_with_pause()
{
// attempt to switch to RTL, if this fails then switch to Land
if (!set_mode(RTL)) {
// set mode to land will trigger mode change notification to pilot
set_mode_land_with_pause();
} else {
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
}
}
static void update_events() static void update_events()
{ {
ServoRelayEvents.update_events(); ServoRelayEvents.update_events();