Skip to content

Commit 80e95f2

Browse files
qqqlabqqqlab
committed
add RDR module
1 parent 0d0a674 commit 80e95f2

File tree

9 files changed

+347
-10
lines changed

9 files changed

+347
-10
lines changed

‎examples/00.HelloWorld/00.HelloWorld.ino‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,8 @@ void loop() {
2424
mag.update(); // magnetometer
2525
gps.update(); // gps
2626
bat.update(); // battery consumption
27-
cli.update(); //process CLI commands
27+
rdr.update(); // radar
28+
cli.update(); // process CLI commands
2829
}
2930

3031
//This is __MAIN__ function of this program. It is called when new IMU data is available.

‎src/madflight.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@ const char* madflight_config = MADFLIGHT_BOARD MADFLIGHT_CONFIG;
6161
#include "madflight/mag/mag.h"
6262
#include "madflight/out/out.h"
6363
#include "madflight/pid/pid.h"
64+
#include "madflight/rdr/rdr.h"
6465
#include "madflight/veh/veh.h"
6566

6667
// toolbox
@@ -169,6 +170,13 @@ void madflight_setup() {
169170
bat.config.rshunt = cfg.bat_cal_i;
170171
bat.setup();
171172

173+
//RDR
174+
rdr.config.gizmo = (Cfg::rdr_gizmo_enum)cfg.rdr_gizmo; //the gizmo to use
175+
rdr.config.ser_bus_id = cfg.rdr_ser_bus; //serial bus
176+
rdr.config.baud = cfg.rdr_baud; //baud rate
177+
rdr.setup();
178+
179+
172180
// GPS
173181
gps.config.gizmo = (Cfg::gps_gizmo_enum)cfg.gps_gizmo; //the gizmo to use
174182
gps.config.ser_bus = hal_get_ser_bus(cfg.gps_ser_bus); //serial bus

‎src/madflight/cfg/cfg.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -221,7 +221,7 @@ SOFTWARE.
221221
MF_PARAM( mag_lp, 1e10, float, 'f') /*Magnetometer Gyro Low Pass Filter cutoff frequency in Hz (default 1e10Hz, i.e. no filtering) */ \
222222
\
223223
/*RDR - Radar*/ \
224-
MF_PARAM( rdr_gizmo, 0, int32_t, 'e', mf_NONE,mf_LM2411S,mf_LM2413) \
224+
MF_PARAM( rdr_gizmo, 0, int32_t, 'e', mf_NONE,mf_LD2411S,mf_LD2413) \
225225
MF_PARAM( rdr_ser_bus, -1, int32_t, 'i') \
226226
MF_PARAM( rdr_baud, 0, int32_t, 'i') \
227227
\

‎src/madflight/cli/cli_cpp.h‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@ SOFTWARE.
4343
#include "../out/out.h"
4444
#include "../pid/pid.h"
4545
#include "../rcl/rcl.h"
46+
#include "../rdr/rdr.h"
4647
#include "../veh/veh.h"
4748

4849
#include "cli_RclCalibrate.h"
@@ -191,13 +192,17 @@ static void cli_print_alt() {
191192
Serial.printf("ahr.aup:%.2f\t", ahr.getAccelUp());
192193
}
193194

195+
void cli_print_rdr() {
196+
Serial.printf("rdr.dist:%d\t", rdr.dist);
197+
}
198+
194199
struct cli_print_s {
195200
const char *cmd;
196201
const char *info;
197202
void (*function)(void);
198203
};
199204

200-
#define CLI_PRINT_FLAG_COUNT 14
205+
#define CLI_PRINT_FLAG_COUNT 15
201206
bool cli_print_flag[CLI_PRINT_FLAG_COUNT] = {false};
202207

203208
static const struct cli_print_s cli_print_options[] = {
@@ -215,6 +220,7 @@ static const struct cli_print_s cli_print_options[] = {
215220
{"pbar", "Barometer", cli_print_bar},
216221
{"palt", "Altitude estimator", cli_print_alt},
217222
{"pgps", "GPS", cli_print_gps},
223+
{"prdr", "Radar", cli_print_rdr},
218224
};
219225

220226

‎src/madflight/hal/RP2040/hal_RP2040_cpp.h‎

Lines changed: 25 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@
3939
//Bus Setup
4040
//-------------------------------------
4141

42-
#define HAL_SER_NUM 2
42+
#define HAL_SER_NUM 6
4343
#define HAL_I2C_NUM 2
4444
#define HAL_SPI_NUM 2
4545

@@ -199,25 +199,37 @@ MF_Serial* hal_get_ser_bus(int bus_id, int baud, MF_SerialMode mode, bool invert
199199
pin_tx = cfg.pin_ser0_tx;
200200
pin_rx = cfg.pin_ser0_rx;
201201
uart = uart0;
202-
strcpy(taskname, "uart0");
202+
strcpy(taskname, "ser0");
203203
break;
204204
case 1:
205205
pin_tx = cfg.pin_ser1_tx;
206206
pin_rx = cfg.pin_ser1_rx;
207207
uart = uart1;
208-
strcpy(taskname, "uart1");
208+
strcpy(taskname, "ser1");
209+
break;
210+
case 2:
211+
pin_tx = cfg.pin_ser2_tx;
212+
pin_rx = cfg.pin_ser2_rx;
213+
uart = nullptr;
214+
strcpy(taskname, "ser2");
209215
break;
210216
case 3:
211217
pin_tx = cfg.pin_ser3_tx;
212218
pin_rx = cfg.pin_ser3_rx;
213219
uart = nullptr;
214-
strcpy(taskname, "uart3");
220+
strcpy(taskname, "ser3");
215221
break;
216222
case 4:
217223
pin_tx = cfg.pin_ser4_tx;
218224
pin_rx = cfg.pin_ser4_rx;
219225
uart = nullptr;
220-
strcpy(taskname, "uart4");
226+
strcpy(taskname, "ser4");
227+
break;
228+
case 5:
229+
pin_tx = cfg.pin_ser5_tx;
230+
pin_rx = cfg.pin_ser5_rx;
231+
uart = nullptr;
232+
strcpy(taskname, "ser5");
221233
break;
222234
default:
223235
return nullptr;
@@ -229,7 +241,10 @@ MF_Serial* hal_get_ser_bus(int bus_id, int baud, MF_SerialMode mode, bool invert
229241
if(uart) {
230242
//create new wrapped SerialUART
231243
if(!hal_ser[bus_id]) {
232-
SerialUART *ser = new SerialUART(uart, pin_rx, pin_tx);
244+
#ifdef MF_DEBUG
245+
Serial.printf("Creating MF_SerialUART for bus %d\n",bus_id);
246+
#endif
247+
SerialUART *ser = new SerialUART(uart, pin_tx, pin_rx); //uart_inst_t *uart, pin_size_t tx, pin_size_t rx, pin_size_t rts = UART_PIN_NOT_DEFINED, pin_size_t cts = UART_PIN_NOT_DEFINED
233248
hal_ser[bus_id] = new MF_SerialUART(ser, taskname);
234249
}
235250

@@ -245,7 +260,10 @@ MF_Serial* hal_get_ser_bus(int bus_id, int baud, MF_SerialMode mode, bool invert
245260
}else{
246261
//create new wrapped SerialPIO
247262
if(!hal_ser[bus_id]) {
248-
SerialPIO *ser = new SerialPIO(pin_rx, pin_tx, 256);
263+
#ifdef MF_DEBUG
264+
Serial.printf("Creating MF_SerialPIO for bus %d\n",bus_id);
265+
#endif
266+
SerialPIO *ser = new SerialPIO(pin_tx, pin_rx, 256); //pin_size_t tx, pin_size_t rx, size_t fifoSize = 32
249267
hal_ser[bus_id] = new MF_SerialPIO(ser, taskname);
250268
}
251269

‎src/madflight/rdr/RdrGizmoLD2411S.h‎

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
// HKL-LD2411S driver
2+
// default baud rate 256000
3+
// default reporting rate: ???
4+
// reported data: AA AA tt xx xx 55 55
5+
// where xx xx is a LE int16 with distance in cm
6+
// and tt is 00=no data, 01="campaign target", 02="mircomotion target"
7+
8+
#pragma once
9+
10+
#include "rdr.h"
11+
#include "../hal/MF_Serial.h"
12+
#include "../hal/hal.h"
13+
14+
class RdrGizmoLD2411S: public RdrGizmo {
15+
private:
16+
int *dist = nullptr;
17+
MF_Serial* ser_bus = nullptr;
18+
19+
int pos = 0;
20+
union{
21+
uint8_t b[2];
22+
int16_t val;
23+
} data;
24+
25+
RdrGizmoLD2411S() {} //private constructor
26+
27+
public:
28+
static RdrGizmoLD2411S* create(int* dist, int ser_bus_id, int baud) {
29+
//get serial bus
30+
if(baud == 0) baud = 256000;
31+
MF_Serial* ser_bus = hal_get_ser_bus(ser_bus_id, baud);
32+
if(!ser_bus) return nullptr;
33+
34+
//setup gizmo
35+
auto gizmo = new RdrGizmoLD2411S();
36+
gizmo->ser_bus = ser_bus;
37+
gizmo->dist = dist;
38+
gizmo->pos = 0;
39+
return gizmo;
40+
}
41+
42+
bool update() override {
43+
bool rv = false;
44+
int n = ser_bus->available();
45+
for(int i=0;i<n;i++) {
46+
uint8_t b;
47+
ser_bus->read(&b,1);
48+
switch(pos) {
49+
case 0:
50+
if(b==0xAA) pos++; else pos=0;
51+
break;
52+
case 1:
53+
if(b==0xAA) pos++; else pos=0;
54+
break;
55+
case 2:
56+
//00=no data, 01="campaign target", 02="mircomotion target"
57+
pos++;
58+
break;
59+
case 3:
60+
case 4:
61+
data.b[pos-3] = b;
62+
pos++;
63+
break;
64+
case 5:
65+
if(b==0x55) pos++; else pos=0;
66+
break;
67+
case 6:
68+
if(b==0x55) {
69+
*dist = data.val * 10;
70+
rv = true;
71+
}
72+
pos = 0;
73+
break;
74+
default:
75+
pos = 0;
76+
}
77+
//Serial.printf("%02X ",b);
78+
}
79+
return rv;
80+
}
81+
};

‎src/madflight/rdr/RdrGizmoLD2413.h‎

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
// HKL-LD2413 driver
2+
// default baud rate 115200
3+
// default reporting rate: 160ms
4+
// reported data: F4 F3 F2 F1 04 00 xx xx xx xx F8 F7 F6 F5
5+
// where xx xx xx xx is a LE float with distance in mm
6+
7+
#pragma once
8+
9+
#include "rdr.h"
10+
#include "../hal/MF_Serial.h"
11+
#include "../hal/hal.h"
12+
13+
class RdrGizmoLD2413: public RdrGizmo {
14+
private:
15+
int *dist = nullptr;
16+
MF_Serial* ser_bus = nullptr;
17+
18+
int pos = 0;
19+
union{
20+
uint8_t b[4];
21+
float f;
22+
} data;
23+
24+
RdrGizmoLD2413() {} //private constructor
25+
26+
public:
27+
static RdrGizmoLD2413* create(int* dist, int ser_bus_id, int baud) {
28+
//get serial bus
29+
if(baud == 0) baud = 115200;
30+
MF_Serial* ser_bus = hal_get_ser_bus(ser_bus_id, baud);
31+
if(!ser_bus) return nullptr;
32+
33+
//setup gizmo
34+
auto gizmo = new RdrGizmoLD2413();
35+
gizmo->ser_bus = ser_bus;
36+
gizmo->dist = dist;
37+
gizmo->pos = 0;
38+
return gizmo;
39+
}
40+
41+
bool update() override {
42+
bool rv = false;
43+
int n = ser_bus->available();
44+
for(int i=0;i<n;i++) {
45+
uint8_t b;
46+
ser_bus->read(&b,1);
47+
switch(pos) {
48+
case 0:
49+
if(b==0xF4) pos++; else pos=0;
50+
break;
51+
case 1:
52+
if(b==0xF3) pos++; else pos=0;
53+
break;
54+
case 2:
55+
if(b==0xF2) pos++; else pos=0;
56+
break;
57+
case 3:
58+
if(b==0xF1) pos++; else pos=0;
59+
break;
60+
case 4:
61+
if(b==0x04) pos++; else pos=0;
62+
break;
63+
case 5:
64+
if(b==0x00) pos++; else pos=0;
65+
break;
66+
case 6:
67+
case 7:
68+
case 8:
69+
case 9:
70+
data.b[pos-6] = b;
71+
pos++;
72+
break;
73+
case 10:
74+
if(b==0xF8) pos++; else pos=0;
75+
break;
76+
case 11:
77+
if(b==0xF7) pos++; else pos=0;
78+
break;
79+
case 12:
80+
if(b==0xF6) pos++; else pos=0;
81+
break;
82+
case 13:
83+
if(b==0xF5) {
84+
*dist = data.f;
85+
//Serial.println(data.f);
86+
rv = true;
87+
}
88+
pos = 0;
89+
break;
90+
default:
91+
pos = 0;
92+
}
93+
//Serial.printf("%02X ",b);
94+
}
95+
return rv;
96+
}
97+
};

0 commit comments

Comments
 (0)