Copter: remove unnecessary check of flight mode from fence response

This commit is contained in:
Randy Mackay 2013-10-24 16:42:14 +09:00
parent fcc1b8e190
commit c7d16d2c3b

View File

@ -39,14 +39,12 @@ void fence_check()
if (GPS_ok()) { if (GPS_ok()) {
// if we are within 100m of the fence, RTL // if we are within 100m of the fence, RTL
if( fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { if( fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if(control_mode != RTL) { if (!set_mode(RTL)) {
set_mode(RTL); set_mode(LAND);
} }
}else{ }else{
// if more than 100m outside the fence just force a land // if more than 100m outside the fence just force a land
if(control_mode != LAND) { set_mode(LAND);
set_mode(LAND);
}
} }
}else{ }else{
// we have no GPS so LAND // we have no GPS so LAND