uint16_t HIL_PackRawServo(uint8_t system_id, uint8_t component_id, mavlink_servo_output_raw_t mlPwmCommands ,uint32_t time_usec){
mavlink_system_t mavlink_system;
mavlink_system.sysid = system_id; ///< ID 20 for this airplane
mavlink_system.compid = component_id;//MAV_COMP_ID_IMU; ///< The component sending the message is the IMU, it could be also a Linux process
//////////////////////////////////////////////////////////////////////////
mavlink_message_t msg;
memset(&msg, 0, sizeof (mavlink_message_t));
mavlink_msg_servo_output_raw_pack(mavlink_system.sysid, mavlink_system.compid, &msg , time_usec , mlPwmCommands.port, mlPwmCommands.servo1_raw, mlPwmCommands.servo2_raw, mlPwmCommands.servo3_raw, mlPwmCommands.servo4_raw, mlPwmCommands.servo5_raw, mlPwmCommands.servo6_raw, mlPwmCommands.servo7_raw, mlPwmCommands.servo8_raw );
return( mavlink_msg_to_send_buffer(Uart4OutBuff, &msg));
}
void servos_telemetry_mavlink_send(servos_telemetry_t* servos_telemetry, Mavlink_stream* mavlink_stream, mavlink_message_t* msg)
{
mavlink_msg_servo_output_raw_pack(mavlink_stream->sysid(),
mavlink_stream->compid(),
msg,
time_keeper_get_us(),
0,
(uint16_t)(1500 + 500 * servos_telemetry->servos[0]->read()),
(uint16_t)(1500 + 500 * servos_telemetry->servos[1]->read()),
(uint16_t)(1500 + 500 * servos_telemetry->servos[2]->read()),
(uint16_t)(1500 + 500 * servos_telemetry->servos[3]->read()),
(uint16_t)( 1500 + 500 * servos_telemetry->servos[4]->read() ),
(uint16_t)( 1500 + 500 * servos_telemetry->servos[5]->read() ),
(uint16_t)( 1500 + 500 * servos_telemetry->servos[6]->read() ),
(uint16_t)( 1500 + 500 * servos_telemetry->servos[7]->read() )
);
}
void cDataLink::SendServoOutputRaw(uint16_t ped_us, uint16_t col_us, uint16_t lon_us,
uint16_t lat_us, uint16_t aux_us, uint16_t mode_us)
{
int bytes_sent;
uint16_t len;
/* Send RC channels */
{
mavlink_msg_servo_output_raw_pack(1, MAV_COMP_ID_ALL, &m_msg, microsSinceEpoch(),
uint8_t(1), // port
ped_us, col_us, lon_us, lat_us, //1...4
aux_us, mode_us, //5, 6
uint16_t(65535), uint16_t(65535)); // 6...8, 65535 implies the channel is unused
len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
(void)bytes_sent; //avoid compiler warning
}
}
/* Raw Servo Output Sender */
bool MAVLink::sendRawServos(uint16_t s1, uint16_t s2, uint16_t s3, uint16_t s4, uint16_t s5, uint16_t s6, uint16_t s7, uint16_t s8) {
mavlink_message_t msg;
mavlink_msg_servo_output_raw_pack(mySystemId,myComponentId,&msg,s1,s2,s3,s4,s5,s6,s7,s8);
return sendMessage(msg);
}
