void GCS_Mavlink<T>::send_scaled_imu(mavlink_scaled_imu_t &scaled_imu)
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_scaled_imu_encode(_systemId, MAV_COMP_ID_ALL, &msg, &scaled_imu);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_serial.write(buf, len);
}
void scheduleData (unsigned char hilOn, unsigned char* dataOut){
// Generic message container used to pack the messages
mavlink_message_t msg;
// Generic buffer used to hold data to be streamed via serial port
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Cycles from 1 to 10 to decide which
// message's turn is to be sent
static uint8_t samplePeriod = 1;
// Contains the total bytes to send via the serial port
uint8_t bytes2Send = 0;
memset(&msg,0,sizeof(mavlink_message_t));
switch (samplePeriod){
case 1: //GPS
mavlink_msg_heartbeat_pack(SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
MAV_FIXED_WING,
MAV_AUTOPILOT_SLUGS);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
memset(&msg,0,sizeof(mavlink_message_t));
// Pack the GPS message
mavlink_msg_gps_raw_pack(SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
mlGpsData.usec,
mlGpsData.fix_type,
mlGpsData.lat,
mlGpsData.lon,
mlGpsData.alt,
mlGpsData.eph,
mlGpsData.epv,
mlGpsData.v,
mlGpsData.hdg);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 2: // LOAD
mavlink_msg_cpu_load_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlCpuLoadData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 3: // XYZ
mavlink_msg_local_position_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlLocalPositionData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 4: // Dynamic and Reboot (if required)
mavlink_msg_scaled_pressure_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlAirData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
// clear the message
memset(&msg,0,sizeof(mavlink_message_t));
// it there has been a reboot
if(mlBoot.version == 1){
// Copy the message to the send buffer
mavlink_msg_boot_pack(SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
1);
mlBoot.version=0;
}
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 5: // Bias
mavlink_msg_sensor_bias_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlSensorBiasData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 6: // Diagnostic
mavlink_msg_diagnostic_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlDiagnosticData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 7: // Pilot Console Data
mavlink_msg_rc_channels_raw_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlPilotConsoleData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 8: // Sensor Data in meaningful units
mavlink_msg_scaled_imu_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlFilteredData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 9: // Raw Pressure
mavlink_msg_raw_pressure_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlRawPressureData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
default:
*dataOut = 0;
break;
}
memset(&msg,0,sizeof(mavlink_message_t));
// Attitude data. Gets included every sample time
mavlink_msg_attitude_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlAttitudeData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
memset(&msg,0,sizeof(mavlink_message_t));
// Sensor Raw data. Gets included every sample time
mavlink_msg_raw_imu_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlRawImuData);
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
// Put the length of the message in the first byte of the outgoing array
*dataOut = bytes2Send;
// increment/overflow the samplePeriod counter
// configured for 10 Hz in non vital messages
samplePeriod = (samplePeriod >= 10)? 1: samplePeriod + 1;
// Send the data via the debug serial port
if (hilOn == 0){
send2DebugPort(dataOut, hilOn);
}
}
