Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: send servo raw 17-32 using port 1

This commit is contained in:
Iampete1 2021-01-12 14:28:43 +00:00 committed by Andrew Tridgell
parent 077401df69
commit 7d3368fa33
1 changed files with 37 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -2718,22 +2718,47 @@ bool GCS_MAVLINK::telemetry_delayed() const
*/
void GCS_MAVLINK::send_servo_output_raw()
{
uint16_t values[16] {};
hal.rcout->read(values, 16);
const uint32_t enabled_mask = ~SRV_Channels::get_output_channel_mask(SRV_Channel::k_GPIO);
if (enabled_mask == 0) {
return;
}
for (uint8_t i=0; i<16; i++) {
#if NUM_SERVO_CHANNELS >= 17
static const uint8_t max_channels = 32;
#else
static const uint8_t max_channels = 16;
#endif
uint16_t values[max_channels] {};
hal.rcout->read(values, max_channels);
for (uint8_t i=0; i<max_channels; i++) {
if (values[i] == 65535) {
values[i] = 0;
}
}
mavlink_msg_servo_output_raw_send(
chan,
AP_HAL::micros(),
0, // port
values[0], values[1], values[2], values[3],
values[4], values[5], values[6], values[7],
values[8], values[9], values[10], values[11],
values[12], values[13], values[14], values[15]);
}
if ((enabled_mask & 0xFFFF) != 0) {
mavlink_msg_servo_output_raw_send(
chan,
AP_HAL::micros(),
0, // port
values[0], values[1], values[2], values[3],
values[4], values[5], values[6], values[7],
values[8], values[9], values[10], values[11],
values[12], values[13], values[14], values[15]);
}
#if NUM_SERVO_CHANNELS >= 17
if ((enabled_mask & 0xFFFF0000) != 0) {
mavlink_msg_servo_output_raw_send(
chan,
AP_HAL::micros(),
1, // port
values[16], values[17], values[18], values[19],
values[20], values[21], values[22], values[23],
values[24], values[25], values[26], values[27],
values[28], values[29], values[30], values[31]);
}
#endif
}