aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: bugfix to condition-yaw for relative angles

Browse Source 
Thanks to roque-canales for raising the issue and paradisephil for finding
the specific piece of code that went wrong and suggesting the fix.
This commit is contained in:
Randy Mackay 2014-09-26 12:20:40 +09:00
parent 45bc9fd10c
commit ef0e37b478
3 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/GCS_Mavlink.pde
View File

@ -1078,12 +1078,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_CONDITION_YAW: case MAV_CMD_CONDITION_YAW:
// param1 : target angle [0-360] // param1 : target angle [0-360]
// param2 : speed during change [deg per second] // param2 : speed during change [deg per second]
// param3 : direction (not supported) // param3 : direction (-1:ccw, +1:cw)
// param4 : relative offset (1) or absolute angle (0) // param4 : relative offset (1) or absolute angle (0)
if ((packet.param1 >= 0.0f) && if ((packet.param1 >= 0.0f) &&
(packet.param1 <= 360.0f) && (packet.param1 <= 360.0f) &&
((packet.param4 == 0) || (packet.param4 == 1))) { ((packet.param4 == 0) || (packet.param4 == 1))) {
set_auto_yaw_look_at_heading(packet.param1, packet.param2, (uint8_t)packet.param4); set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4);
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else { } else {
result = MAV_RESULT_FAILED; result = MAV_RESULT_FAILED;

1
ArduCopter/commands_logic.pde
View File

@ -812,6 +812,7 @@ static void do_yaw(const AP_Mission::Mission_Command& cmd)
set_auto_yaw_look_at_heading( set_auto_yaw_look_at_heading(
cmd.content.yaw.angle_deg, cmd.content.yaw.angle_deg,
cmd.content.yaw.turn_rate_dps, cmd.content.yaw.turn_rate_dps,
cmd.content.yaw.direction,
cmd.content.yaw.relative_angle); cmd.content.yaw.relative_angle);
} }

7
ArduCopter/control_auto.pde
View File

@ -475,7 +475,7 @@ void set_auto_yaw_mode(uint8_t yaw_mode)
} }
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode // set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, uint8_t relative_angle) static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
{ {
// get current yaw target // get current yaw target
int32_t curr_yaw_target = attitude_control.angle_ef_targets().z; int32_t curr_yaw_target = attitude_control.angle_ef_targets().z;
@ -486,7 +486,10 @@ static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, u
yaw_look_at_heading = wrap_360_cd(angle_deg * 100); yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
} else { } else {
// relative angle // relative angle
yaw_look_at_heading = wrap_360_cd(angle_deg * 100); if (direction < 0) {
angle_deg = -angle_deg;
}
yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target));
} }
// get turn speed // get turn speed