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); } }