Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: reindent all of the new GCS_MAVLink handling methods (NFC)

Browse Source
This commit is contained in:
Peter Barker 2021-01-04 20:23:55 +11:00 committed by Randy Mackay
parent 2240a60e20
commit 0273d92887
1 changed files with 303 additions and 303 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

606
Rover/GCS_Mavlink.cpp
View File

@ -779,344 +779,344 @@ void GCS_MAVLINK_Rover::handleMessage(const mavlink_message_t &msg)
} }
void GCS_MAVLINK_Rover::handle_manual_control(const mavlink_message_t &msg) void GCS_MAVLINK_Rover::handle_manual_control(const mavlink_message_t &msg)
{ {
if (msg.sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs if (msg.sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs
return; return;
}
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(&msg, &packet);
if (packet.target != rover.g.sysid_this_mav) {
return; // only accept control aimed at us
}
uint32_t tnow = AP_HAL::millis();
manual_override(rover.channel_steer, packet.y, 1000, 2000, tnow);
manual_override(rover.channel_throttle, packet.z, 1000, 2000, tnow);
// a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
rover.failsafe.last_heartbeat_ms = tnow;
} }
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(&msg, &packet);
if (packet.target != rover.g.sysid_this_mav) {
return; // only accept control aimed at us
}
uint32_t tnow = AP_HAL::millis();
manual_override(rover.channel_steer, packet.y, 1000, 2000, tnow);
manual_override(rover.channel_throttle, packet.z, 1000, 2000, tnow);
// a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
rover.failsafe.last_heartbeat_ms = tnow;
}
void GCS_MAVLINK_Rover::handle_heartbeat(const mavlink_message_t &msg) void GCS_MAVLINK_Rover::handle_heartbeat(const mavlink_message_t &msg)
{ {
// we keep track of the last time we received a heartbeat from our GCS for failsafe purposes // we keep track of the last time we received a heartbeat from our GCS for failsafe purposes
if (msg.sysid != rover.g.sysid_my_gcs) { if (msg.sysid != rover.g.sysid_my_gcs) {
return; return;
} }
rover.failsafe.last_heartbeat_ms = AP_HAL::millis(); rover.failsafe.last_heartbeat_ms = AP_HAL::millis();
} }
void GCS_MAVLINK_Rover::handle_set_attitude_target(const mavlink_message_t &msg) void GCS_MAVLINK_Rover::handle_set_attitude_target(const mavlink_message_t &msg)
{ {
// decode packet // decode packet
mavlink_set_attitude_target_t packet; mavlink_set_attitude_target_t packet;
mavlink_msg_set_attitude_target_decode(&msg, &packet); mavlink_msg_set_attitude_target_decode(&msg, &packet);
// 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; return;
} }
// ensure type_mask specifies to use thrust // ensure type_mask specifies to use thrust
if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) { if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) {
return; return;
} }
// convert thrust to ground speed // convert thrust to ground speed
packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f); packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
const float target_speed = rover.control_mode->get_speed_default() * packet.thrust; const float target_speed = rover.control_mode->get_speed_default() * packet.thrust;
// if the body_yaw_rate field is ignored, convert quaternion to heading // if the body_yaw_rate field is ignored, convert quaternion to heading
if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) { if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) {
// convert quaternion to heading // convert quaternion to heading
float target_heading_cd = degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100.0f; float target_heading_cd = degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100.0f;
rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed); rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed);
} else { } else {
// use body_yaw_rate field // use body_yaw_rate field
rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed); rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed);
} }
} }
void GCS_MAVLINK_Rover::handle_set_position_target_local_ned(const mavlink_message_t &msg) void GCS_MAVLINK_Rover::handle_set_position_target_local_ned(const mavlink_message_t &msg)
{ {
// decode packet // decode packet
mavlink_set_position_target_local_ned_t packet; mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(&msg, &packet); mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
// 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; return;
} }
// need ekf origin // need ekf origin
Location ekf_origin; Location ekf_origin;
if (!rover.ahrs.get_origin(ekf_origin)) { if (!rover.ahrs.get_origin(ekf_origin)) {
return; return;
} }
// check for supported coordinate frames // check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_NED && packet.coordinate_frame != MAV_FRAME_BODY_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
return; return;
} }
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_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; bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
// prepare 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) {
case MAV_FRAME_BODY_NED: case MAV_FRAME_BODY_NED:
case MAV_FRAME_BODY_OFFSET_NED: { case MAV_FRAME_BODY_OFFSET_NED: {
// rotate from body-frame to NE frame // rotate from body-frame to NE frame
const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw(); const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw();
const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw(); const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw();
// add offset to current location // add offset to current location
target_loc.offset(ne_x, ne_y); target_loc.offset(ne_x, ne_y);
}
break;
case MAV_FRAME_LOCAL_OFFSET_NED:
// add offset to current location
target_loc.offset(packet.x, packet.y);
break;
case MAV_FRAME_LOCAL_NED:
default:
// MAV_FRAME_LOCAL_NED is interpreted as an offset from EKF origin
target_loc = ekf_origin;
target_loc.offset(packet.x, packet.y);
break;
}
}
float target_speed = 0.0f;
float target_yaw_cd = 0.0f;
// consume velocity and convert to target speed and heading
if (!vel_ignore) {
const float speed_max = rover.control_mode->get_speed_default();
// convert vector length into a speed
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
// convert vector direction to target yaw
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);
}
}
// consume yaw heading
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);
}
}
// consume yaw rate
float target_turn_rate_cds = 0.0f;
if (!yaw_rate_ignore) {
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// handling case when both velocity and either yaw or yaw-rate are provided
// by default, we consider that the rover will drive forward
float speed_dir = 1.0f;
if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
// Note: we are using the x-axis velocity to determine direction even though
// the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED
if (is_negative(packet.vx)) {
speed_dir = -1.0f;
}
}
// set guided mode targets
if (!pos_ignore) {
// consume position target
if (!rover.mode_guided.set_desired_location(target_loc)) {
// GCS will need to monitor desired location to
// see if they are having an effect.
}
} else if (!vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (!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, speed_dir * target_speed);
} else if (!vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume velocity and heading
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (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 (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;
case MAV_FRAME_LOCAL_OFFSET_NED:
// add offset to current location
target_loc.offset(packet.x, packet.y);
break;
case MAV_FRAME_LOCAL_NED:
default:
// MAV_FRAME_LOCAL_NED is interpreted as an offset from EKF origin
target_loc = ekf_origin;
target_loc.offset(packet.x, packet.y);
break;
}
}
float target_speed = 0.0f;
float target_yaw_cd = 0.0f;
// consume velocity and convert to target speed and heading
if (!vel_ignore) {
const float speed_max = rover.control_mode->get_speed_default();
// convert vector length into a speed
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
// convert vector direction to target yaw
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);
}
}
// consume yaw heading
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);
}
}
// consume yaw rate
float target_turn_rate_cds = 0.0f;
if (!yaw_rate_ignore) {
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// handling case when both velocity and either yaw or yaw-rate are provided
// by default, we consider that the rover will drive forward
float speed_dir = 1.0f;
if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
// Note: we are using the x-axis velocity to determine direction even though
// the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED
if (is_negative(packet.vx)) {
speed_dir = -1.0f;
}
}
// set guided mode targets
if (!pos_ignore) {
// consume position target
if (!rover.mode_guided.set_desired_location(target_loc)) {
// GCS will need to monitor desired location to
// see if they are having an effect.
}
} else if (!vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (!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, speed_dir * target_speed);
} else if (!vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume velocity and heading
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (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 (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);
}
}
void GCS_MAVLINK_Rover::handle_set_position_target_global_int(const mavlink_message_t &msg) void GCS_MAVLINK_Rover::handle_set_position_target_global_int(const mavlink_message_t &msg)
{ {
// decode packet // decode packet
mavlink_set_position_target_global_int_t packet; mavlink_set_position_target_global_int_t packet;
mavlink_msg_set_position_target_global_int_decode(&msg, &packet); mavlink_msg_set_position_target_global_int_decode(&msg, &packet);
// 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; return;
} }
// check for supported coordinate frames // check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_GLOBAL && if (packet.coordinate_frame != MAV_FRAME_GLOBAL &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT && packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
return; return;
} }
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_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; bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
// prepare 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
if (!check_latlng(packet.lat_int, packet.lon_int)) { if (!check_latlng(packet.lat_int, packet.lon_int)) {
// result = MAV_RESULT_FAILED; // result = MAV_RESULT_FAILED;
return; return;
}
target_loc.lat = packet.lat_int;
target_loc.lng = packet.lon_int;
}
float target_speed = 0.0f;
float target_yaw_cd = 0.0f;
// consume velocity and convert to target speed and heading
if (!vel_ignore) {
const float speed_max = rover.control_mode->get_speed_default();
// convert vector length into a speed
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
// convert vector direction to target yaw
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);
}
}
// consume yaw heading
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);
}
}
// consume yaw rate
float target_turn_rate_cds = 0.0f;
if (!yaw_rate_ignore) {
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// handling case when both velocity and either yaw or yaw-rate are provided
// by default, we consider that the rover will drive forward
float speed_dir = 1.0f;
if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
// Note: we are using the x-axis velocity to determine direction even though
// the frame is provided in MAV_FRAME_GLOBAL_xxx
if (is_negative(packet.vx)) {
speed_dir = -1.0f;
}
}
// set guided mode targets
if (!pos_ignore) {
// consume position target
if (!rover.mode_guided.set_desired_location(target_loc)) {
// GCS will just need to look at desired location
// outputs to see if it having an effect.
}
} else if (!vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (!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, speed_dir * target_speed);
} else if (!vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (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 (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);
}
} }
target_loc.lat = packet.lat_int;
target_loc.lng = packet.lon_int;
}
float target_speed = 0.0f;
float target_yaw_cd = 0.0f;
// consume velocity and convert to target speed and heading
if (!vel_ignore) {
const float speed_max = rover.control_mode->get_speed_default();
// convert vector length into a speed
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
// convert vector direction to target yaw
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);
}
}
// consume yaw heading
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);
}
}
// consume yaw rate
float target_turn_rate_cds = 0.0f;
if (!yaw_rate_ignore) {
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// handling case when both velocity and either yaw or yaw-rate are provided
// by default, we consider that the rover will drive forward
float speed_dir = 1.0f;
if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
// Note: we are using the x-axis velocity to determine direction even though
// the frame is provided in MAV_FRAME_GLOBAL_xxx
if (is_negative(packet.vx)) {
speed_dir = -1.0f;
}
}
// set guided mode targets
if (!pos_ignore) {
// consume position target
if (!rover.mode_guided.set_desired_location(target_loc)) {
// GCS will just need to look at desired location
// outputs to see if it having an effect.
}
} else if (!vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (!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, speed_dir * target_speed);
} else if (!vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (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 (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);
}
}
#if HIL_MODE != HIL_MODE_DISABLED #if HIL_MODE != HIL_MODE_DISABLED
void GCS_MAVLINK_Rover::handle_hil_state(const mavlink_message_t &msg) void GCS_MAVLINK_Rover::handle_hil_state(const mavlink_message_t &msg)
{ {
mavlink_hil_state_t packet; mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(&msg, &packet); mavlink_msg_hil_state_decode(&msg, &packet);
// sanity check location // sanity check location
if (!check_latlng(packet.lat, packet.lon)) { if (!check_latlng(packet.lat, packet.lon)) {
return; return;
} }
// set gps hil sensor // set gps hil sensor
Location loc; Location loc;
loc.lat = packet.lat; loc.lat = packet.lat;
loc.lng = packet.lon; loc.lng = packet.lon;
loc.alt = packet.alt/10; loc.alt = packet.alt/10;
Vector3f vel(packet.vx, packet.vy, packet.vz); Vector3f vel(packet.vx, packet.vy, packet.vz);
vel *= 0.01f; vel *= 0.01f;
gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
packet.time_usec/1000, packet.time_usec/1000,
loc, vel, 10, 0); loc, vel, 10, 0);
// rad/sec // rad/sec
Vector3f gyros; Vector3f gyros;
gyros.x = packet.rollspeed; gyros.x = packet.rollspeed;
gyros.y = packet.pitchspeed; gyros.y = packet.pitchspeed;
gyros.z = packet.yawspeed; gyros.z = packet.yawspeed;
// m/s/s // m/s/s
Vector3f accels; Vector3f accels;
accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0f); accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0f); accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
ins.set_gyro(0, gyros); ins.set_gyro(0, gyros);
ins.set_accel(0, accels); ins.set_accel(0, accels);
compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
} }
#endif // HIL_MODE #endif // HIL_MODE
void GCS_MAVLINK_Rover::handle_radio(const mavlink_message_t &msg) void GCS_MAVLINK_Rover::handle_radio(const mavlink_message_t &msg)
{ {
handle_radio_status(msg, rover.should_log(MASK_LOG_PM)); handle_radio_status(msg, rover.should_log(MASK_LOG_PM));
} }
uint64_t GCS_MAVLINK_Rover::capabilities() const uint64_t GCS_MAVLINK_Rover::capabilities() const