diff --git a/make/source.mk b/make/source.mk
index c248b3b36a..101e4dec72 100644
--- a/make/source.mk
+++ b/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 \
diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index a6d455d0c9..ac38a3e948 100644
--- a/src/main/rx/fport.c
+++ b/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];
diff --git a/src/main/rx/frsky_crc.c b/src/main/rx/frsky_crc.c
new file mode 100644
index 0000000000..8550373e3e
--- /dev/null
+++ b/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 .
+ */
+
+#include
+#include
+
+#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;
+}
diff --git a/src/main/rx/frsky_crc.h b/src/main/rx/frsky_crc.h
new file mode 100644
index 0000000000..ede2e4dc8d
--- /dev/null
+++ b/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 .
+ */
+
+#include
+#include
+
+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
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 6b34a1d625..5cc9bbbfe5 100644
--- a/src/main/telemetry/smartport.c
+++ b/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));
- smartPortSendByte((uint8_t)checksum, NULL, port);
+ frskyCheckSumFini(&checksum);
+ smartPortSendByte(checksum, NULL, port);
}
static void smartPortWriteFrameInternal(const smartPortPayload_t *payload)