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

Rover: adjust set-position-target-local-ned handling 

add support for just yaw or yaw-rate
remove support for simultaneous position and velocity
This commit is contained in:
Randy Mackay 2017-08-01 19:09:05 +09:00
parent 46faece76b
commit 3863542194
1 changed files with 74 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
APMrover2/GCS_Mavlink.cpp
View File

@ -1086,8 +1086,10 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
// prepare and send target position // prepare target position
Location target_loc = rover.current_loc; Location target_loc = rover.current_loc;
if (!pos_ignore) { if (!pos_ignore) {
switch (packet.coordinate_frame) { switch (packet.coordinate_frame) {
@ -1113,35 +1115,49 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
break; break;
} }
} }
float target_speed = 0.0f; float target_speed = 0.0f;
float target_turn_rate_cds = 0.0f; float target_yaw_cd = 0.0f;
// consume velocity and convert to target speed and heading
if (!vel_ignore) { if (!vel_ignore) {
// use packet vy (forward in NED) for speed in m/s and packet yaw_rate for turn in rad/s // convert vector length into a speed
float velx = packet.vx; target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -rover.g.speed_cruise, rover.g.speed_cruise);
float vely = packet.vy; // convert vector direction to target yaw
// rotate to body-frame if necessary 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) { if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
// rotate from body-frame to NE frame target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
velx = velx * rover.ahrs.cos_yaw() - vely * rover.ahrs.sin_yaw();
vely = velx * rover.ahrs.sin_yaw() + vely * rover.ahrs.cos_yaw();
} }
target_speed = vely;
target_turn_rate_cds = RAD_TO_DEG * packet.yaw_rate;
// TODO : take into account reverse speed
// TODO : handle yaw heading cmd
} }
if (!pos_ignore && !vel_ignore && acc_ignore) { float target_turn_rate_cds = 0.0f;
rover.mode_guided.set_desired_location(target_loc);
if (!is_zero(target_speed)) { // consume yaw heading and yaw rate
rover.mode_guided.set_desired_speed(target_speed); if (!yaw_ignore) {
target_yaw_cd = ToDeg(packet.yaw) * 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);
} }
} else if (pos_ignore && !vel_ignore && acc_ignore) { }
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, target_speed); if (!yaw_rate_ignore) {
} else if (!pos_ignore && vel_ignore && acc_ignore) { target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// set guided mode targets
if (!pos_ignore && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume position target
rover.mode_guided.set_desired_location(target_loc); rover.mode_guided.set_desired_location(target_loc);
} else { } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// result = MAV_RESULT_FAILED; // TODO : support that // consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_speed);
} 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)
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
} else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume just turn rate(probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
} }
break; break;
} }
@ -1166,8 +1182,10 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
// prepare and send target position // prepare target position
Location target_loc = rover.current_loc; Location target_loc = rover.current_loc;
if (!pos_ignore) { if (!pos_ignore) {
// sanity check location // sanity check location
@ -1178,27 +1196,45 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
target_loc.lat = packet.lat_int; target_loc.lat = packet.lat_int;
target_loc.lng = packet.lon_int; target_loc.lng = packet.lon_int;
} }
float target_speed = 0.0f; float target_speed = 0.0f;
float target_turn_rate_cds = 0.0f; float target_yaw_cd = 0.0f;
// consume velocity and convert to target speed and heading
if (!vel_ignore) { if (!vel_ignore) {
// use packet vy (forward in NED) for speed in m/s and packet yaw_rate for turn in rad/s // convert vector length into a speed
target_speed = packet.vy; target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -rover.g.speed_cruise, rover.g.speed_cruise);
target_turn_rate_cds = RAD_TO_DEG * packet.yaw_rate; // convert vector direction to target yaw
// TODO : take into account reverse speed target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
// TODO : handle yaw heading cmd
} }
if (!pos_ignore && !vel_ignore && acc_ignore) { float target_turn_rate_cds = 0.0f;
rover.mode_guided.set_desired_location(target_loc);
if (!is_zero(target_speed)) { // consume yaw heading and yaw rate
rover.mode_guided.set_desired_speed(target_speed); if (!yaw_ignore) {
target_yaw_cd = ToDeg(packet.yaw) * 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);
} }
} else if (pos_ignore && !vel_ignore && acc_ignore) { }
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, target_speed); if (!yaw_rate_ignore) {
} else if (!pos_ignore && vel_ignore && acc_ignore) { target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// set guided mode targets
if (!pos_ignore && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume position target
rover.mode_guided.set_desired_location(target_loc); rover.mode_guided.set_desired_location(target_loc);
} else { } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// result = MAV_RESULT_FAILED; // TODO : support that // consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_speed);
} 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)
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
} else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume just turn rate(probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
} }
break; break;
} }