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

Copter: do_yaw takes yaw target from AttControl

This commit is contained in:
Randy Mackay 2014-02-03 22:02:59 +09:00 committed by Andrew Tridgell
parent ea1158a9d5
commit 1acd33e290
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/commands_logic.pde
View File

@ -517,13 +517,16 @@ static void do_within_distance()
static void do_yaw() static void do_yaw()
{ {
// get current yaw target
int32_t curr_yaw_target = attitude_control.angle_ef_targets().z;
// get final angle, 1 = Relative, 0 = Absolute // get final angle, 1 = Relative, 0 = Absolute
if( command_cond_queue.lng == 0 ) { if( command_cond_queue.lng == 0 ) {
// absolute angle // absolute angle
yaw_look_at_heading = wrap_360_cd(command_cond_queue.alt * 100); yaw_look_at_heading = wrap_360_cd(command_cond_queue.alt * 100);
}else{ }else{
// relative angle // relative angle
yaw_look_at_heading = wrap_360_cd(control_yaw + command_cond_queue.alt * 100); yaw_look_at_heading = wrap_360_cd(curr_yaw_target + command_cond_queue.alt * 100);
} }
// get turn speed // get turn speed
@ -531,7 +534,7 @@ static void do_yaw()
// default to regular auto slew rate // default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
}else{ }else{
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - control_yaw) / 100) / command_cond_queue.lat; int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / command_cond_queue.lat;
yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec
} }