diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 4eb63da04a..0247d04f60 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1243,11 +1243,9 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) // prepare yaw float yaw_cd = 0.0f; - bool yaw_relative = false; float yaw_rate_cds = 0.0f; if (!yaw_ignore) { yaw_cd = ToDeg(packet.yaw) * 100.0f; - yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED; } if (!yaw_rate_ignore) { yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; @@ -1268,13 +1266,13 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) copter.mode_guided.init(true); break; } - copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds); } else if (pos_ignore && !vel_ignore) { - copter.mode_guided.set_velaccel(vel_vector, accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.mode_guided.set_velaccel(vel_vector, accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds); } else if (pos_ignore && vel_ignore && !acc_ignore) { - copter.mode_guided.set_accel(accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.mode_guided.set_accel(accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds); } else if (!pos_ignore && vel_ignore && acc_ignore) { - copter.mode_guided.set_destination(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.mode_guided.set_destination(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds); } else { // input is not valid so stop copter.mode_guided.init(true);