Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: use a command queue for dshot commands and telemetry requests to avoid losing data

Browse Source
This commit is contained in:
Andy Piper 2023-07-10 10:32:29 +01:00 committed by Andrew Tridgell
parent c6c78a9c32
commit 2af3d53eac
2 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -279,8 +279,8 @@ void AP_IOMCU::thread_main(void)
} }
if (mask & EVENT_MASK(IOEVENT_DSHOT)) { if (mask & EVENT_MASK(IOEVENT_DSHOT)) {
if (!write_registers(PAGE_DSHOT, 0, sizeof(dshot)/sizeof(uint16_t), (const uint16_t*)&dshot)) { page_dshot dshot;
memset(&dshot, 0, sizeof(dshot)); if (!dshot_command_queue.pop(dshot) || !write_registers(PAGE_DSHOT, 0, sizeof(dshot)/sizeof(uint16_t), (const uint16_t*)&dshot)) {
event_failed(mask); event_failed(mask);
continue; continue;
} }
@ -842,17 +842,23 @@ void AP_IOMCU::set_dshot_period(uint16_t period_us, uint8_t drate)
// set output mode // set output mode
void AP_IOMCU::set_telem_request_mask(uint32_t mask) void AP_IOMCU::set_telem_request_mask(uint32_t mask)
{ {
dshot.telem_mask = mask; page_dshot dshot {
.telem_mask = uint16_t(mask)
};
dshot_command_queue.push(dshot);
trigger_event(IOEVENT_DSHOT); trigger_event(IOEVENT_DSHOT);
} }
void AP_IOMCU::send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority) void AP_IOMCU::send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority)
{ {
dshot.command = command; page_dshot dshot {
dshot.chan = chan; .command = command,
dshot.command_timeout_ms = command_timeout_ms; .chan = chan,
dshot.repeat_count = repeat_count; .command_timeout_ms = command_timeout_ms,
dshot.priority = priority; .repeat_count = uint8_t(repeat_count),
.priority = priority
};
dshot_command_queue.push(dshot);
trigger_event(IOEVENT_DSHOT); trigger_event(IOEVENT_DSHOT);
} }
#endif #endif

3
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -256,7 +256,8 @@ private:
uint16_t rate; uint16_t rate;
} dshot_rate; } dshot_rate;
struct page_dshot dshot; // queue of dshot commands that need sending
ObjectBuffer<page_dshot> dshot_command_queue{8};
struct page_GPIO GPIO; struct page_GPIO GPIO;
// output mode values // output mode values