Added minimum Altitude for RTL failsafe
This commit is contained in:
parent
b09ab99699
commit
67873813ce
@ -17,8 +17,10 @@ static void failsafe_on_event()
|
|||||||
|
|
||||||
default:
|
default:
|
||||||
if(home_is_set == true){
|
if(home_is_set == true){
|
||||||
if (get_distance(¤t_loc, &home) > 15){
|
if ((get_distance(¤t_loc, &home) > 15) && (current_loc.alt > 400)){
|
||||||
set_mode(RTL);
|
set_mode(RTL);
|
||||||
|
// override safety
|
||||||
|
motor_auto_armed = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
Loading…
Reference in New Issue
Block a user