aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: adjust for 16 channels in SERVO_OUTPUT_RAW

Browse Source
This commit is contained in:
Andrew Tridgell 2016-07-15 20:04:19 +10:00
parent 995fb7cd86
commit 306487cad4
2 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -166,6 +166,7 @@ public:
void send_home(const Location &home) const;
static void send_home_all(const Location &home);
void send_heartbeat(uint8_t type, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status);
void send_servo_output_raw(bool hil);
// return a bitmap of active channels. Used by libraries to loop
// over active channels to send to all active channels

27
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1667,3 +1667,30 @@ bool GCS_MAVLINK::telemetry_delayed(mavlink_channel_t _chan)
}
/*
send SERVO_OUTPUT_RAW
*/
void GCS_MAVLINK::send_servo_output_raw(bool hil)
{
uint16_t values[16] {};
if (hil) {
for (uint8_t i=0; i<16; i++) {
values[i] = RC_Channel::rc_channel(i)->get_radio_out();
}
} else {
hal.rcout->read(values, 16);
}
for (uint8_t i=0; i<16; 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]);
}