static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev)
{
#if defined RADIO_CONTROL
mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
get_sys_time_msec(),
RC_CHANNELS,
PPM_PULSES(0), PPM_PULSES(1), PPM_PULSES(2),
PPM_PULSES(3), PPM_PULSES(4), PPM_PULSES(5),
PPM_PULSES(6), PPM_PULSES(7), PPM_PULSES(8),
PPM_PULSES(9), PPM_PULSES(10), PPM_PULSES(11),
PPM_PULSES(12), PPM_PULSES(13), PPM_PULSES(14),
PPM_PULSES(15), PPM_PULSES(16), PPM_PULSES(17),
255); // rssi unknown
#else
mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
get_sys_time_msec(),
0, // zero channels available
UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
UINT16_MAX, UINT16_MAX, 255);
#endif
MAVLinkSendMessage();
}
/*
send RC_CHANNELS_RAW, and RC_CHANNELS messages
*/
void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi)
{
uint32_t now = hal.scheduler->millis();
uint16_t values[8];
memset(values, 0, sizeof(values));
hal.rcin->read(values, 8);
mavlink_msg_rc_channels_raw_send(
chan,
now,
0, // port
values[0],
values[1],
values[2],
values[3],
values[4],
values[5],
values[6],
values[7],
receiver_rssi);
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
if (hal.rcin->num_channels() > 8 &&
comm_get_txspace(chan) >= MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
mavlink_msg_rc_channels_send(
chan,
now,
hal.rcin->num_channels(),
hal.rcin->read(CH_1),
hal.rcin->read(CH_2),
hal.rcin->read(CH_3),
hal.rcin->read(CH_4),
hal.rcin->read(CH_5),
hal.rcin->read(CH_6),
hal.rcin->read(CH_7),
hal.rcin->read(CH_8),
hal.rcin->read(CH_9),
hal.rcin->read(CH_10),
hal.rcin->read(CH_11),
hal.rcin->read(CH_12),
hal.rcin->read(CH_13),
hal.rcin->read(CH_14),
hal.rcin->read(CH_15),
hal.rcin->read(CH_16),
hal.rcin->read(CH_17),
hal.rcin->read(CH_18),
receiver_rssi);
}
#endif
}
void send(const hrt_abstime t)
{
struct rc_input_values rc;
if (rc_sub->update(&rc_time, &rc)) {
const unsigned port_width = 8;
// Deprecated message (but still needed for compatibility!)
for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(_channel,
rc.timestamp_publication / 1000,
i,
(rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX,
rc.rssi);
}
// New message
mavlink_msg_rc_channels_send(_channel,
rc.timestamp_publication / 1000,
rc.channel_count,
((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
rc.rssi);
}
}
static void mavlink_send_rc_raw(void)
{
mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
millis(),
0,
pwmRead(0),
pwmRead(1),
pwmRead(2),
pwmRead(3),
pwmRead(4),
pwmRead(5),
pwmRead(6),
pwmRead(7),
0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 0);
}
