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

AP_ToshibaCAN: correct checking of payload space in telem send

Browse Source
This commit is contained in:
Peter Barker 2020-07-19 10:37:32 +10:00 committed by Andrew Tridgell
parent 721d94a23b
commit 3f4a14c610
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp
View File

@ -499,11 +499,6 @@ void AP_ToshibaCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
return;
}
// return if no space in output buffer to send mavlink messages
if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)mav_chan, ESC_TELEMETRY_1_TO_4)) {
return;
}
// output telemetry messages
{
// take semaphore to access telemetry data
@ -512,6 +507,11 @@ void AP_ToshibaCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
// loop through 3 groups of 4 ESCs
for (uint8_t i = 0; i < 3; i++) {
// return if no space in output buffer to send mavlink messages
if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)mav_chan, ESC_TELEMETRY_1_TO_4)) {
return;
}
// skip this group of ESCs if no data to send
if ((_esc_present_bitmask & ((uint32_t)0x0F << i*4)) == 0) {
continue;