Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: RESETTOARMEDYAW order and comments 

We should keep the order in the case statement the same as the #define
order.
This commit is contained in:
Randy Mackay 2013-05-21 10:58:34 +09:00
parent f646b79e5f
commit 00bd1bf8ea
3 changed files with 21 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduCopter/ArduCopter.pde
View File

@ -1376,8 +1376,6 @@ bool set_yaw_mode(uint8_t new_yaw_mode)
}
switch( new_yaw_mode ) {
case YAW_RESETTOARMEDYAW:
nav_yaw = ahrs.yaw_sensor; // store current yaw so we can start rotating back to correct one
case YAW_HOLD:
case YAW_ACRO:
yaw_initialised = true;
@ -1411,14 +1409,18 @@ bool set_yaw_mode(uint8_t new_yaw_mode)
yaw_initialised = true;
}
break;
case YAW_TOY:
yaw_initialised = true;
break;
case YAW_LOOK_AHEAD:
if( ap.home_is_set ) {
yaw_initialised = true;
}
break;
case YAW_TOY:
yaw_initialised = true;
break;
case YAW_RESETTOARMEDYAW:
nav_yaw = ahrs.yaw_sensor; // store current yaw so we can start rotating back to correct one
yaw_initialised = true;
break;
}
// if initialisation has been successful update the yaw mode
@ -1450,11 +1452,6 @@ void update_yaw_mode(void)
}
break;
case YAW_RESETTOARMEDYAW:
nav_yaw = get_yaw_slew(nav_yaw, initial_simple_bearing, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(nav_yaw);
break;
case YAW_LOOK_AT_NEXT_WP:
// point towards next waypoint (no pilot input accepted)
// we don't use wp_bearing because we don't want the copter to turn too much during flight
@ -1517,6 +1514,12 @@ void update_yaw_mode(void)
get_stabilize_yaw(nav_yaw);
break;
#endif
case YAW_RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
nav_yaw = get_yaw_slew(nav_yaw, initial_simple_bearing, AUTO_YAW_SLEW_RATE);
get_stabilize_yaw(nav_yaw);
break;
}
}

14
ArduCopter/control_modes.pde
View File

@ -91,13 +91,6 @@ static void do_aux_switch_function(int8_t ch_function, bool ch_flag)
}
switch(tmp_function) {
case AUX_SWITCH_RESETTOARMEDYAW:
if(ch_flag)
set_yaw_mode(YAW_RESETTOARMEDYAW);
else
set_yaw_mode(YAW_HOLD);
break;
case AUX_SWITCH_FLIP:
// flip if switch is on, positive throttle and we're actually flying
if(ch_flag && g.rc_3.control_in >= 0 && ap.takeoff_complete) {
@ -198,6 +191,13 @@ static void do_aux_switch_function(int8_t ch_function, bool ch_flag)
fence.enable(ch_flag);
break;
#endif
case AUX_SWITCH_RESETTOARMEDYAW:
if (ch_flag) {
set_yaw_mode(YAW_RESETTOARMEDYAW);
}else{
set_yaw_mode(YAW_HOLD);
}
break;
}
}

2
ArduCopter/defines.h
View File

@ -24,7 +24,7 @@
#define YAW_LOOK_AT_HEADING 6 // point towards a particular angle (not pilot input accepted)
#define YAW_LOOK_AHEAD 7 // WARNING! CODE IN DEVELOPMENT NOT PROVEN
#define YAW_TOY 8 // THOR This is the Yaw mode
#define YAW_RESETTOARMEDYAW 9
#define YAW_RESETTOARMEDYAW 9 // point towards heading at time motors were armed
#define ROLL_PITCH_STABLE 0 // pilot input roll, pitch angles
#define ROLL_PITCH_ACRO 1 // pilot inputs roll, pitch rotation rates