mirror of https://github.com/ArduPilot/ardupilot
GCS_MAVLink: adjust for 16 channels in SERVO_OUTPUT_RAW
This commit is contained in:
parent
995fb7cd86
commit
306487cad4
|
@ -166,6 +166,7 @@ public:
|
||||||
void send_home(const Location &home) const;
|
void send_home(const Location &home) const;
|
||||||
static void send_home_all(const Location &home);
|
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_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
|
// return a bitmap of active channels. Used by libraries to loop
|
||||||
// over active channels to send to all active channels
|
// over active channels to send to all active channels
|
||||||
|
|
|
@ -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]);
|
||||||
|
}
|
||||||
|
|
Loading…
Reference in New Issue