From 0273d92887f3ebd4607ea30290e58fd232efb4ff Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 4 Jan 2021 20:23:55 +1100 Subject: [PATCH] Rover: reindent all of the new GCS_MAVLink handling methods (NFC) --- Rover/GCS_Mavlink.cpp | 606 +++++++++++++++++++++--------------------- 1 file changed, 303 insertions(+), 303 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 9bc9da0d5e..38c078c2c4 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -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) - { - if (msg.sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs - 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; +{ + if (msg.sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs + 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; +} + 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 - if (msg.sysid != rover.g.sysid_my_gcs) { - return; - } - rover.failsafe.last_heartbeat_ms = AP_HAL::millis(); - } +{ + // 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) { + return; + } + rover.failsafe.last_heartbeat_ms = AP_HAL::millis(); +} void GCS_MAVLINK_Rover::handle_set_attitude_target(const mavlink_message_t &msg) - { - // decode packet - mavlink_set_attitude_target_t packet; - mavlink_msg_set_attitude_target_decode(&msg, &packet); +{ + // decode packet + mavlink_set_attitude_target_t packet; + mavlink_msg_set_attitude_target_decode(&msg, &packet); - // exit if vehicle is not in Guided mode - if (!rover.control_mode->in_guided_mode()) { - return; - } + // exit if vehicle is not in Guided mode + if (!rover.control_mode->in_guided_mode()) { + return; + } - // ensure type_mask specifies to use thrust - if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) { - return; - } + // ensure type_mask specifies to use thrust + if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) { + return; + } - // convert thrust to ground speed - packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f); - const float target_speed = rover.control_mode->get_speed_default() * packet.thrust; + // convert thrust to ground speed + packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f); + 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 ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) { - // 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; - rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed); - } else { - // 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); - } - } + // 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) { + // 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; + rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed); + } else { + // 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); + } +} void GCS_MAVLINK_Rover::handle_set_position_target_local_ned(const mavlink_message_t &msg) - { - // decode packet - mavlink_set_position_target_local_ned_t packet; - mavlink_msg_set_position_target_local_ned_decode(&msg, &packet); +{ + // decode packet + mavlink_set_position_target_local_ned_t packet; + mavlink_msg_set_position_target_local_ned_decode(&msg, &packet); - // exit if vehicle is not in Guided mode - if (!rover.control_mode->in_guided_mode()) { - return; - } + // exit if vehicle is not in Guided mode + if (!rover.control_mode->in_guided_mode()) { + return; + } - // need ekf origin - Location ekf_origin; - if (!rover.ahrs.get_origin(ekf_origin)) { - return; - } + // need ekf origin + Location ekf_origin; + if (!rover.ahrs.get_origin(ekf_origin)) { + return; + } - // check for supported coordinate frames - if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && - packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && - packet.coordinate_frame != MAV_FRAME_BODY_NED && - packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { - return; - } + // check for supported coordinate frames + if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && + packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && + packet.coordinate_frame != MAV_FRAME_BODY_NED && + packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { + return; + } - 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 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; + 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 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 target position - Location target_loc = rover.current_loc; - if (!pos_ignore) { - switch (packet.coordinate_frame) { - case MAV_FRAME_BODY_NED: - case MAV_FRAME_BODY_OFFSET_NED: { - // 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_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw(); - // add offset to current location - 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); - } + // prepare target position + Location target_loc = rover.current_loc; + if (!pos_ignore) { + switch (packet.coordinate_frame) { + case MAV_FRAME_BODY_NED: + case MAV_FRAME_BODY_OFFSET_NED: { + // 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_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw(); + // add offset to current location + 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); + } +} void GCS_MAVLINK_Rover::handle_set_position_target_global_int(const mavlink_message_t &msg) - { - // decode packet - mavlink_set_position_target_global_int_t packet; - mavlink_msg_set_position_target_global_int_decode(&msg, &packet); +{ + // decode packet + mavlink_set_position_target_global_int_t packet; + mavlink_msg_set_position_target_global_int_decode(&msg, &packet); - // exit if vehicle is not in Guided mode - if (!rover.control_mode->in_guided_mode()) { - return; - } - // check for supported coordinate frames - if (packet.coordinate_frame != MAV_FRAME_GLOBAL && - packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && - 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_INT) { - return; - } - 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 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; + // exit if vehicle is not in Guided mode + if (!rover.control_mode->in_guided_mode()) { + return; + } + // check for supported coordinate frames + if (packet.coordinate_frame != MAV_FRAME_GLOBAL && + packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && + packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && + 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_INT) { + return; + } + 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 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 target position - Location target_loc = rover.current_loc; - if (!pos_ignore) { - // sanity check location - if (!check_latlng(packet.lat_int, packet.lon_int)) { - // result = MAV_RESULT_FAILED; - 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); - } + // prepare target position + Location target_loc = rover.current_loc; + if (!pos_ignore) { + // sanity check location + if (!check_latlng(packet.lat_int, packet.lon_int)) { + // result = MAV_RESULT_FAILED; + 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); + } +} #if HIL_MODE != HIL_MODE_DISABLED void GCS_MAVLINK_Rover::handle_hil_state(const mavlink_message_t &msg) - { - mavlink_hil_state_t packet; - mavlink_msg_hil_state_decode(&msg, &packet); +{ + mavlink_hil_state_t packet; + mavlink_msg_hil_state_decode(&msg, &packet); - // sanity check location - if (!check_latlng(packet.lat, packet.lon)) { - return; - } + // sanity check location + if (!check_latlng(packet.lat, packet.lon)) { + return; + } - // set gps hil sensor - Location loc; - loc.lat = packet.lat; - loc.lng = packet.lon; - loc.alt = packet.alt/10; - Vector3f vel(packet.vx, packet.vy, packet.vz); - vel *= 0.01f; + // set gps hil sensor + Location loc; + loc.lat = packet.lat; + loc.lng = packet.lon; + loc.alt = packet.alt/10; + Vector3f vel(packet.vx, packet.vy, packet.vz); + vel *= 0.01f; - gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, - packet.time_usec/1000, - loc, vel, 10, 0); + gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, + packet.time_usec/1000, + loc, vel, 10, 0); - // rad/sec - Vector3f gyros; - gyros.x = packet.rollspeed; - gyros.y = packet.pitchspeed; - gyros.z = packet.yawspeed; + // rad/sec + Vector3f gyros; + gyros.x = packet.rollspeed; + gyros.y = packet.pitchspeed; + gyros.z = packet.yawspeed; - // m/s/s - Vector3f accels; - accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); - accels.y = packet.yacc * (GRAVITY_MSS/1000.0f); - accels.z = packet.zacc * (GRAVITY_MSS/1000.0f); + // m/s/s + Vector3f accels; + accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); + accels.y = packet.yacc * (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); - compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); - compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); - } + ins.set_accel(0, accels); + compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); + compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); +} #endif // HIL_MODE 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