// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
int16_t payload_space = serial_free();
if (telemetry_delayed(chan)) {
return false;
}
switch(id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;
case MSG_AHRS:
CHECK_PAYLOAD_SIZE(AHRS);
send_ahrs(chan);
break;
case MSG_STATUSTEXT:
CHECK_PAYLOAD_SIZE(STATUSTEXT);
send_statustext(chan);
break;
default:
break;
}
return true;
}
// 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);
send_servo_output_raw(false);
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;
}
// try to send a message, return false if it won't fit in the serial tx buffer
bool GCS_MAVLINK_Tracker::try_send_message(enum ap_message id)
{
switch (id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
last_heartbeat_time = AP_HAL::millis();
tracker.send_heartbeat(chan);
return true;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
tracker.send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
tracker.send_location(chan);
break;
case MSG_LOCAL_POSITION:
CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED);
send_local_position(tracker.ahrs);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
tracker.send_nav_controller_output(chan);
break;
case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
send_gps_raw(tracker.gps);
break;
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS);
send_radio_in(0);
break;
case MSG_SERVO_OUTPUT_RAW:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_servo_output_raw(false);
break;
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu(tracker.ins, tracker.compass);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_scaled_pressure(tracker.barometer);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_sensor_offsets(tracker.ins, tracker.compass, tracker.barometer);
break;
case MSG_NEXT_PARAM:
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
queued_param_send();
break;
case MSG_NEXT_WAYPOINT:
CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
tracker.send_waypoint_request(chan);
break;
case MSG_STATUSTEXT:
// depreciated, use GCS_MAVLINK::send_statustext*
return false;
case MSG_AHRS:
CHECK_PAYLOAD_SIZE(AHRS);
send_ahrs(tracker.ahrs);
break;
case MSG_SIMSTATE:
CHECK_PAYLOAD_SIZE(SIMSTATE);
tracker.send_simstate(chan);
break;
case MSG_HWSTATUS:
CHECK_PAYLOAD_SIZE(HWSTATUS);
tracker.send_hwstatus(chan);
break;
case MSG_MAG_CAL_PROGRESS:
tracker.compass.send_mag_cal_progress(chan);
break;
case MSG_MAG_CAL_REPORT:
tracker.compass.send_mag_cal_report(chan);
break;
case MSG_SERVO_OUT:
case MSG_EXTENDED_STATUS1:
case MSG_EXTENDED_STATUS2:
case MSG_RETRY_DEFERRED:
case MSG_ADSB_VEHICLE:
case MSG_CURRENT_WAYPOINT:
case MSG_VFR_HUD:
case MSG_SYSTEM_TIME:
case MSG_LIMITS_STATUS:
case MSG_FENCE_STATUS:
case MSG_WIND:
case MSG_RANGEFINDER:
case MSG_TERRAIN:
case MSG_BATTERY2:
case MSG_BATTERY_STATUS:
case MSG_CAMERA_FEEDBACK:
case MSG_MOUNT_STATUS:
case MSG_OPTICAL_FLOW:
case MSG_GIMBAL_REPORT:
case MSG_EKF_STATUS_REPORT:
case MSG_PID_TUNING:
case MSG_VIBRATION:
case MSG_RPM:
case MSG_MISSION_ITEM_REACHED:
case MSG_POSITION_TARGET_GLOBAL_INT:
break; // just here to prevent a warning
}
return true;
}
// try to send a message, return false if it won't fit in the serial tx buffer
bool GCS_MAVLINK_Tracker::try_send_message(enum ap_message id)
{
switch (id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
last_heartbeat_time = AP_HAL::millis();
tracker.send_heartbeat(chan);
return true;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
tracker.send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
tracker.send_location(chan);
break;
case MSG_LOCAL_POSITION:
CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED);
send_local_position(tracker.ahrs);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
tracker.send_nav_controller_output(chan);
break;
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS);
send_radio_in(0);
break;
case MSG_SERVO_OUTPUT_RAW:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_servo_output_raw(false);
break;
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu(tracker.ins, tracker.compass);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_scaled_pressure(tracker.barometer);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_sensor_offsets(tracker.ins, tracker.compass, tracker.barometer);
break;
case MSG_AHRS:
CHECK_PAYLOAD_SIZE(AHRS);
send_ahrs(tracker.ahrs);
break;
case MSG_SIMSTATE:
CHECK_PAYLOAD_SIZE(SIMSTATE);
tracker.send_simstate(chan);
break;
default:
return GCS_MAVLINK::try_send_message(id);
}
return true;
}