Merge 2070f93c44 into acbab53d13

mk/source.mk

@@ -253,6 +253,7 @@ COMMON_SRC = \
             io/rcdevice_cam.c \
             io/rcdevice.c \
             io/gps.c \
+            io/gps_utils.c \
             io/ledstrip.c \
             io/pidaudio.c \
             osd/osd.c \

src/main/drivers/serial.c

@@ -20,6 +20,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 
 #include "platform.h"
 
@@ -28,10 +29,7 @@
 
 void serialPrint(serialPort_t *instance, const char *str)
 {
-    uint8_t ch;
-    while ((ch = *(str++)) != 0) {
-        serialWrite(instance, ch);
-    }
+    serialWriteBuf(instance, (const uint8_t*)str, strlen(str));
 }
 
 uint32_t serialGetBaudRate(serialPort_t *instance)

src/main/io/gps_utils.c

@@ -0,0 +1,80 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is 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.
+ *
+ * Betaflight is distributed in the hope that it 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 <stdarg.h>
+
+#include "platform.h"
+
+#include "common/printf.h"
+#include "io/serial.h"
+
+struct nmea_putp {
+    serialPort_t* port;
+    uint8_t csum;
+};
+
+static void nmea_putcf_csum(void* putp, char c)
+{
+    struct nmea_putp* p = putp;
+    serialWrite(p->port, c);
+    p->csum ^= c;
+}
+
+static void nmea_putcf(void* putp, char c)
+{
+    struct nmea_putp* p = putp;
+    serialWrite(p->port, c);
+}
+
+static void nmeaPrintfva_raw(struct nmea_putp* putp, const char *format, va_list va)
+{
+    tfp_format(putp, nmea_putcf, format, va);
+}
+
+// tfp_prinf shall replace this
+static void nmeaPrintf_raw(struct nmea_putp* putp, const char *format, ...)
+{
+    va_list va;
+    va_start(va, format);
+    nmeaPrintfva_raw(putp, format, va);
+    va_end(va);
+}
+
+static void nmeaPrintfva(serialPort_t *port, const char *format, va_list va)
+{
+    struct nmea_putp putp = {
+        .port = port,
+        .csum = '$'             // don't checkfum '$' in header
+    };
+    tfp_format(&putp, nmea_putcf_csum, format, va);
+    nmeaPrintf_raw(&putp, "*%02X\r\n", putp.csum);
+}
+
+// printf format (+ arguments), append "*<csum>\r\n"
+// nmeaPrintf(port, "$PUBX,41,1,0003,0001,%d,0", 115200);
+void nmeaPrintf(serialPort_t *port, const char *format, ...)
+{
+    va_list va;
+    va_start(va, format);
+    nmeaPrintfva(port, format, va);
+    va_end(va);
+}
+

src/main/io/gps_utils.h

@@ -0,0 +1,24 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is 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.
+ *
+ * Betaflight is distributed in the hope that it 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/>.
+ */
+
+#pragma once
+
+int nmeaPrintf(serialPort_t *port, const char *format, ...);

src/test/Makefile

@@ -187,6 +187,10 @@ flight_mixer_unittest :=  \
 gps_conversion_unittest_SRC := \
 		$(USER_DIR)/common/gps_conversion.c
 
+gps_nmea_unittest_SRC := \
+		$(USER_DIR)/io/gps_utils.c \
+		$(USER_DIR)/common/printf.c \
+		$(USER_DIR)/common/typeconversion.c
 
 io_serial_unittest_SRC := \
 		$(USER_DIR)/io/serial.c \

src/test/unit/gps_nmea_unittest.cc

@@ -0,0 +1,86 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is 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.
+ *
+ * Betaflight is distributed in the hope that it 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 <string.h>
+
+extern "C" {
+    #include "platform.h"
+
+    #include "drivers/serial.h"
+    #include "io/serial.h"
+
+    #include "io/gps_utils.h"
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+static serialPort_t *gpsPort;
+static int serialWritePos = 0;
+
+#define SERIAL_BUFFER_SIZE 256
+static uint8_t serialWriteBuffer[SERIAL_BUFFER_SIZE];
+
+serialPort_t serialTestInstance;
+
+void serialWrite(serialPort_t *instance, uint8_t ch)
+{
+    EXPECT_EQ(instance, &serialTestInstance);
+    EXPECT_LT(serialWritePos, sizeof(serialWriteBuffer));
+    serialWriteBuffer[serialWritePos++] = ch;
+}
+
+void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count)
+{
+    while(count--)
+        serialWrite(instance, *data++);
+}
+
+void serialTestResetBuffers()
+{
+    gpsPort = &serialTestInstance;
+    memset(&serialWriteBuffer, 0, sizeof(serialWriteBuffer));
+    serialWritePos = 0;
+}
+
+
+// list of valid NMEA commands
+const char * nmeaSamples[] = {
+    "$PUBX,41,1,0003,0001,38400,0*26\r\n",
+    "$PSRF103,00,6,00,0*23\r\n",
+    "$PCAS02,100*1E\r\n",
+    "$PSRF103,03,00,00,01*27\r\n",
+};
+
+TEST(nmeaUtilsTest, TestNmeaPrintf)
+{
+    for (unsigned i = 0; i < ARRAYLEN(nmeaSamples); i++) {
+        serialTestResetBuffers();
+        const char* sample = nmeaSamples[i];
+        int starPos = strcspn(sample, "*");
+        EXPECT_EQ(strlen(sample) - strlen("*XX\r\n"), starPos);
+        const char* tx = strndup(sample, starPos);
+        nmeaPrintf(gpsPort, "%s", tx);
+        serialWrite(gpsPort, 0);  // terminate string for simplicity
+        free((void*)tx);
+        ASSERT_STREQ((const char*)serialWriteBuffer, sample);
+    }
+}