diff --git a/ports/stm/common-hal/canio/CAN.c b/ports/stm/common-hal/canio/CAN.c index 244cc037ce..8ff91aaca4 100644 --- a/ports/stm/common-hal/canio/CAN.c +++ b/ports/stm/common-hal/canio/CAN.c @@ -225,10 +225,28 @@ void common_hal_canio_can_send(canio_can_obj_t *self, mp_obj_t message_in) .RTR = rtr ? CAN_RTR_REMOTE : CAN_RTR_DATA, .DLC = message->size, }; + uint32_t free_level = HAL_CAN_GetTxMailboxesFreeLevel(&self->handle); + if (free_level == 0) { + // There's no free Tx mailbox. We need to cancel some message without + // transmitting it, because once the bus returns to active state it's + // preferable to transmit the newest messages instead of older messages. + // + // We don't strictly guarantee that we abort the oldest Tx request, + // rather we just abort a different index each time. This permits us + // to avoid tracking this information altogether. + HAL_CAN_AbortTxRequest(&self->handle, 1 << (self->cancel_mailbox)); + self->cancel_mailbox = (self->cancel_mailbox + 1) % 3; + } HAL_StatusTypeDef status = HAL_CAN_AddTxMessage(&self->handle, &header, message->data, &mailbox); if (status != HAL_OK) { mp_raise_OSError(MP_ENOMEM); } + + // wait 8ms (hard coded for now) for TX to occur + uint64_t deadline = port_get_raw_ticks(NULL) + 8; + while (port_get_raw_ticks(NULL) < deadline && HAL_CAN_IsTxMessagePending(&self->handle, 1 << mailbox)) { + RUN_BACKGROUND_TASKS; + } } bool common_hal_canio_can_silent_get(canio_can_obj_t *self) { diff --git a/ports/stm/common-hal/canio/CAN.h b/ports/stm/common-hal/canio/CAN.h index bffc0f65f0..94d7e99a8a 100644 --- a/ports/stm/common-hal/canio/CAN.h +++ b/ports/stm/common-hal/canio/CAN.h @@ -54,6 +54,7 @@ typedef struct canio_can_obj { bool fifo0_in_use:1; bool fifo1_in_use:1; uint8_t periph_index:2; + uint8_t cancel_mailbox; uint8_t start_filter_bank; uint8_t end_filter_bank; long filter_in_use; // bitmask for the 28 filter banks