From 633eb0db8b7d9c271957449f643d271d71842f17 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 2 Mar 2022 14:54:19 +0530 Subject: [PATCH] AP_HAL_ChibiOS: move to using data_32 for copying into CANFD buffer --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 84837325f6..b84173b82b 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -317,13 +317,6 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing return true; } -static uint32_t bytes_to_word(const uint8_t data[]) { - return (uint32_t(data[3]) << 24) | - (uint32_t(data[2]) << 16) | - (uint32_t(data[1]) << 8) | - (uint32_t(data[0]) << 0); -} - int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags) { @@ -378,8 +371,9 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, // Write Frame to the message RAM const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc); - for (uint8_t i = 0, l = 2; i < data_length; i+=4, l++) { - buffer[l] = bytes_to_word(&frame.data[i]); + uint32_t *data_ptr = &buffer[2]; + for (uint8_t i = 0; i < (data_length+3)/4; i++) { + data_ptr[i] = frame.data_32[i]; } //Set Add Request