Quick RTL Fix, logic was backward

This commit is contained in:
Jason Short 2011-10-07 11:06:31 -07:00
parent 05ee33d9ad
commit c25478448b
1 changed files with 3 additions and 3 deletions

View File

@ -1125,6 +1125,9 @@ static void update_navigation()
case RTL:
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// lets just jump to Loiter Mode after RTL
set_mode(LOITER);
}else{
// calculates desired Yaw
// XXX this is an experiment
#if FRAME_CONFIG == HELI_FRAME
@ -1132,9 +1135,6 @@ static void update_navigation()
#endif
wp_control = WP_MODE;
}else{
// lets just jump to Loiter Mode after RTL
set_mode(LOITER);
}
// calculates the desired Roll and Pitch