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

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

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 (!write_registers(PAGE_DSHOT, 0, sizeof(dshot)/sizeof(uint16_t), (const uint16_t*)&dshot)) {
memset(&dshot, 0, sizeof(dshot));
page_dshot dshot;
if (!dshot_command_queue.pop(dshot) || !write_registers(PAGE_DSHOT, 0, sizeof(dshot)/sizeof(uint16_t), (const uint16_t*)&dshot)) {
event_failed(mask);
continue;
}
@ -842,17 +842,23 @@ void AP_IOMCU::set_dshot_period(uint16_t period_us, uint8_t drate)
// set output mode
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);
}
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;
dshot.chan = chan;
dshot.command_timeout_ms = command_timeout_ms;
dshot.repeat_count = repeat_count;
dshot.priority = priority;
page_dshot dshot {
.command = command,
.chan = chan,
.command_timeout_ms = command_timeout_ms,
.repeat_count = uint8_t(repeat_count),
.priority = priority
};
dshot_command_queue.push(dshot);
trigger_event(IOEVENT_DSHOT);
}
#endif

3
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -256,7 +256,8 @@ private:
uint16_t 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;
// output mode values