void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } uint32_t tnow = AP_HAL::millis(); mavlink_rc_channels_override_t packet; mavlink_msg_rc_channels_override_decode(msg, &packet); RC_Channels::set_override(0, packet.chan1_raw, tnow); RC_Channels::set_override(1, packet.chan2_raw, tnow); RC_Channels::set_override(2, packet.chan3_raw, tnow); RC_Channels::set_override(3, packet.chan4_raw, tnow); RC_Channels::set_override(4, packet.chan5_raw, tnow); RC_Channels::set_override(5, packet.chan6_raw, tnow); RC_Channels::set_override(6, packet.chan7_raw, tnow); RC_Channels::set_override(7, packet.chan8_raw, tnow); break; } case MAVLINK_MSG_ID_MANUAL_CONTROL: { if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); if (packet.target != rover.g.sysid_this_mav) { break; // only accept control aimed at us } uint32_t tnow = AP_HAL::millis(); const int16_t roll = (packet.y == INT16_MAX) ? 0 : rover.channel_steer->get_radio_min() + (rover.channel_steer->get_radio_max() - rover.channel_steer->get_radio_min()) * (packet.y + 1000) / 2000.0f; const int16_t throttle = (packet.z == INT16_MAX) ? 0 : rover.channel_throttle->get_radio_min() + (rover.channel_throttle->get_radio_max() - rover.channel_throttle->get_radio_min()) * (packet.z + 1000) / 2000.0f; RC_Channels::set_override(uint8_t(rover.rcmap.roll() - 1), roll, tnow); RC_Channels::set_override(uint8_t(rover.rcmap.throttle() - 1), throttle, tnow); break; } case MAVLINK_MSG_ID_HEARTBEAT: { // 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) { break; } rover.last_heartbeat_ms = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false); break; } case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82 { // 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 != &rover.mode_guided) { break; } // ensure type_mask specifies to use thrust if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) { break; } // 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); } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84 { // 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 != &rover.mode_guided) { break; } // 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) { break; } 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 location_offset(target_loc, ne_x, ne_y); } break; case MAV_FRAME_LOCAL_OFFSET_NED: // add offset to current location location_offset(target_loc, packet.x, packet.y); break; default: // MAV_FRAME_LOCAL_NED interpret as an offset from home target_loc = rover.ahrs.get_home(); location_offset(target_loc, 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 rover.mode_guided.set_desired_location(target_loc); } 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, speed_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, speed_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, speed_dir * 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; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86 { // 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 != &rover.mode_guided) { break; } // 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) { break; } 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; break; } 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 rover.mode_guided.set_desired_location(target_loc); } 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, speed_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, speed_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, speed_dir * 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; } #if HIL_MODE != HIL_MODE_DISABLED case MAVLINK_MSG_ID_HIL_STATE: { mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); // sanity check location if (!check_latlng(packet.lat, packet.lon)) { break; } // 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); // 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); 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); break; } #endif // HIL_MODE #if MOUNT == ENABLED // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { rover.camera_mount.configure_msg(msg); break; } // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: { rover.camera_mount.control_msg(msg); break; } #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: { handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM)); break; } // send or receive fence points with GCS case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160 case MAVLINK_MSG_ID_FENCE_FETCH_POINT: rover.g2.fence.handle_msg(*this, msg); break; case MAVLINK_MSG_ID_DISTANCE_SENSOR: rover.rangefinder.handle_msg(msg); rover.g2.proximity.handle_msg(msg); break; default: handle_common_message(msg); break; } // end switch } // end handle mavlink
void mavlink_common_message_handler(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: break; /* When requesting data streams say we can't send them */ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { mavlink_request_data_stream_t cmd; mavlink_msg_request_data_stream_decode(msg, &cmd); mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0); MAVLinkSendMessage(); break; } /* Override channels with RC */ case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { mavlink_rc_channels_override_t cmd; mavlink_msg_rc_channels_override_decode(msg, &cmd); #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100; int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2; int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2; int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100; parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw); //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n", // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw); #endif break; } /* When a request is made of the parameters list */ case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_params_idx = 0; break; } /* When a request os made for a specific parameter */ case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t cmd; mavlink_msg_param_request_read_decode(msg, &cmd); // First check param_index and search for the ID if needed if (cmd.param_index == -1) { cmd.param_index = settings_idx_from_param_id(cmd.param_id); } // Send message only if the param_index was found (Coverity Scan) if (cmd.param_index > -1) { mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[cmd.param_index], settings_get_value(cmd.param_index), MAV_PARAM_TYPE_REAL32, NB_SETTING, cmd.param_index); MAVLinkSendMessage(); } break; } case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == AC_ID) { int16_t idx = settings_idx_from_param_id(set.param_id); // setting found if (idx >= 0) { // Only write if new value is NOT "not-a-number" // AND is NOT infinity if (set.param_type == MAV_PARAM_TYPE_REAL32 && !isnan(set.param_value) && !isinf(set.param_value)) { DlSetting(idx, set.param_value); // Report back new value mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[idx], settings_get_value(idx), MAV_PARAM_TYPE_REAL32, NB_SETTING, idx); MAVLinkSendMessage(); } } } } break; #ifndef AP /* only for rotorcraft */ case MAVLINK_MSG_ID_COMMAND_LONG: { mavlink_command_long_t cmd; mavlink_msg_command_long_decode(msg, &cmd); // Check if this message is for this system if ((uint8_t) cmd.target_system == AC_ID) { uint8_t result = MAV_RESULT_UNSUPPORTED; switch (cmd.command) { case MAV_CMD_NAV_GUIDED_ENABLE: MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1); result = MAV_RESULT_FAILED; if (cmd.param1 > 0.5) { autopilot_set_mode(AP_MODE_GUIDED); if (autopilot_mode == AP_MODE_GUIDED) { result = MAV_RESULT_ACCEPTED; } } else { // turn guided mode off - to what? maybe NAV? or MODE_AUTO2? } break; case MAV_CMD_COMPONENT_ARM_DISARM: /* supposed to use this command to arm or SET_MODE?? */ MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1); result = MAV_RESULT_FAILED; if (cmd.param1 > 0.5) { autopilot_set_motors_on(TRUE); if (autopilot_motors_on) result = MAV_RESULT_ACCEPTED; } else { autopilot_set_motors_on(FALSE); if (!autopilot_motors_on) result = MAV_RESULT_ACCEPTED; } break; default: break; } // confirm command with result mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result); MAVLinkSendMessage(); } break; } case MAVLINK_MSG_ID_SET_MODE: { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(msg, &mode); if (mode.target_system == AC_ID) { MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode); if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { autopilot_set_motors_on(TRUE); } else { autopilot_set_motors_on(FALSE); } if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { autopilot_set_mode(AP_MODE_GUIDED); } else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { autopilot_set_mode(AP_MODE_NAV); } } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: { mavlink_set_position_target_local_ned_t target; mavlink_msg_set_position_target_local_ned_decode(msg, &target); // Check if this message is for this system if (target.target_system == AC_ID) { MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask); /* if position and yaw bits are not set to ignored, use only position for now */ if (!(target.type_mask & 0b1110000000100000)) { switch (target.coordinate_frame) { case MAV_FRAME_LOCAL_NED: MAVLINK_DEBUG("set position target, frame LOCAL_NED\n"); autopilot_guided_goto_ned(target.x, target.y, target.z, target.yaw); break; case MAV_FRAME_LOCAL_OFFSET_NED: MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n"); autopilot_guided_goto_ned_relative(target.x, target.y, target.z, target.yaw); break; case MAV_FRAME_BODY_OFFSET_NED: MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n"); autopilot_guided_goto_body_relative(target.x, target.y, target.z, target.yaw); break; default: break; } } else if (!(target.type_mask & 0b0001110000100000)) { /* position is set to ignore, but velocity not */ switch (target.coordinate_frame) { case MAV_FRAME_LOCAL_NED: MAVLINK_DEBUG("set velocity target, frame LOCAL_NED\n"); autopilot_guided_move_ned(target.vx, target.vy, target.vz, target.yaw); break; default: break; } } } break; } #endif default: //Do nothing MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid); break; } }
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); break; } case MAVLINK_MSG_ID_COMMAND_LONG: { // decode mavlink_command_long_t packet; mavlink_msg_command_long_decode(msg, &packet); uint8_t result = MAV_RESULT_UNSUPPORTED; // do command switch(packet.command) { case MAV_CMD_START_RX_PAIR: // initiate bind procedure if (!hal.rcin->rc_bind(packet.param1)) { result = MAV_RESULT_FAILED; } else { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: rover.set_mode(RTL); result = MAV_RESULT_ACCEPTED; break; #if MOUNT == ENABLED // Sets the region of interest (ROI) for the camera case MAV_CMD_DO_SET_ROI: // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location roi_loc; roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); roi_loc.alt = (int32_t)(packet.param7 * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; #endif #if CAMERA == ENABLED case MAV_CMD_DO_DIGICAM_CONFIGURE: rover.camera.configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6, packet.param7); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_DO_DIGICAM_CONTROL: if (rover.camera.control(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6)) { rover.log_picture(); } result = MAV_RESULT_ACCEPTED; break; #endif // CAMERA == ENABLED case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED rover.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); result = MAV_RESULT_ACCEPTED; #endif break; case MAV_CMD_MISSION_START: rover.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_CALIBRATION: if(hal.util->get_soft_armed()) { result = MAV_RESULT_FAILED; break; } if (is_equal(packet.param1,1.0f)) { rover.ins.init_gyro(); if (rover.ins.gyro_calibrated_ok_all()) { rover.ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_equal(packet.param3,1.0f)) { rover.init_barometer(false); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param4,1.0f)) { rover.trim_radio(); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param5,1.0f)) { result = MAV_RESULT_ACCEPTED; // start with gyro calibration rover.ins.init_gyro(); // reset ahrs gyro bias if (rover.ins.gyro_calibrated_ok_all()) { rover.ahrs.reset_gyro_drift(); } else { result = MAV_RESULT_FAILED; } rover.ins.acal_init(); rover.ins.get_acal()->start(this); } else if (is_equal(packet.param5,2.0f)) { // start with gyro calibration rover.ins.init_gyro(); // accel trim float trim_roll, trim_pitch; if(rover.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration"); } break; case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: { uint8_t compassNumber = -1; if (is_equal(packet.param1, 2.0f)) { compassNumber = 0; } else if (is_equal(packet.param1, 5.0f)) { compassNumber = 1; } else if (is_equal(packet.param1, 6.0f)) { compassNumber = 2; } if (compassNumber != (uint8_t) -1) { rover.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_SET_MODE: switch ((uint16_t)packet.param1) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: rover.set_mode(MANUAL); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: rover.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_STABILIZE_DISARMED: case MAV_MODE_STABILIZE_ARMED: rover.set_mode(LEARNING); result = MAV_RESULT_ACCEPTED; break; default: result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_SERVO: if (rover.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_SERVO: if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_SET_RELAY: if (rover.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_RELAY: if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1,3.0f)); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1,1.0f)) { // run pre_arm_checks and arm_checks and display failures if (rover.arm_motors(AP_Arming::MAVLINK)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_zero(packet.param1)) { if (rover.disarm_motors()) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_GET_HOME_POSITION: if (rover.home_is_set != HOME_UNSET) { send_home(rover.ahrs.get_home()); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1,1.0f)) { send_autopilot_version(FIRMWARE_VERSION); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_SET_HOME: { // param1 : use current (1=use current location, 0=use specified location) // param5 : latitude // param6 : longitude // param7 : altitude result = MAV_RESULT_FAILED; // assume failure if (is_equal(packet.param1,1.0f)) { rover.init_home(); } else { if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) { // don't allow the 0,0 position break; } // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location new_home_loc {}; new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); rover.ahrs.set_home(new_home_loc); rover.home_is_set = HOME_SET_NOT_LOCKED; rover.Log_Write_Home_And_Origin(); GCS_MAVLINK::send_home_all(new_home_loc); result = MAV_RESULT_ACCEPTED; rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", (double)(new_home_loc.lat*1.0e-7f), (double)(new_home_loc.lng*1.0e-7f), (uint32_t)(new_home_loc.alt*0.01f)); } break; } case MAV_CMD_DO_START_MAG_CAL: case MAV_CMD_DO_ACCEPT_MAG_CAL: case MAV_CMD_DO_CANCEL_MAG_CAL: result = rover.compass.handle_mag_cal_command(packet); break; default: break; } mavlink_msg_command_ack_send_buf( msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_SET_MODE: { handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t)); break; } case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { handle_mission_request_list(rover.mission, msg); break; } // XXX read a WP from EEPROM and send it to the GCS case MAVLINK_MSG_ID_MISSION_REQUEST_INT: case MAVLINK_MSG_ID_MISSION_REQUEST: { handle_mission_request(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_ACK: { // not used break; } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // mark the firmware version in the tlog send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING); #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); #endif handle_param_request_list(msg); break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { handle_param_request_read(msg); break; } case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { handle_mission_clear_all(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { handle_mission_set_current(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_COUNT: { handle_mission_count(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { handle_mission_write_partial_list(rover.mission, msg); break; } // GCS has sent us a mission item, store to EEPROM case MAVLINK_MSG_ID_MISSION_ITEM: { if (handle_mission_item(msg, rover.mission)) { rover.DataFlash.Log_Write_EntireMission(rover.mission); } break; } case MAVLINK_MSG_ID_MISSION_ITEM_INT: { if (handle_mission_item(msg, rover.mission)) { rover.DataFlash.Log_Write_EntireMission(rover.mission); } break; } case MAVLINK_MSG_ID_PARAM_SET: { handle_param_set(msg, &rover.DataFlash); break; } case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. if(msg->sysid != rover.g.sysid_my_gcs) break; // Only accept control from our gcs mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); v[0] = packet.chan1_raw; v[1] = packet.chan2_raw; v[2] = packet.chan3_raw; v[3] = packet.chan4_raw; v[4] = packet.chan5_raw; v[5] = packet.chan6_raw; v[6] = packet.chan7_raw; v[7] = packet.chan8_raw; hal.rcin->set_overrides(v, 8); rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_RC, false); break; } case MAVLINK_MSG_ID_HEARTBEAT: { // 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) break; rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false); break; } case MAVLINK_MSG_ID_GPS_INPUT: { rover.gps.handle_msg(msg); break; } #if HIL_MODE != HIL_MODE_DISABLED case MAVLINK_MSG_ID_HIL_STATE: { mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); // sanity check location if (!check_latlng(packet.lat, packet.lon)) { break; } // 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); // 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); 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); break; } #endif // HIL_MODE #if CAMERA == ENABLED //deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: { break; } //deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONTROL: { rover.camera.control_msg(msg); rover.log_picture(); break; } #endif // CAMERA == ENABLED #if MOUNT == ENABLED //deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { rover.camera_mount.configure_msg(msg); break; } //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: { rover.camera_mount.control_msg(msg); break; } #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: { handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM)); break; } case MAVLINK_MSG_ID_LOG_REQUEST_DATA: case MAVLINK_MSG_ID_LOG_ERASE: rover.in_log_download = true; /* no break */ case MAVLINK_MSG_ID_LOG_REQUEST_LIST: if (!rover.in_mavlink_delay) { handle_log_message(msg, rover.DataFlash); } break; case MAVLINK_MSG_ID_LOG_REQUEST_END: rover.in_log_download = false; if (!rover.in_mavlink_delay) { handle_log_message(msg, rover.DataFlash); } break; case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg, rover.gps); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_gps_inject(msg, rover.gps); break; case MAVLINK_MSG_ID_DISTANCE_SENSOR: rover.sonar.handle_msg(msg); break; case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: rover.DataFlash.remote_log_block_status_msg(chan, msg); break; case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: send_autopilot_version(FIRMWARE_VERSION); break; case MAVLINK_MSG_ID_SETUP_SIGNING: handle_setup_signing(msg); break; case MAVLINK_MSG_ID_LED_CONTROL: // send message to Notify AP_Notify::handle_led_control(msg); break; case MAVLINK_MSG_ID_PLAY_TUNE: // send message to Notify AP_Notify::handle_play_tune(msg); break; } // end switch } // end handle mavlink
void mavlink_common_message_handler(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: break; /* When requesting data streams say we can't send them */ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { mavlink_request_data_stream_t cmd; mavlink_msg_request_data_stream_decode(msg, &cmd); mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0); MAVLinkSendMessage(); break; } /* Override channels with RC */ case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { mavlink_rc_channels_override_t cmd; mavlink_msg_rc_channels_override_decode(msg, &cmd); #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100; int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2; int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2; int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100; parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw); //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n", // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw); #endif break; } /* When a request is made of the parameters list */ case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_params_idx = 0; break; } /* When a request os made for a specific parameter */ case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t cmd; mavlink_msg_param_request_read_decode(msg, &cmd); // First check param_index and search for the ID if needed if (cmd.param_index == -1) { cmd.param_index = settings_idx_from_param_id(cmd.param_id); } mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[cmd.param_index], settings_get_value(cmd.param_index), MAV_PARAM_TYPE_REAL32, NB_SETTING, cmd.param_index); MAVLinkSendMessage(); break; } case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == AC_ID) { int16_t idx = settings_idx_from_param_id(set.param_id); // setting found if (idx >= 0) { // Only write if new value is NOT "not-a-number" // AND is NOT infinity if (set.param_type == MAV_PARAM_TYPE_REAL32 && !isnan(set.param_value) && !isinf(set.param_value)) { DlSetting(idx, set.param_value); // Report back new value mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlink_param_names[idx], settings_get_value(idx), MAV_PARAM_TYPE_REAL32, NB_SETTING, idx); MAVLinkSendMessage(); } } } } break; default: //Do nothing //MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid); break; } }
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); break; } case MAVLINK_MSG_ID_COMMAND_INT: { // decode packet mavlink_command_int_t packet; mavlink_msg_command_int_decode(msg, &packet); MAV_RESULT result = MAV_RESULT_UNSUPPORTED; switch (packet.command) { case MAV_CMD_DO_SET_HOME: { // assume failure result = MAV_RESULT_FAILED; if (is_equal(packet.param1, 1.0f)) { // if param1 is 1, use current location if (rover.set_home_to_current_location(true)) { result = MAV_RESULT_ACCEPTED; } break; } // ensure param1 is zero if (!is_zero(packet.param1)) { break; } // check frame type is supported if (packet.frame != MAV_FRAME_GLOBAL && packet.frame != MAV_FRAME_GLOBAL_INT && packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { break; } // sanity check location if (!check_latlng(packet.x, packet.y)) { break; } Location new_home_loc {}; new_home_loc.lat = packet.x; new_home_loc.lng = packet.y; new_home_loc.alt = packet.z * 100; // handle relative altitude if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { if (!rover.ahrs.home_is_set()) { // cannot use relative altitude if home is not set break; } new_home_loc.alt += rover.ahrs.get_home().alt; } if (rover.set_home(new_home_loc, true)) { result = MAV_RESULT_ACCEPTED; } break; } #if MOUNT == ENABLED case MAV_CMD_DO_SET_ROI: { // param1 : /* Region of interest mode (not used)*/ // param2 : /* MISSION index/ target ID (not used)*/ // param3 : /* ROI index (not used)*/ // param4 : /* empty */ // x : lat // y : lon // z : alt // sanity check location if (!check_latlng(packet.x, packet.y)) { break; } Location roi_loc; roi_loc.lat = packet.x; roi_loc.lng = packet.y; roi_loc.alt = (int32_t)(packet.z * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; } #endif default: result = MAV_RESULT_UNSUPPORTED; break; } // send ACK or NAK mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_COMMAND_LONG: { // decode mavlink_command_long_t packet; mavlink_msg_command_long_decode(msg, &packet); MAV_RESULT result = MAV_RESULT_UNSUPPORTED; // do command switch (packet.command) { case MAV_CMD_NAV_RETURN_TO_LAUNCH: rover.set_mode(rover.mode_rtl, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; #if MOUNT == ENABLED // Sets the region of interest (ROI) for the camera case MAV_CMD_DO_SET_ROI: // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location roi_loc; roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); roi_loc.alt = (int32_t)(packet.param7 * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; #endif case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED rover.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); result = MAV_RESULT_ACCEPTED; #endif break; case MAV_CMD_MISSION_START: rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_equal(packet.param1, 1.0f) || is_equal(packet.param1, 3.0f)) { // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1, 3.0f)); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1, 1.0f)) { // run pre_arm_checks and arm_checks and display failures if (rover.arm_motors(AP_Arming::MAVLINK)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_zero(packet.param1)) { if (rover.disarm_motors()) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_FENCE_ENABLE: result = MAV_RESULT_ACCEPTED; switch ((uint16_t)packet.param1) { case 0: rover.g2.fence.enable(false); break; case 1: rover.g2.fence.enable(true); break; default: result = MAV_RESULT_FAILED; break; } break; case MAV_CMD_DO_SET_HOME: { // param1 : use current (1=use current location, 0=use specified location) // param5 : latitude // param6 : longitude // param7 : altitude result = MAV_RESULT_FAILED; // assume failure if (is_equal(packet.param1, 1.0f)) { if (rover.set_home_to_current_location(true)) { result = MAV_RESULT_ACCEPTED; } } else { // ensure param1 is zero if (!is_zero(packet.param1)) { break; } Location new_home_loc {}; new_home_loc.lat = static_cast<int32_t>(packet.param5 * 1.0e7f); new_home_loc.lng = static_cast<int32_t>(packet.param6 * 1.0e7f); new_home_loc.alt = static_cast<int32_t>(packet.param7 * 100.0f); if (rover.set_home(new_home_loc, true)) { result = MAV_RESULT_ACCEPTED; } } break; } case MAV_CMD_NAV_SET_YAW_SPEED: { // param1 : yaw angle to adjust direction by in centidegress // param2 : Speed - normalized to 0 .. 1 // exit if vehicle is not in Guided mode if (rover.control_mode != &rover.mode_guided) { break; } // send yaw change and target speed to guided mode controller const float speed_max = rover.control_mode->get_speed_default(); const float target_speed = constrain_float(packet.param2 * speed_max, -speed_max, speed_max); rover.mode_guided.set_desired_heading_delta_and_speed(packet.param1, target_speed); result = MAV_RESULT_ACCEPTED; break; } case MAV_CMD_ACCELCAL_VEHICLE_POS: result = MAV_RESULT_FAILED; if (rover.ins.get_acal()->gcs_vehicle_position(packet.param1)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_MOTOR_TEST: // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) // param3 : throttle (range depends upon param2) // param4 : timeout (in seconds) result = rover.mavlink_motor_test_start(chan, static_cast<uint8_t>(packet.param1), static_cast<uint8_t>(packet.param2), static_cast<int16_t>(packet.param3), packet.param4); break; default: result = handle_command_long_message(packet); break; } mavlink_msg_command_ack_send_buf( msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } mavlink_rc_channels_override_t packet; mavlink_msg_rc_channels_override_decode(msg, &packet); RC_Channels::set_override(0, packet.chan1_raw); RC_Channels::set_override(1, packet.chan2_raw); RC_Channels::set_override(2, packet.chan3_raw); RC_Channels::set_override(3, packet.chan4_raw); RC_Channels::set_override(4, packet.chan5_raw); RC_Channels::set_override(5, packet.chan6_raw); RC_Channels::set_override(6, packet.chan7_raw); RC_Channels::set_override(7, packet.chan8_raw); rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_RC, false); break; } case MAVLINK_MSG_ID_MANUAL_CONTROL: { if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); if (packet.target != rover.g.sysid_this_mav) { break; // only accept control aimed at us } const int16_t roll = (packet.y == INT16_MAX) ? 0 : rover.channel_steer->get_radio_min() + (rover.channel_steer->get_radio_max() - rover.channel_steer->get_radio_min()) * (packet.y + 1000) / 2000.0f; const int16_t throttle = (packet.z == INT16_MAX) ? 0 : rover.channel_throttle->get_radio_min() + (rover.channel_throttle->get_radio_max() - rover.channel_throttle->get_radio_min()) * (packet.z + 1000) / 2000.0f; RC_Channels::set_override(uint8_t(rover.rcmap.roll() - 1), roll); RC_Channels::set_override(uint8_t(rover.rcmap.throttle() - 1), throttle); rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_RC, false); break; } case MAVLINK_MSG_ID_HEARTBEAT: { // 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) { break; } rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false); break; } case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82 { // 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 != &rover.mode_guided) { break; } // ensure type_mask specifies to use thrust if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) { break; } // 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); } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84 { // 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 != &rover.mode_guided) { break; } // 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) { break; } 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 location_offset(target_loc, ne_x, ne_y); } break; case MAV_FRAME_LOCAL_OFFSET_NED: // add offset to current location location_offset(target_loc, packet.x, packet.y); break; default: // MAV_FRAME_LOCAL_NED interpret as an offset from home target_loc = rover.ahrs.get_home(); location_offset(target_loc, 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 && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { // consume position target rover.mode_guided.set_desired_location(target_loc); } 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, speed_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, speed_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, speed_dir * 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; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86 { // 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 != &rover.mode_guided) { break; } // 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) { break; } 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; break; } 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 && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { // consume position target rover.mode_guided.set_desired_location(target_loc); } 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, speed_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, speed_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, speed_dir * 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; } #if HIL_MODE != HIL_MODE_DISABLED case MAVLINK_MSG_ID_HIL_STATE: { mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); // sanity check location if (!check_latlng(packet.lat, packet.lon)) { break; } // 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); // 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); 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); break; } #endif // HIL_MODE #if MOUNT == ENABLED // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { rover.camera_mount.configure_msg(msg); break; } // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: { rover.camera_mount.control_msg(msg); break; } #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: { handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM)); break; } // send or receive fence points with GCS case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160 case MAVLINK_MSG_ID_FENCE_FETCH_POINT: rover.g2.fence.handle_msg(*this, msg); break; case MAVLINK_MSG_ID_DISTANCE_SENSOR: rover.rangefinder.handle_msg(msg); rover.g2.proximity.handle_msg(msg); break; default: handle_common_message(msg); break; } // end switch } // end handle mavlink
void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { // MAV ID: 0 // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if (msg->sysid != sub.g.sysid_my_gcs) { break; } sub.failsafe.last_heartbeat_ms = AP_HAL::millis(); break; } case MAVLINK_MSG_ID_MANUAL_CONTROL: { // MAV ID: 69 if (msg->sysid != sub.g.sysid_my_gcs) { break; // Only accept control from our gcs } mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); if (packet.target != sub.g.sysid_this_mav) { break; // only accept control aimed at us } sub.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons); sub.failsafe.last_pilot_input_ms = AP_HAL::millis(); // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes sub.failsafe.last_heartbeat_ms = AP_HAL::millis(); break; } case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // MAV ID: 70 // allow override of RC input if (msg->sysid != sub.g.sysid_my_gcs) { break; // Only accept control from our gcs } uint32_t tnow = AP_HAL::millis(); mavlink_rc_channels_override_t packet; mavlink_msg_rc_channels_override_decode(msg, &packet); RC_Channels::set_override(0, packet.chan1_raw, tnow); RC_Channels::set_override(1, packet.chan2_raw, tnow); RC_Channels::set_override(2, packet.chan3_raw, tnow); RC_Channels::set_override(3, packet.chan4_raw, tnow); RC_Channels::set_override(4, packet.chan5_raw, tnow); RC_Channels::set_override(5, packet.chan6_raw, tnow); RC_Channels::set_override(6, packet.chan7_raw, tnow); RC_Channels::set_override(7, packet.chan8_raw, tnow); sub.failsafe.last_pilot_input_ms = tnow; // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes sub.failsafe.last_heartbeat_ms = tnow; break; } case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: { // MAV ID: 82 // decode packet mavlink_set_attitude_target_t packet; mavlink_msg_set_attitude_target_decode(msg, &packet); // ensure type_mask specifies to use attitude // the thrust can be used from the altitude hold if (packet.type_mask & (1<<6)) { sub.set_attitude_target_no_gps = {AP_HAL::millis(), packet}; } // ensure type_mask specifies to use attitude and thrust if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) { break; } // convert thrust to climb rate packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f); float climb_rate_cms = 0.0f; if (is_equal(packet.thrust, 0.5f)) { climb_rate_cms = 0.0f; } else if (packet.thrust > 0.5f) { // climb at up to WPNAV_SPEED_UP climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_up(); } else { // descend at up to WPNAV_SPEED_DN climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * fabsf(sub.wp_nav.get_default_speed_down()); } sub.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms); break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: { // MAV ID: 84 // 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 or Auto-Guided mode if ((sub.control_mode != GUIDED) && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided)) { break; } // 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) { break; } 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; /* * for future use: * bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE; * 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 position Vector3f pos_vector; if (!pos_ignore) { // convert to cm pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f); // rotate to body-frame if necessary if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { sub.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y); } // add body offset if necessary if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED || packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { pos_vector += sub.inertial_nav.get_position(); } else { // convert from alt-above-home to alt-above-ekf-origin pos_vector.z = sub.pv_alt_above_origin(pos_vector.z); } } // prepare velocity Vector3f vel_vector; if (!vel_ignore) { // convert to cm vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f); // rotate to body-frame if necessary if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { sub.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y); } } // send request if (!pos_ignore && !vel_ignore && acc_ignore) { sub.guided_set_destination_posvel(pos_vector, vel_vector); } else if (pos_ignore && !vel_ignore && acc_ignore) { sub.guided_set_velocity(vel_vector); } else if (!pos_ignore && vel_ignore && acc_ignore) { sub.guided_set_destination(pos_vector); } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: { // MAV ID: 86 // 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 or Auto-Guided mode if ((sub.control_mode != GUIDED) && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided)) { break; } // 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 && // solo shot manager incorrectly sends RELATIVE_ALT instead of 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_INT) { break; } 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; /* * for future use: * bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE; * 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; */ Vector3f pos_neu_cm; // position (North, East, Up coordinates) in centimeters if (!pos_ignore) { // sanity check location if (!check_latlng(packet.lat_int, packet.lon_int)) { break; } Location loc; loc.lat = packet.lat_int; loc.lng = packet.lon_int; loc.alt = packet.alt*100; switch (packet.coordinate_frame) { case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: loc.relative_alt = true; loc.terrain_alt = false; break; case MAV_FRAME_GLOBAL_TERRAIN_ALT: case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: loc.relative_alt = true; loc.terrain_alt = true; break; case MAV_FRAME_GLOBAL: case MAV_FRAME_GLOBAL_INT: default: loc.relative_alt = false; loc.terrain_alt = false; break; } pos_neu_cm = sub.pv_location_to_vector(loc); } if (!pos_ignore && !vel_ignore && acc_ignore) { sub.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (pos_ignore && !vel_ignore && acc_ignore) { sub.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (!pos_ignore && vel_ignore && acc_ignore) { sub.guided_set_destination(pos_neu_cm); } break; } case MAVLINK_MSG_ID_DISTANCE_SENSOR: { sub.rangefinder.handle_msg(msg); break; } #if AC_FENCE == ENABLED // send or receive fence points with GCS case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160 case MAVLINK_MSG_ID_FENCE_FETCH_POINT: sub.fence.handle_msg(*this, msg); break; #endif // AC_FENCE == ENABLED case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: #if AP_TERRAIN_AVAILABLE && AC_TERRAIN sub.terrain.handle_data(chan, msg); #endif break; case MAVLINK_MSG_ID_SET_HOME_POSITION: { mavlink_set_home_position_t packet; mavlink_msg_set_home_position_decode(msg, &packet); if ((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) { sub.set_home_to_current_location(true); } else { // sanity check location if (!check_latlng(packet.latitude, packet.longitude)) { break; } Location new_home_loc; new_home_loc.lat = packet.latitude; new_home_loc.lng = packet.longitude; new_home_loc.alt = packet.altitude / 10; if (sub.far_from_EKF_origin(new_home_loc)) { break; } sub.set_home(new_home_loc, true); } break; } // This adds support for leak detectors in a separate enclosure // connected to a mavlink enabled subsystem case MAVLINK_MSG_ID_SYS_STATUS: { uint32_t MAV_SENSOR_WATER = 0x20000000; mavlink_sys_status_t packet; mavlink_msg_sys_status_decode(msg, &packet); if ((packet.onboard_control_sensors_enabled & MAV_SENSOR_WATER) && !(packet.onboard_control_sensors_health & MAV_SENSOR_WATER)) { sub.leak_detector.set_detect(); } } break; default: handle_common_message(msg); break; } // end switch } // end handle mavlink