/* handle an incoming mission item */ void GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &mission) { mavlink_mission_item_t packet; uint8_t result = MAV_MISSION_ACCEPTED; struct AP_Mission::Mission_Command cmd = {}; mavlink_msg_mission_item_decode(msg, &packet); // convert mavlink packet to mission command if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) { result = MAV_MISSION_INVALID; goto mission_ack; } if (packet.current == 2) { // current = 2 is a flag to tell us this is a "guided mode" // waypoint and not for the mission handle_guided_request(cmd); // verify we received the command result = 0; goto mission_ack; } if (packet.current == 3) { //current = 3 is a flag to tell us this is a alt change only // add home alt if needed handle_change_alt_request(cmd); // verify we recevied the command result = 0; goto mission_ack; } // Check if receiving waypoints (mission upload expected) if (!waypoint_receiving) { result = MAV_MISSION_ERROR; goto mission_ack; } // check if this is the requested waypoint if (packet.seq != waypoint_request_i) { result = MAV_MISSION_INVALID_SEQUENCE; goto mission_ack; } // if command index is within the existing list, replace the command if (packet.seq < mission.num_commands()) { if (mission.replace_cmd(packet.seq,cmd)) { result = MAV_MISSION_ACCEPTED; }else{ result = MAV_MISSION_ERROR; goto mission_ack; } // if command is at the end of command list, add the command } else if (packet.seq == mission.num_commands()) { if (mission.add_cmd(cmd)) { result = MAV_MISSION_ACCEPTED; }else{ result = MAV_MISSION_ERROR; goto mission_ack; } // if beyond the end of the command list, return an error } else { result = MAV_MISSION_ERROR; goto mission_ack; } // update waypoint receiving state machine waypoint_timelast_receive = hal.scheduler->millis(); waypoint_request_i++; if (waypoint_request_i >= waypoint_request_last) { mavlink_msg_mission_ack_send_buf( msg, chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); send_text_P(SEVERITY_LOW,PSTR("flight plan received")); waypoint_receiving = false; // XXX ignores waypoint radius for individual waypoints, can // only set WP_RADIUS parameter } else { waypoint_timelast_request = hal.scheduler->millis(); // if we have enough space, then send the next WP immediately if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_MISSION_ITEM_LEN) { queued_waypoint_send(); } else { send_message(MSG_NEXT_WAYPOINT); } } return; mission_ack: // we are rejecting the mission/waypoint mavlink_msg_mission_ack_send_buf( msg, chan, msg->sysid, msg->compid, result); }
// try to send a message, return false if it won't fit in the serial tx buffer bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) { if (telemetry_delayed(chan)) { return false; } // if we don't have at least 1ms remaining before the main loop // wants to fire then don't send a mavlink message. We want to // prioritise the main flight control loop over communications if (!rover.in_mavlink_delay && rover.scheduler.time_available_usec() < 1200) { rover.gcs_out_of_time = true; return false; } switch (id) { case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); last_heartbeat_time = AP_HAL::millis(); rover.send_heartbeat(chan); return true; case MSG_EXTENDED_STATUS1: CHECK_PAYLOAD_SIZE(SYS_STATUS); rover.send_extended_status1(chan); CHECK_PAYLOAD_SIZE(POWER_STATUS); send_power_status(); break; case MSG_EXTENDED_STATUS2: CHECK_PAYLOAD_SIZE(MEMINFO); send_meminfo(); break; case MSG_ATTITUDE: CHECK_PAYLOAD_SIZE(ATTITUDE); rover.send_attitude(chan); break; case MSG_LOCATION: CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); rover.send_location(chan); break; case MSG_LOCAL_POSITION: CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); send_local_position(rover.ahrs); break; case MSG_NAV_CONTROLLER_OUTPUT: if (rover.control_mode != MANUAL) { CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); rover.send_nav_controller_output(chan); } break; case MSG_GPS_RAW: CHECK_PAYLOAD_SIZE(GPS_RAW_INT); send_gps_raw(rover.gps); break; case MSG_SYSTEM_TIME: CHECK_PAYLOAD_SIZE(SYSTEM_TIME); send_system_time(rover.gps); break; case MSG_SERVO_OUT: CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); rover.send_servo_out(chan); break; case MSG_RADIO_IN: CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); send_radio_in(rover.receiver_rssi); break; case MSG_RADIO_OUT: CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); rover.send_radio_out(chan); break; case MSG_VFR_HUD: CHECK_PAYLOAD_SIZE(VFR_HUD); rover.send_vfr_hud(chan); break; case MSG_RAW_IMU1: CHECK_PAYLOAD_SIZE(RAW_IMU); send_raw_imu(rover.ins, rover.compass); break; case MSG_RAW_IMU3: CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); send_sensor_offsets(rover.ins, rover.compass, rover.barometer); break; case MSG_CURRENT_WAYPOINT: CHECK_PAYLOAD_SIZE(MISSION_CURRENT); rover.send_current_waypoint(chan); break; case MSG_NEXT_PARAM: CHECK_PAYLOAD_SIZE(PARAM_VALUE); queued_param_send(); break; case MSG_NEXT_WAYPOINT: CHECK_PAYLOAD_SIZE(MISSION_REQUEST); queued_waypoint_send(); break; case MSG_STATUSTEXT: // depreciated, use GCS_MAVLINK::send_statustext* return false; case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); send_ahrs(rover.ahrs); break; case MSG_SIMSTATE: CHECK_PAYLOAD_SIZE(SIMSTATE); rover.send_simstate(chan); break; case MSG_HWSTATUS: CHECK_PAYLOAD_SIZE(HWSTATUS); rover.send_hwstatus(chan); break; case MSG_RANGEFINDER: CHECK_PAYLOAD_SIZE(RANGEFINDER); rover.send_rangefinder(chan); break; case MSG_MOUNT_STATUS: #if MOUNT == ENABLED CHECK_PAYLOAD_SIZE(MOUNT_STATUS); rover.camera_mount.status_msg(chan); #endif // MOUNT == ENABLED break; case MSG_RAW_IMU2: case MSG_LIMITS_STATUS: case MSG_FENCE_STATUS: case MSG_WIND: // unused break; case MSG_VIBRATION: CHECK_PAYLOAD_SIZE(VIBRATION); send_vibration(rover.ins); break; case MSG_BATTERY2: CHECK_PAYLOAD_SIZE(BATTERY2); send_battery2(rover.battery); break; case MSG_CAMERA_FEEDBACK: #if CAMERA == ENABLED CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); rover.camera.send_feedback(chan, rover.gps, rover.ahrs, rover.current_loc); #endif break; case MSG_EKF_STATUS_REPORT: #if AP_AHRS_NAVEKF_AVAILABLE CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT); rover.ahrs.send_ekf_status_report(chan); #endif break; case MSG_PID_TUNING: CHECK_PAYLOAD_SIZE(PID_TUNING); rover.send_pid_tuning(chan); break; case MSG_MISSION_ITEM_REACHED: CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED); mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index); break; case MSG_MAG_CAL_PROGRESS: CHECK_PAYLOAD_SIZE(MAG_CAL_PROGRESS); rover.compass.send_mag_cal_progress(chan); break; case MSG_MAG_CAL_REPORT: CHECK_PAYLOAD_SIZE(MAG_CAL_REPORT); rover.compass.send_mag_cal_report(chan); break; case MSG_RETRY_DEFERRED: case MSG_ADSB_VEHICLE: case MSG_TERRAIN: case MSG_OPTICAL_FLOW: case MSG_GIMBAL_REPORT: case MSG_RPM: case MSG_POSITION_TARGET_GLOBAL_INT: break; // just here to prevent a warning } return true; }