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