added nav_WP I term reset at WP. Would like to consider scaling it with a trig function to keep continuity during nav.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2336 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-16 06:23:21 +00:00
parent 27ab016d98
commit 57f98db7ce
1 changed files with 3 additions and 0 deletions

View File

@ -119,6 +119,9 @@ void verify_commands(void)
if(verify_must()){ if(verify_must()){
Serial.printf("verified must cmd %d\n" , command_must_index); Serial.printf("verified must cmd %d\n" , command_must_index);
command_must_index = NO_COMMAND; command_must_index = NO_COMMAND;
// reset rate controlled nav
g.pid_nav_wp.reset_I();
}else{ }else{
Serial.printf("verified must false %d\n" , command_must_index); Serial.printf("verified must false %d\n" , command_must_index);
} }