new NRA protocol fro radars from nanoradar

new rangerfinder NRA15/NRA24 from nanoradar

docs/Rangefinder.md

@@ -21,6 +21,22 @@ Following rangefinders are supported:
 * MSP - experimental
 * TOF10120 - small & lightweight laser range sensor, usable up to 200cm
 * TERARANGER EVO - 30cm to 600cm, depends on version https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
+* NRA15/NRA24 - experimental, UART version
+
+#### NRA15/NRA24
+NRA15/NRA24 from nanoradar use US-D1_V0 or NRA protocol, it depends which firmware you use. Radar can be set by firmware
+to two different resolutions. See table below.
+
+| Radar | Protocol | Resolution      | Name in configurator  |
+|-------|----------|-----------------|-----------------------|
+| NRA15 | US-D1_V0 | 0-30m (+-4cm)   | USD1_V0               |
+| NRA15 | NRA      | 0-30m (+-4cm)   | NRA15/NRA24           | 
+| NRA15 | NRA      | 0-100m (+-10cm) | NRA15/NRA24           | 
+| NRA24 | US-D1_V0 | 0-50m (+-4cm)   | USD1_V0               |
+| NRA24 | US-D1_V0 | 0-200m (+-10cm) | USD1_V0               |
+| NRA24 | NRA      | 0-50m (+-4cm)   | NRA15/NRA24           | 
+| NRA24 | NRA      | 0-200m (+-10cm) | NRA15/NRA24           | 
+
 
 ## Connections
 

src/main/CMakeLists.txt

@@ -1,3 +1,4 @@
+
 main_sources(COMMON_SRC
     main.c
 
@@ -485,6 +486,8 @@ main_sources(COMMON_SRC
     io/rangefinder.h
     io/rangefinder_msp.c
     io/rangefinder_benewake.c
+    io/rangefinder_usd1_v0.c
+    io/rangefinder_nanoradar.c
     io/rangefinder_fake.c
     io/opflow.h
     io/opflow_cxof.c

src/main/fc/settings.yaml

@@ -5,7 +5,7 @@ tables:
     values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX",  "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
-    values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO"]
+    values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA15/NRA24"]
     enum: rangefinderType_e
   - name: mag_hardware
     values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]

src/main/io/rangefinder.h

@@ -31,6 +31,8 @@
 
 extern virtualRangefinderVTable_t rangefinderMSPVtable;
 extern virtualRangefinderVTable_t rangefinderBenewakeVtable;
+extern virtualRangefinderVTable_t rangefinderUSD1Vtable;
+extern virtualRangefinderVTable_t rangefinderNanoradarVtable; //NRA15/NRA24
 extern virtualRangefinderVTable_t rangefinderFakeVtable;
 
 void mspRangefinderReceiveNewData(uint8_t * bufferPtr);

src/main/io/rangefinder_nanoradar.c

@@ -0,0 +1,167 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it 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.
+ *
+ * This file 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 program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <ctype.h>
+
+#include "platform.h"
+#include "io/serial.h"
+#include "drivers/time.h"
+
+#if defined(USE_RANGEFINDER_NANORADAR)
+#include "drivers/rangefinder/rangefinder_virtual.h"
+
+#define NANORADAR_HDR 0xAA // Header Byte
+#define NANORADAR_END 0x55
+
+#define NANORADAR_COMMAND_TARGET_INFO 0x70C
+
+typedef struct __attribute__((packed)) {
+    uint8_t     header0;
+    uint8_t     header1;
+    uint8_t     commandH;
+    uint8_t     commandL;
+    uint8_t     index; // Target ID
+    uint8_t     rcs; // The section of radar reflection
+    uint8_t     rangeH; // Target distance high 8 bi
+    uint8_t     rangeL; // Target distance low 8 bit
+    uint8_t     rsvd1;
+    uint8_t     info; // VrelH Rsvd1 RollCount
+    uint8_t     vrelL;
+    uint8_t     SNR; // Signal-Noise Ratio
+    uint8_t     end0;
+    uint8_t     end1;
+} nanoradarPacket_t;
+
+#define NANORADAR_PACKET_SIZE    sizeof(nanoradarPacket_t)
+#define NANORADAR_TIMEOUT_MS     2000 // 2s
+
+static bool hasNewData = false;
+static bool hasEverData = false;
+static serialPort_t * serialPort = NULL;
+static serialPortConfig_t * portConfig;
+static uint8_t buffer[NANORADAR_PACKET_SIZE];
+static unsigned bufferPtr;
+
+static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
+static timeMs_t lastProtocolActivityMs;
+
+static bool nanoradarRangefinderDetect(void)
+{
+    portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
+    if (!portConfig) {
+        return false;
+    }
+
+    return true;
+}
+
+static void nanoradarRangefinderInit(void)
+{
+    if (!portConfig) {
+        return;
+    }
+
+    serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
+    if (!serialPort) {
+        return;
+    }
+
+    lastProtocolActivityMs = 0;
+}
+
+static void nanoradarRangefinderUpdate(void)
+{
+
+    nanoradarPacket_t *nanoradarPacket = (nanoradarPacket_t *)buffer;
+    while (serialRxBytesWaiting(serialPort) > 0) {
+        uint8_t c = serialRead(serialPort);
+
+        if (bufferPtr < NANORADAR_PACKET_SIZE) {
+            buffer[bufferPtr++] = c;
+        }
+
+        if ((bufferPtr == 1) && (nanoradarPacket->header0 != NANORADAR_HDR)) {
+            bufferPtr = 0;
+            continue;
+        }
+
+        if ((bufferPtr == 2) && (nanoradarPacket->header1 != NANORADAR_HDR)) {
+            bufferPtr = 0;
+            continue;
+        }
+
+        //only target info packet we are interested
+        if (bufferPtr == 4) {
+            uint16_t command = nanoradarPacket->commandH + (nanoradarPacket->commandL * 0x100);
+
+            if (command != NANORADAR_COMMAND_TARGET_INFO) {
+                bufferPtr = 0;
+                continue;
+            }
+        }
+
+        // Check for complete packet
+        if (bufferPtr == NANORADAR_PACKET_SIZE) {
+            if (nanoradarPacket->end0 == NANORADAR_END && nanoradarPacket->end1 == NANORADAR_END) {
+                // Valid packet
+                hasNewData = true;
+                hasEverData = true;
+                lastProtocolActivityMs = millis();
+
+                sensorData = ((nanoradarPacket->rangeH * 0x100) + nanoradarPacket->rangeL);
+            }
+
+            // Prepare for new packet
+            bufferPtr = 0;
+        }
+    }
+}
+
+static int32_t nanoradarRangefinderGetDistance(void)
+{
+    int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
+
+    if (hasNewData) {
+        hasNewData = false;
+        return altitude;
+    }
+    else {
+        //keep last value for timeout, because radar sends data only if change
+        if ((millis() - lastProtocolActivityMs) < NANORADAR_TIMEOUT_MS) {
+            return altitude;
+        }
+
+        return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
+    }
+}
+
+virtualRangefinderVTable_t rangefinderNanoradarVtable = {
+    .detect = nanoradarRangefinderDetect,
+    .init = nanoradarRangefinderInit,
+    .update = nanoradarRangefinderUpdate,
+    .read = nanoradarRangefinderGetDistance
+};
+
+#endif

src/main/io/rangefinder_usd1_v0.c

@@ -0,0 +1,136 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it 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.
+ *
+ * This file 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 program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <ctype.h>
+
+
+#include "platform.h"
+#include "io/serial.h"
+#include "drivers/time.h"
+
+#if defined(USE_RANGEFINDER_USD1_V0)
+#include "drivers/rangefinder/rangefinder_virtual.h"
+
+#define USD1_HDR_V0 72 // Header Byte for beta V0 of USD1_Serial (0x48)
+
+#define USD1_PACKET_SIZE 3
+#define USD1_KEEP_DATA_TIMEOUT 2000 // 2s
+
+
+static serialPort_t * serialPort = NULL;
+static serialPortConfig_t * portConfig;
+
+static bool hasNewData = false;
+static bool hasEverData = false;
+static uint8_t  lineBuf[USD1_PACKET_SIZE];
+static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
+static timeMs_t lastProtocolActivityMs;
+
+static bool usd1RangefinderDetect(void)
+{
+    portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
+    if (!portConfig) {
+        return false;
+    }
+
+    return true;
+}
+
+static void usd1RangefinderInit(void)
+{
+    if (!portConfig) {
+        return;
+    }
+
+    serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
+    if (!serialPort) {
+        return;
+    }
+
+    lastProtocolActivityMs = 0;
+}
+
+static void usd1RangefinderUpdate(void)
+{
+    float sum = 0;
+    uint16_t count = 0;
+    uint8_t  index = 0;
+
+    while (serialRxBytesWaiting(serialPort) > 0) {
+        uint8_t c = serialRead(serialPort);
+
+        if (c == USD1_HDR_V0 && index == 0) {
+            lineBuf[index] = c;
+            index = 1;
+            continue;
+        }
+
+        if (index > 0) {
+            lineBuf[index] = c;
+            index++;
+            if (index == 3) {
+                index = 0;
+                sum += (float)((lineBuf[2]&0x7F) * 128 + (lineBuf[1]&0x7F));
+                count++;
+            }
+        }
+    }
+
+    if (count == 0) {
+        return;
+    }
+
+    hasNewData = true;
+    hasEverData = true;
+    lastProtocolActivityMs = millis();
+    sensorData = (int32_t)(2.5f * sum / (float)count);
+}
+
+static int32_t usd1RangefinderGetDistance(void)
+{
+    int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
+
+    if (hasNewData) {
+        hasNewData = false;
+        return altitude;
+    }
+    else {
+        //keep last value for timeout, because radar sends data only if change
+        if ((millis() - lastProtocolActivityMs) < USD1_KEEP_DATA_TIMEOUT) {
+            return altitude;
+        }
+
+        return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
+    }
+}
+
+virtualRangefinderVTable_t rangefinderUSD1Vtable = {
+    .detect = usd1RangefinderDetect,
+    .init = usd1RangefinderInit,
+    .update = usd1RangefinderUpdate,
+    .read = usd1RangefinderGetDistance
+};
+
+#endif

src/main/sensors/rangefinder.c

@@ -130,6 +130,22 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
                 rangefinderHardware = RANGEFINDER_BENEWAKE;
                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
             }
+#endif
+            break;
+        case RANGEFINDER_USD1_V0:
+#if defined(USE_RANGEFINDER_USD1_V0)
+            if (virtualRangefinderDetect(dev, &rangefinderUSD1Vtable)) {
+                rangefinderHardware = RANGEFINDER_USD1_V0;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
+            }
+#endif
+            break;
+        case RANGEFINDER_NANORADAR:
+#if defined(USE_RANGEFINDER_NANORADAR)
+            if (virtualRangefinderDetect(dev, &rangefinderNanoradarVtable)) {
+                rangefinderHardware = RANGEFINDER_NANORADAR;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
+            }
 #endif
             break;
 

src/main/sensors/rangefinder.h

@@ -32,6 +32,8 @@ typedef enum {
     RANGEFINDER_TOF10102I2C         = 7,
     RANGEFINDER_FAKE                = 8,
     RANGEFINDER_TERARANGER_EVO      = 9,
+    RANGEFINDER_USD1_V0             = 10,
+    RANGEFINDER_NANORADAR           = 11,
 } rangefinderType_e;
 
 typedef struct rangefinderConfig_s {

src/main/target/common.h

@@ -84,6 +84,8 @@
 #define USE_RANGEFINDER_US42
 #define USE_RANGEFINDER_TOF10120_I2C
 #define USE_RANGEFINDER_TERARANGER_EVO_I2C
+#define USE_RANGEFINDER_USD1_V0
+#define USE_RANGEFINDER_NANORADAR
 
 // Allow default optic flow boards
 #define USE_OPFLOW