Merge pull request #9771 from shellixyz/fix/frsky_crc

make/source.mk

@@ -105,6 +105,7 @@ COMMON_SRC = \
             rx/jetiexbus.c \
             rx/msp.c \
             rx/pwm.c \
+            rx/frsky_crc.c \
             rx/rx.c \
             rx/rx_spi.c \
             rx/rx_spi_common.c \
@@ -253,6 +254,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             rx/rx.c \
             rx/rx_spi.c \
             rx/crsf.c \
+            rx/frsky_crc.c \
             rx/sbus.c \
             rx/sbus_channels.c \
             rx/spektrum.c \
@@ -345,6 +347,7 @@ ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
 SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             drivers/bus_i2c_hal.c \
             drivers/bus_spi_ll.c \
+            rx/frsky_crc.c \
             drivers/max7456.c \
             drivers/pwm_output_dshot.c \
             drivers/pwm_output_dshot_shared.c \

src/main/rx/fport.c

@@ -43,6 +43,7 @@
 
 #include "pg/rx.h"
 
+#include "rx/frsky_crc.h"
 #include "rx/rx.h"
 #include "rx/sbus_channels.h"
 #include "rx/fport.h"
@@ -61,8 +62,6 @@
 #define FPORT_ESCAPE_CHAR 0x7D
 #define FPORT_ESCAPE_MASK 0x20
 
-#define FPORT_CRC_VALUE 0xFF
-
 #define FPORT_BAUDRATE 115200
 
 #define FPORT_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
@@ -240,18 +239,6 @@ static void smartPortWriteFrameFport(const smartPortPayload_t *payload)
 }
 #endif
 
-static bool checkChecksum(uint8_t *data, uint8_t length)
-{
-    uint16_t checksum = 0;
-    for (unsigned i = 0; i < length; i++) {
-        checksum = checksum + *(uint8_t *)(data + i);
-    }
-
-    checksum = (checksum & 0xff) + (checksum >> 8);
-
-    return checksum == FPORT_CRC_VALUE;
-}
-
 static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     static bool hasTelemetryRequest = false;
@@ -271,7 +258,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         if (frameLength != bufferLength - 2) {
             reportFrameError(DEBUG_FPORT_ERROR_SIZE);
         } else {
-            if (!checkChecksum(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
+            if (!frskyCheckSumIsGood(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
                 reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM);
             } else {
                 fportFrame_t *frame = (fportFrame_t *)&rxBuffer[rxBufferReadIndex].data[1];

src/main/rx/frsky_crc.c

@@ -0,0 +1,56 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "rx/frsky_crc.h"
+
+void frskyCheckSumStep(uint16_t *checksum, uint8_t byte)
+{
+    *checksum += byte;
+}
+
+void frskyCheckSumFini(uint16_t *checksum)
+{
+    while (*checksum > 0xFF) {
+        *checksum = (*checksum & 0xFF) + (*checksum >> 8);
+    }
+
+    *checksum = 0xFF - *checksum;
+}
+
+static uint8_t frskyCheckSum(uint8_t *data, uint8_t length)
+{
+    uint16_t checksum = 0;
+    for (unsigned i = 0; i < length; i++) {
+        frskyCheckSumStep(&checksum, *data++);
+    }
+    frskyCheckSumFini(&checksum);
+    return checksum;
+}
+
+bool frskyCheckSumIsGood(uint8_t *data, uint8_t length)
+{
+    uint16_t checksum = frskyCheckSum(data, length);
+    return checksum == 0xFF;
+}

src/main/rx/frsky_crc.h

@@ -0,0 +1,26 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+bool frskyCheckSumIsGood(uint8_t *data, uint8_t length);
+void frskyCheckSumStep(uint16_t *checksum, uint8_t byte);   // Add byte to checksum
+void frskyCheckSumFini(uint16_t *checksum);                 // Finalize checksum

src/main/telemetry/smartport.c

@@ -39,6 +39,8 @@
 
 #include "config/feature.h"
 
+#include "rx/frsky_crc.h"
+
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/compass/compass.h"
 #include "drivers/sensor.h"
@@ -282,7 +284,7 @@ void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
     }
 
     if (checksum != NULL) {
-        *checksum += c;
+        frskyCheckSumStep(checksum, c);
     }
 }
 
@@ -297,8 +299,8 @@ void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *
     for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
         smartPortSendByte(*data++, &checksum, port);
     }
-    checksum = 0xff - ((checksum & 0xff) + (checksum >> 8));
+    frskyCheckSumFini(&checksum);
-    smartPortSendByte((uint8_t)checksum, NULL, port);
+    smartPortSendByte(checksum, NULL, port);
 }
 
 static void smartPortWriteFrameInternal(const smartPortPayload_t *payload)