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

Rover: adjust setpoint_velocity handling

This commit is contained in:
khancyr 2017-10-31 01:00:03 +01:00 committed by Randy Mackay
parent 9b2e48ce10
commit 9299943347
1 changed files with 34 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
APMrover2/GCS_Mavlink.cpp
View File

@ -1118,6 +1118,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -rover.g.speed_cruise, rover.g.speed_cruise); target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -rover.g.speed_cruise, rover.g.speed_cruise);
// convert vector direction to target yaw // convert vector direction to target yaw
target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f; target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
// rotate target yaw if provided in body-frame // rotate target yaw if provided in body-frame
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor); target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
@ -1137,6 +1138,13 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
if (!yaw_rate_ignore) { if (!yaw_rate_ignore) {
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
} }
// by default, we consider that the rover will drive forward
float target_dir = 1.0f;
if (!yaw_ignore || !yaw_rate_ignore) {
if (is_negative(packet.vx)) {
target_dir = -1.0f;
}
}
// set guided mode targets // set guided mode targets
if (!pos_ignore && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { if (!pos_ignore && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
@ -1144,7 +1152,13 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
rover.mode_guided.set_desired_location(target_loc); rover.mode_guided.set_desired_location(target_loc);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity // consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_speed); rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume velocity and turn rate
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, target_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_dir * target_speed);
} else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { } else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume just target heading (probably only skid steering vehicles can do this) // consume just target heading (probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f); rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
@ -1201,6 +1215,11 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -rover.g.speed_cruise, rover.g.speed_cruise); target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -rover.g.speed_cruise, rover.g.speed_cruise);
// convert vector direction to target yaw // convert vector direction to target yaw
target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f; target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
// rotate target yaw if provided in body-frame
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
}
} }
float target_turn_rate_cds = 0.0f; float target_turn_rate_cds = 0.0f;
@ -1216,6 +1235,13 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
if (!yaw_rate_ignore) { if (!yaw_rate_ignore) {
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
} }
// by default, we consider that the rover will drive forward
float target_dir = 1.0f;
if (!yaw_ignore || !yaw_rate_ignore) {
if (is_negative(packet.vx)) {
target_dir = -1.0f;
}
}
// set guided mode targets // set guided mode targets
if (!pos_ignore && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { if (!pos_ignore && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
@ -1223,7 +1249,13 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
rover.mode_guided.set_desired_location(target_loc); rover.mode_guided.set_desired_location(target_loc);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity // consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_speed); rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume velocity and turn rate
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, target_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_dir * target_speed);
} else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { } else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume just target heading (probably only skid steering vehicles can do this) // consume just target heading (probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f); rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);