Copter: move init of global nav variables to set_nav_mode

This commit is contained in:
Randy Mackay 2013-09-09 10:37:44 +09:00
parent 4842335a24
commit b7579bb028
2 changed files with 2 additions and 4 deletions

View File

@ -89,6 +89,8 @@ static bool set_nav_mode(uint8_t new_nav_mode)
case NAV_NONE:
nav_initialised = true;
// initialise global navigation variables including wp_distance, nav_roll
reset_nav_params();
break;
case NAV_CIRCLE:

View File

@ -523,10 +523,6 @@ static bool set_mode(uint8_t mode)
// update flight mode
if (success) {
if(ap.manual_attitude) {
// We are under manual attitude control so initialise nav parameter for when we next enter an autopilot mode
reset_nav_params();
}
control_mode = mode;
Log_Write_Mode(control_mode);
}else{