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

Rover: fix guided consumption of SET_YAW_SPEED

This commit is contained in:
Randy Mackay 2019-11-05 12:07:24 +09:00
parent 129651b7e4
commit f15459f25e
1 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
APMrover2/GCS_Mavlink.cpp
View File

@ -648,16 +648,21 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l
{ {
// param1 : yaw angle to adjust direction by in centidegress // param1 : yaw angle to adjust direction by in centidegress
// param2 : Speed - normalized to 0 .. 1 // param2 : Speed - normalized to 0 .. 1
// param3 : 0 = absolute, 1 = relative
// exit if vehicle is not in Guided mode // exit if vehicle is not in Guided mode
if (!rover.control_mode->in_guided_mode()) { if (!rover.control_mode->in_guided_mode()) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
// send yaw change and target speed to guided mode controller // get final angle, 1 = Relative, 0 = Absolute
const float speed_max = rover.control_mode->get_speed_default(); if (packet.param3 > 0) {
const float target_speed = constrain_float(packet.param2 * speed_max, -speed_max, speed_max); // relative angle
rover.mode_guided.set_desired_heading_delta_and_speed(packet.param1, target_speed); rover.mode_guided.set_desired_heading_delta_and_speed(packet.param1 * 100.0f, packet.param2);
} else {
// absolute angle
rover.mode_guided.set_desired_heading_and_speed(packet.param1 * 100.0f, packet.param2);
}
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }