Skip to content

Commit 52564d6

Browse files
mmoscammosca
authored
Merge pull request #9842 from error414/NRA15
RangerFinder NRA15/24 from nanoradar
2 parents b7cae2e + 09ed11c commit 52564d6

9 files changed

+345
-1
lines changed

‎docs/Rangefinder.md

+16
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,22 @@ Following rangefinders are supported:
2121
* MSP - experimental
2222
* TOF10120 - small & lightweight laser range sensor, usable up to 200cm
2323
* TERARANGER EVO - 30cm to 600cm, depends on version https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
24+
* NRA15/NRA24 - experimental, UART version
25+
26+
#### NRA15/NRA24
27+
NRA15/NRA24 from nanoradar use US-D1_V0 or NRA protocol, it depends which firmware you use. Radar can be set by firmware
28+
to two different resolutions. See table below.
29+
30+
| Radar | Protocol | Resolution | Name in configurator |
31+
|-------|----------|-----------------|----------------------|
32+
| NRA15 | US-D1_V0 | 0-30m (+-4cm) | USD1_V0 |
33+
| NRA15 | NRA | 0-30m (+-4cm) | NRA |
34+
| NRA15 | NRA | 0-100m (+-10cm) | NRA |
35+
| NRA24 | US-D1_V0 | 0-50m (+-4cm) | USD1_V0 |
36+
| NRA24 | US-D1_V0 | 0-200m (+-10cm) | USD1_V0 |
37+
| NRA24 | NRA | 0-50m (+-4cm) | NRA |
38+
| NRA24 | NRA | 0-200m (+-10cm) | NRA |
39+
2440

2541
## Connections
2642

‎src/main/CMakeLists.txt

+3
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
12
main_sources(COMMON_SRC
23
main.c
34

@@ -485,6 +486,8 @@ main_sources(COMMON_SRC
485486
io/rangefinder.h
486487
io/rangefinder_msp.c
487488
io/rangefinder_benewake.c
489+
io/rangefinder_usd1_v0.c
490+
io/rangefinder_nanoradar.c
488491
io/rangefinder_fake.c
489492
io/opflow.h
490493
io/opflow_cxof.c

‎src/main/fc/settings.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@ tables:
55
values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "FAKE"]
66
enum: accelerationSensor_e
77
- name: rangefinder_hardware
8-
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO"]
8+
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"]
99
enum: rangefinderType_e
1010
- name: mag_hardware
1111
values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]

‎src/main/io/rangefinder.h

+2
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,8 @@
3131

3232
extern virtualRangefinderVTable_t rangefinderMSPVtable;
3333
extern virtualRangefinderVTable_t rangefinderBenewakeVtable;
34+
extern virtualRangefinderVTable_t rangefinderUSD1Vtable;
35+
extern virtualRangefinderVTable_t rangefinderNanoradarVtable; //NRA15/NRA24
3436
extern virtualRangefinderVTable_t rangefinderFakeVtable;
3537

3638
void mspRangefinderReceiveNewData(uint8_t * bufferPtr);

‎src/main/io/rangefinder_nanoradar.c

+167
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,167 @@
1+
/*
2+
* This file is part of INAV Project.
3+
*
4+
* This Source Code Form is subject to the terms of the Mozilla Public
5+
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
6+
* You can obtain one at http://mozilla.org/MPL/2.0/.
7+
*
8+
* Alternatively, the contents of this file may be used under the terms
9+
* of the GNU General Public License Version 3, as described below:
10+
*
11+
* This file is free software: you may copy, redistribute and/or modify
12+
* it under the terms of the GNU General Public License as published by the
13+
* Free Software Foundation, either version 3 of the License, or (at your
14+
* option) any later version.
15+
*
16+
* This file is distributed in the hope that it will be useful, but
17+
* WITHOUT ANY WARRANTY; without even the implied warranty of
18+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
19+
* Public License for more details.
20+
*
21+
* You should have received a copy of the GNU General Public License
22+
* along with this program. If not, see http://www.gnu.org/licenses/.
23+
*/
24+
25+
#include <stdbool.h>
26+
#include <ctype.h>
27+
28+
#include "platform.h"
29+
#include "io/serial.h"
30+
#include "drivers/time.h"
31+
32+
#if defined(USE_RANGEFINDER_NANORADAR)
33+
#include "drivers/rangefinder/rangefinder_virtual.h"
34+
35+
#define NANORADAR_HDR 0xAA // Header Byte
36+
#define NANORADAR_END 0x55
37+
38+
#define NANORADAR_COMMAND_TARGET_INFO 0x70C
39+
40+
typedef struct __attribute__((packed)) {
41+
uint8_t header0;
42+
uint8_t header1;
43+
uint8_t commandH;
44+
uint8_t commandL;
45+
uint8_t index; // Target ID
46+
uint8_t rcs; // The section of radar reflection
47+
uint8_t rangeH; // Target distance high 8 bi
48+
uint8_t rangeL; // Target distance low 8 bit
49+
uint8_t rsvd1;
50+
uint8_t info; // VrelH Rsvd1 RollCount
51+
uint8_t vrelL;
52+
uint8_t SNR; // Signal-Noise Ratio
53+
uint8_t end0;
54+
uint8_t end1;
55+
} nanoradarPacket_t;
56+
57+
#define NANORADAR_PACKET_SIZE sizeof(nanoradarPacket_t)
58+
#define NANORADAR_TIMEOUT_MS 2000 // 2s
59+
60+
static bool hasNewData = false;
61+
static bool hasEverData = false;
62+
static serialPort_t * serialPort = NULL;
63+
static serialPortConfig_t * portConfig;
64+
static uint8_t buffer[NANORADAR_PACKET_SIZE];
65+
static unsigned bufferPtr;
66+
67+
static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
68+
static timeMs_t lastProtocolActivityMs;
69+
70+
static bool nanoradarRangefinderDetect(void)
71+
{
72+
portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
73+
if (!portConfig) {
74+
return false;
75+
}
76+
77+
return true;
78+
}
79+
80+
static void nanoradarRangefinderInit(void)
81+
{
82+
if (!portConfig) {
83+
return;
84+
}
85+
86+
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
87+
if (!serialPort) {
88+
return;
89+
}
90+
91+
lastProtocolActivityMs = 0;
92+
}
93+
94+
static void nanoradarRangefinderUpdate(void)
95+
{
96+
97+
nanoradarPacket_t *nanoradarPacket = (nanoradarPacket_t *)buffer;
98+
while (serialRxBytesWaiting(serialPort) > 0) {
99+
uint8_t c = serialRead(serialPort);
100+
101+
if (bufferPtr < NANORADAR_PACKET_SIZE) {
102+
buffer[bufferPtr++] = c;
103+
}
104+
105+
if ((bufferPtr == 1) && (nanoradarPacket->header0 != NANORADAR_HDR)) {
106+
bufferPtr = 0;
107+
continue;
108+
}
109+
110+
if ((bufferPtr == 2) && (nanoradarPacket->header1 != NANORADAR_HDR)) {
111+
bufferPtr = 0;
112+
continue;
113+
}
114+
115+
//only target info packet we are interested
116+
if (bufferPtr == 4) {
117+
uint16_t command = nanoradarPacket->commandH + (nanoradarPacket->commandL * 0x100);
118+
119+
if (command != NANORADAR_COMMAND_TARGET_INFO) {
120+
bufferPtr = 0;
121+
continue;
122+
}
123+
}
124+
125+
// Check for complete packet
126+
if (bufferPtr == NANORADAR_PACKET_SIZE) {
127+
if (nanoradarPacket->end0 == NANORADAR_END && nanoradarPacket->end1 == NANORADAR_END) {
128+
// Valid packet
129+
hasNewData = true;
130+
hasEverData = true;
131+
lastProtocolActivityMs = millis();
132+
133+
sensorData = ((nanoradarPacket->rangeH * 0x100) + nanoradarPacket->rangeL);
134+
}
135+
136+
// Prepare for new packet
137+
bufferPtr = 0;
138+
}
139+
}
140+
}
141+
142+
static int32_t nanoradarRangefinderGetDistance(void)
143+
{
144+
int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
145+
146+
if (hasNewData) {
147+
hasNewData = false;
148+
return altitude;
149+
}
150+
else {
151+
//keep last value for timeout, because radar sends data only if change
152+
if ((millis() - lastProtocolActivityMs) < NANORADAR_TIMEOUT_MS) {
153+
return altitude;
154+
}
155+
156+
return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
157+
}
158+
}
159+
160+
virtualRangefinderVTable_t rangefinderNanoradarVtable = {
161+
.detect = nanoradarRangefinderDetect,
162+
.init = nanoradarRangefinderInit,
163+
.update = nanoradarRangefinderUpdate,
164+
.read = nanoradarRangefinderGetDistance
165+
};
166+
167+
#endif

‎src/main/io/rangefinder_usd1_v0.c

+136
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,136 @@
1+
/*
2+
* This file is part of INAV Project.
3+
*
4+
* This Source Code Form is subject to the terms of the Mozilla Public
5+
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
6+
* You can obtain one at http://mozilla.org/MPL/2.0/.
7+
*
8+
* Alternatively, the contents of this file may be used under the terms
9+
* of the GNU General Public License Version 3, as described below:
10+
*
11+
* This file is free software: you may copy, redistribute and/or modify
12+
* it under the terms of the GNU General Public License as published by the
13+
* Free Software Foundation, either version 3 of the License, or (at your
14+
* option) any later version.
15+
*
16+
* This file is distributed in the hope that it will be useful, but
17+
* WITHOUT ANY WARRANTY; without even the implied warranty of
18+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
19+
* Public License for more details.
20+
*
21+
* You should have received a copy of the GNU General Public License
22+
* along with this program. If not, see http://www.gnu.org/licenses/.
23+
*/
24+
25+
#include <stdbool.h>
26+
#include <ctype.h>
27+
28+
29+
#include "platform.h"
30+
#include "io/serial.h"
31+
#include "drivers/time.h"
32+
33+
#if defined(USE_RANGEFINDER_USD1_V0)
34+
#include "drivers/rangefinder/rangefinder_virtual.h"
35+
36+
#define USD1_HDR_V0 72 // Header Byte for beta V0 of USD1_Serial (0x48)
37+
38+
#define USD1_PACKET_SIZE 3
39+
#define USD1_KEEP_DATA_TIMEOUT 2000 // 2s
40+
41+
42+
static serialPort_t * serialPort = NULL;
43+
static serialPortConfig_t * portConfig;
44+
45+
static bool hasNewData = false;
46+
static bool hasEverData = false;
47+
static uint8_t lineBuf[USD1_PACKET_SIZE];
48+
static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
49+
static timeMs_t lastProtocolActivityMs;
50+
51+
static bool usd1RangefinderDetect(void)
52+
{
53+
portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
54+
if (!portConfig) {
55+
return false;
56+
}
57+
58+
return true;
59+
}
60+
61+
static void usd1RangefinderInit(void)
62+
{
63+
if (!portConfig) {
64+
return;
65+
}
66+
67+
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
68+
if (!serialPort) {
69+
return;
70+
}
71+
72+
lastProtocolActivityMs = 0;
73+
}
74+
75+
static void usd1RangefinderUpdate(void)
76+
{
77+
float sum = 0;
78+
uint16_t count = 0;
79+
uint8_t index = 0;
80+
81+
while (serialRxBytesWaiting(serialPort) > 0) {
82+
uint8_t c = serialRead(serialPort);
83+
84+
if (c == USD1_HDR_V0 && index == 0) {
85+
lineBuf[index] = c;
86+
index = 1;
87+
continue;
88+
}
89+
90+
if (index > 0) {
91+
lineBuf[index] = c;
92+
index++;
93+
if (index == 3) {
94+
index = 0;
95+
sum += (float)((lineBuf[2]&0x7F) * 128 + (lineBuf[1]&0x7F));
96+
count++;
97+
}
98+
}
99+
}
100+
101+
if (count == 0) {
102+
return;
103+
}
104+
105+
hasNewData = true;
106+
hasEverData = true;
107+
lastProtocolActivityMs = millis();
108+
sensorData = (int32_t)(2.5f * sum / (float)count);
109+
}
110+
111+
static int32_t usd1RangefinderGetDistance(void)
112+
{
113+
int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
114+
115+
if (hasNewData) {
116+
hasNewData = false;
117+
return altitude;
118+
}
119+
else {
120+
//keep last value for timeout, because radar sends data only if change
121+
if ((millis() - lastProtocolActivityMs) < USD1_KEEP_DATA_TIMEOUT) {
122+
return altitude;
123+
}
124+
125+
return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
126+
}
127+
}
128+
129+
virtualRangefinderVTable_t rangefinderUSD1Vtable = {
130+
.detect = usd1RangefinderDetect,
131+
.init = usd1RangefinderInit,
132+
.update = usd1RangefinderUpdate,
133+
.read = usd1RangefinderGetDistance
134+
};
135+
136+
#endif

‎src/main/sensors/rangefinder.c

+16
Original file line numberDiff line numberDiff line change
@@ -130,6 +130,22 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
130130
rangefinderHardware = RANGEFINDER_BENEWAKE;
131131
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
132132
}
133+
#endif
134+
break;
135+
case RANGEFINDER_USD1_V0:
136+
#if defined(USE_RANGEFINDER_USD1_V0)
137+
if (virtualRangefinderDetect(dev, &rangefinderUSD1Vtable)) {
138+
rangefinderHardware = RANGEFINDER_USD1_V0;
139+
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
140+
}
141+
#endif
142+
break;
143+
case RANGEFINDER_NANORADAR:
144+
#if defined(USE_RANGEFINDER_NANORADAR)
145+
if (virtualRangefinderDetect(dev, &rangefinderNanoradarVtable)) {
146+
rangefinderHardware = RANGEFINDER_NANORADAR;
147+
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
148+
}
133149
#endif
134150
break;
135151

‎src/main/sensors/rangefinder.h

+2
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,8 @@ typedef enum {
3232
RANGEFINDER_TOF10102I2C = 7,
3333
RANGEFINDER_FAKE = 8,
3434
RANGEFINDER_TERARANGER_EVO = 9,
35+
RANGEFINDER_USD1_V0 = 10,
36+
RANGEFINDER_NANORADAR = 11,
3537
} rangefinderType_e;
3638

3739
typedef struct rangefinderConfig_s {

0 commit comments

Comments
 (0)