Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 01:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

nav updates

Browse Source
This commit is contained in:
Jason Short 2011-11-12 21:46:57 -08:00
parent b8c8d22fed
commit e6acbacdb6
1 changed files with 19 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
ArduCopter/system.pde
View File

@ -377,42 +377,33 @@ static void set_mode(byte mode)
// report the GPS and Motor arming status // report the GPS and Motor arming status
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
reset_nav();
switch(control_mode) switch(control_mode)
{ {
case ACRO: case ACRO:
yaw_mode = YAW_ACRO; yaw_mode = YAW_ACRO;
roll_pitch_mode = ROLL_PITCH_ACRO; roll_pitch_mode = ROLL_PITCH_ACRO;
throttle_mode = THROTTLE_MANUAL; throttle_mode = THROTTLE_MANUAL;
reset_hold_I();
break; break;
case STABILIZE: case STABILIZE:
yaw_mode = YAW_HOLD; yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_STABLE; roll_pitch_mode = ROLL_PITCH_STABLE;
throttle_mode = THROTTLE_MANUAL; throttle_mode = THROTTLE_MANUAL;
reset_hold_I();
break; break;
case ALT_HOLD: case ALT_HOLD:
yaw_mode = ALT_HOLD_YAW; yaw_mode = ALT_HOLD_YAW;
roll_pitch_mode = ALT_HOLD_RP; roll_pitch_mode = ALT_HOLD_RP;
throttle_mode = ALT_HOLD_THR; throttle_mode = ALT_HOLD_THR;
reset_hold_I();
init_throttle_cruise();
next_WP = current_loc; next_WP = current_loc;
break; break;
case AUTO: case AUTO:
reset_hold_I();
yaw_mode = AUTO_YAW; yaw_mode = AUTO_YAW;
roll_pitch_mode = AUTO_RP; roll_pitch_mode = AUTO_RP;
throttle_mode = AUTO_THR; throttle_mode = AUTO_THR;
init_throttle_cruise();
// loads the commands from where we left off // loads the commands from where we left off
init_commands(); init_commands();
break; break;
@ -422,8 +413,7 @@ static void set_mode(byte mode)
roll_pitch_mode = CIRCLE_RP; roll_pitch_mode = CIRCLE_RP;
throttle_mode = CIRCLE_THR; throttle_mode = CIRCLE_THR;
init_throttle_cruise(); next_WP = current_loc;
next_WP = current_loc;
break; break;
case LOITER: case LOITER:
@ -431,8 +421,7 @@ static void set_mode(byte mode)
roll_pitch_mode = LOITER_RP; roll_pitch_mode = LOITER_RP;
throttle_mode = LOITER_THR; throttle_mode = LOITER_THR;
init_throttle_cruise(); next_WP = current_loc;
next_WP = current_loc;
break; break;
case POSITION: case POSITION:
@ -440,7 +429,7 @@ static void set_mode(byte mode)
roll_pitch_mode = ROLL_PITCH_AUTO; roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_MANUAL; throttle_mode = THROTTLE_MANUAL;
next_WP = current_loc; next_WP = current_loc;
break; break;
case GUIDED: case GUIDED:
@ -448,8 +437,6 @@ static void set_mode(byte mode)
roll_pitch_mode = ROLL_PITCH_AUTO; roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO; throttle_mode = THROTTLE_AUTO;
//xtrack_enabled = true;
init_throttle_cruise();
next_WP = current_loc; next_WP = current_loc;
set_next_WP(&guided_WP); set_next_WP(&guided_WP);
break; break;
@ -459,8 +446,6 @@ static void set_mode(byte mode)
roll_pitch_mode = RTL_RP; roll_pitch_mode = RTL_RP;
throttle_mode = RTL_THR; throttle_mode = RTL_THR;
//xtrack_enabled = true;
init_throttle_cruise();
do_RTL(); do_RTL();
break; break;
@ -468,6 +453,22 @@ static void set_mode(byte mode)
break; break;
} }
if(throttle_mode == THROTTLE_MANUAL){
// reset all of the throttle iterms
g.pi_alt_hold.reset_I();
g.pi_throttle.reset_I();
}else { // an automatic throttle
// todo: replace with a throttle cruise estimator
init_throttle_cruise();
}
if(roll_pitch_mode <= ROLL_PITCH_ACRO){
// We are under manual attitude control
// reset out nav parameters
reset_nav();
}
Log_Write_Mode(control_mode); Log_Write_Mode(control_mode);
} }