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

AP_IOMCU: always run iofirmware loop at 1Khz to avoid uart races 

don't look for multiple pages on single-page packets
This commit is contained in:
Andy Piper 2023-11-25 15:47:55 +00:00 committed by Andrew Tridgell
parent 5d9f9db2fe
commit 1fb6205439
1 changed files with 10 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_IOMCU/iofirmware/iofirmware.cpp
View File

@ -411,13 +411,7 @@ static void stackCheck(uint16_t& mstack, uint16_t& pstack) {
*/ */
void AP_IOMCU_FW::update() void AP_IOMCU_FW::update()
{ {
#if CH_CFG_ST_FREQUENCY == 1000000
eventmask_t mask = chEvtWaitAnyTimeout(IOEVENT_PWM | IOEVENT_TX_END | IOEVENT_TX_BEGIN, TIME_US2I(1000)); eventmask_t mask = chEvtWaitAnyTimeout(IOEVENT_PWM | IOEVENT_TX_END | IOEVENT_TX_BEGIN, TIME_US2I(1000));
#else
// we are not running any other threads, so we can use an
// immediate timeout here for lowest latency
eventmask_t mask = chEvtWaitAnyTimeout(IOEVENT_PWM | IOEVENT_TX_END, TIME_IMMEDIATE);
#endif
#ifdef HAL_GPIO_LINE_GPIO107 #ifdef HAL_GPIO_LINE_GPIO107
TOGGLE_PIN_DEBUG(107); TOGGLE_PIN_DEBUG(107);
#endif #endif
@ -948,18 +942,17 @@ bool AP_IOMCU_FW::handle_code_write()
// no input when override is active // no input when override is active
break; break;
} }
/* copy channel data */ if (rx_io_packet.count != sizeof(reg_direct_pwm.pwm)/2) {
uint16_t i = 0, offset = rx_io_packet.offset, num_values = rx_io_packet.count;
if (offset + num_values > sizeof(reg_direct_pwm.pwm)/2) {
return false; return false;
} }
while ((offset < IOMCU_MAX_CHANNELS) && (num_values > 0)) { /* copy channel data */
uint16_t i = 0, num_values = rx_io_packet.count;
while ((i < IOMCU_MAX_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */ /* XXX range-check value? */
if (rx_io_packet.regs[i] != PWM_IGNORE_THIS_CHANNEL) { if (rx_io_packet.regs[i] != PWM_IGNORE_THIS_CHANNEL) {
reg_direct_pwm.pwm[offset] = rx_io_packet.regs[i]; reg_direct_pwm.pwm[i] = rx_io_packet.regs[i];
} }
offset++;
num_values--; num_values--;
i++; i++;
} }
@ -968,7 +961,7 @@ bool AP_IOMCU_FW::handle_code_write()
break; break;
} }
case PAGE_MIXING: { case PAGE_MIXING: { // multi-packet message
uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count; uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
if (offset + num_values > sizeof(mixing)/2) { if (offset + num_values > sizeof(mixing)/2) {
return false; return false;
@ -978,11 +971,10 @@ bool AP_IOMCU_FW::handle_code_write()
} }
case PAGE_FAILSAFE_PWM: { case PAGE_FAILSAFE_PWM: {
uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count; if (rx_io_packet.count != sizeof(reg_failsafe_pwm.pwm)/2) {
if (offset + num_values > sizeof(reg_failsafe_pwm.pwm)/2) {
return false; return false;
} }
memcpy((&reg_failsafe_pwm.pwm[0])+offset, &rx_io_packet.regs[0], num_values*2); memcpy((&reg_failsafe_pwm.pwm[0]), &rx_io_packet.regs[0], rx_io_packet.count*2);
break; break;
} }
@ -994,11 +986,10 @@ bool AP_IOMCU_FW::handle_code_write()
break; break;
case PAGE_DSHOT: { case PAGE_DSHOT: {
uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count; if (rx_io_packet.count != sizeof(dshot)/2) {
if (offset + num_values > sizeof(dshot)/2) {
return false; return false;
} }
memcpy(((uint16_t *)&dshot)+offset, &rx_io_packet.regs[0], num_values*2); memcpy(((uint16_t *)&dshot)+rx_io_packet.offset, &rx_io_packet.regs[0], rx_io_packet.count*2);
if(dshot.telem_mask) { if(dshot.telem_mask) {
hal.rcout->set_telem_request_mask(dshot.telem_mask); hal.rcout->set_telem_request_mask(dshot.telem_mask);
} }