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

ArduCopter: on throttle failsafe, only initiate RTL if we are at least 15m from home

Browse Source
This commit is contained in:
rmackay9 2012-12-21 14:39:48 +09:00 committed by Andrew Tridgell
parent 466097b383
commit 5526ca204c
2 changed files with 20 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/config.h
View File

@ -445,6 +445,10 @@
# define FS_THR_VALUE_DEFAULT 975 # define FS_THR_VALUE_DEFAULT 975
#endif #endif
#ifndef FS_THR_RTL_MIN_DISTANCE
# define FS_THR_RTL_MIN_DISTANCE 1500 // the minimum distance from home in which
#endif
#ifndef MINIMUM_THROTTLE #ifndef MINIMUM_THROTTLE
# define MINIMUM_THROTTLE 130 # define MINIMUM_THROTTLE 130
@ -453,10 +457,6 @@
# define MAXIMUM_THROTTLE 1000 # define MAXIMUM_THROTTLE 1000
#endif #endif
#ifndef RTL_LOITER_TIME
# define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before begining final descent
#endif
#ifndef LAND_SPEED #ifndef LAND_SPEED
# define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s # define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s
#endif #endif
@ -652,7 +652,11 @@
#endif #endif
#ifndef RTL_ALT_MAX #ifndef RTL_ALT_MAX
# define RTL_ALT_MAX 8000 // Max height to return to home in cm # define RTL_ALT_MAX 8000 // Max height to return to home in cm (i.e 80m)
#endif
#ifndef RTL_LOITER_TIME
# define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before begining final descent
#endif #endif

24
ArduCopter/events.pde
View File

@ -18,33 +18,31 @@ static void failsafe_on_event()
// if throttle is zero disarm motors // if throttle is zero disarm motors
if (g.rc_3.control_in == 0) { if (g.rc_3.control_in == 0) {
init_disarm_motors(); init_disarm_motors();
}else if(ap.home_is_set == true) { }else if(ap.home_is_set == true && home_distance >= FS_THR_RTL_MIN_DISTANCE) {
set_mode(RTL); set_mode(RTL);
}else{ }else{
// We have no GPS so we will land // We have no GPS or are very close to home so we will land
set_mode(LAND); set_mode(LAND);
} }
break; break;
case AUTO: case AUTO:
// failsafe_throttle is 1 do RTL, 2 means continue with the mission // failsafe_throttle is 1 do RTL, 2 means continue with the mission
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) { if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
// To-Do: check distance from home before initiating RTL if(home_distance >= FS_THR_RTL_MIN_DISTANCE) {
set_mode(RTL); set_mode(RTL);
}else{
// We are very close to home so we will land
set_mode(LAND);
}
} }
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything // if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break; break;
default: default:
if(ap.home_is_set == true) { if(ap.home_is_set == true && home_distance >= FS_THR_RTL_MIN_DISTANCE) {
// same as above ^
// do_rtl sets the altitude to the current altitude by default
// To-Do: check distance from home before initiating RTL
set_mode(RTL); set_mode(RTL);
}else{ }else{
// We have no GPS so we will crash land in alt hold mode // We have no GPS or are very close to home so we will land
// the low throttle will bring us down at the maximum descent speed set_mode(LAND);
// To-Do: make LAND a throttle mode so it can operate without GPS
set_mode(ALT_HOLD);
set_new_altitude(0);
} }
break; break;
} }