From 21937a78302cb252f9d310c82979520ba681eb6b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 6 Jul 2020 08:19:54 +1000 Subject: [PATCH] AP_RCProtocol: fixed alignment errors in SRXL2 invalid alignment used --- libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp | 14 ++++++++------ libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h | 4 ++-- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp index d40950effe..6c030b3b9d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "spm_srxl.h" @@ -149,17 +150,17 @@ void AP_RCProtocol_SRXL2::update(void) } } -void AP_RCProtocol_SRXL2::capture_scaled_input(const uint16_t *values, bool in_failsafe, int16_t new_rssi) +void AP_RCProtocol_SRXL2::capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t new_rssi) { AP_RCProtocol_SRXL2* srxl2 = AP_RCProtocol_SRXL2::get_singleton(); if (srxl2 != nullptr) { - srxl2->_capture_scaled_input(values, in_failsafe, new_rssi); + srxl2->_capture_scaled_input(values_p, in_failsafe, new_rssi); } } // capture SRXL2 encoded values -void AP_RCProtocol_SRXL2::_capture_scaled_input(const uint16_t *values, bool in_failsafe, int16_t new_rssi) +void AP_RCProtocol_SRXL2::_capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t new_rssi) { _in_failsafe = in_failsafe; // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link @@ -200,7 +201,8 @@ void AP_RCProtocol_SRXL2::_capture_scaled_input(const uint16_t *values, bool in_ * * So here we scale to DSMX-2048 and then use our regular Spektrum conversion. */ - _channels[channel] = ((int32_t)(values[i] >> 5) * 1194) / 2048 + 903; + const uint16_t v = le16toh_ptr(&values_p[i*2]); + _channels[channel] = ((int32_t)(v >> 5) * 1194) / 2048 + 903; } } @@ -316,9 +318,9 @@ void srxlFillTelemetry(SrxlTelemetryData* pTelemetryData) void srxlReceivedChannelData(SrxlChannelData* pChannelData, bool isFailsafe) { if (isFailsafe) { - AP_RCProtocol_SRXL2::capture_scaled_input(pChannelData->values, true, pChannelData->rssi); + AP_RCProtocol_SRXL2::capture_scaled_input((const uint8_t *)pChannelData->values, true, pChannelData->rssi); } else { - AP_RCProtocol_SRXL2::capture_scaled_input(srxlChData.values, false, srxlChData.rssi); + AP_RCProtocol_SRXL2::capture_scaled_input((const uint8_t *)srxlChData.values, false, srxlChData.rssi); } } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h index 217f4521bd..93b0644d70 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h @@ -35,7 +35,7 @@ public: } // static functions for SRXL2 callbacks - static void capture_scaled_input(const uint16_t *values, bool in_failsafe, int16_t rssi); + static void capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t rssi); static void send_on_uart(uint8_t* pBuffer, uint8_t length); static void change_baud_rate(uint32_t baudrate); @@ -48,7 +48,7 @@ private: void _process_byte(uint32_t timestamp_us, uint8_t byte); void _send_on_uart(uint8_t* pBuffer, uint8_t length); void _change_baud_rate(uint32_t baudrate); - void _capture_scaled_input(const uint16_t *values, bool in_failsafe, int16_t rssi); + void _capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t rssi); uint8_t _buffer[SRXL2_FRAMELEN_MAX]; /* buffer for raw srxl frame data in correct order --> buffer[0]=byte0 buffer[1]=byte1 */ uint8_t _buflen; /* length in number of bytes of received srxl dataframe in buffer */