AP_HAL_ChibiOS: use datalength when copying payload into CAN Frames
This commit is contained in:
parent
cf5d94b81f
commit
4bdbd37731
@ -887,7 +887,7 @@ bool CANIface::readRxFIFO(uint8_t fifo_index)
|
|||||||
frame.dlc = (frame_ptr[1] & DLC_MASK) >> 16;
|
frame.dlc = (frame_ptr[1] & DLC_MASK) >> 16;
|
||||||
uint8_t *data = (uint8_t*)&frame_ptr[2];
|
uint8_t *data = (uint8_t*)&frame_ptr[2];
|
||||||
|
|
||||||
for (uint8_t i = 0; i < AP_HAL::CANFrame::MaxDataLen; i++) {
|
for (uint8_t i = 0; i < AP_HAL::CANFrame::dlcToDataLength(frame.dlc); i++) {
|
||||||
frame.data[i] = data[i];
|
frame.data[i] = data[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user