Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
mateksys_3901_l0x.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2020 Paparazzi Team
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 *
21 */
22
27
28#include <stdlib.h>
29#include "mateksys_3901_l0x.h"
30#include "mcu_periph/uart.h"
31#include "modules/core/abi.h"
32
33// State interface for rotation compensation
34#include "state.h"
35
36// Messages
37#include "pprzlink/messages.h"
39
40
41// Define configuration parameters
42#ifndef MATEKSYS_3901_L0X_MOTION_THRES
43#define MATEKSYS_3901_L0X_MOTION_THRES 100
44#endif
45
46#ifndef MATEKSYS_3901_L0X_DISTANCE_THRES
47#define MATEKSYS_3901_L0X_DISTANCE_THRES 200
48#endif
49
50#ifndef MATEKSYS_3901_L0X_MAX_FLOW
51#define MATEKSYS_3901_L0X_MAX_FLOW 300
52#endif
53
54#ifndef MATEKSYS_3901_L0X_MAX_DISTANCE
55#define MATEKSYS_3901_L0X_MAX_DISTANCE 3000
56#endif
57
58#ifndef USE_MATEKSYS_3901_L0X_AGL
59#define USE_MATEKSYS_3901_L0X_AGL 1
60#endif
61
62#ifndef USE_MATEKSYS_3901_L0X_OPTICAL_FLOW
63#define USE_MATEKSYS_3901_L0X_OPTICAL_FLOW 1
64#endif
65
66#ifndef MATEKSYS_3901_L0X_COMPENSATE_ROTATION
67#define MATEKSYS_3901_L0X_COMPENSATE_ROTATION 1
68#endif
69
70#ifndef MATEKSYS_3901_L0X_FLOW_X_SCALER
71#define MATEKSYS_3901_L0X_FLOW_X_SCALER 1
72#endif
73
74#ifndef MATEKSYS_3901_L0X_FLOW_Y_SCALER
75#define MATEKSYS_3901_L0X_FLOW_Y_SCALER 1
76#endif
77
81
82static void mateksys3901l0x_parse(uint8_t byte);
83
84#if PERIODIC_TELEMETRY
86
90static void mateksys3901l0x_send_optical_flow(struct transport_tx *trans, struct link_device *dev)
91{
92 pprz_msg_send_OPTICAL_FLOW(trans, dev, AC_ID,
93 &mateksys3901l0x.time_usec,
94 &mateksys3901l0x.sensor_id,
95 &mateksys3901l0x.motionX,
96 &mateksys3901l0x.motionY,
97 &mateksys3901l0x.velocityX,
98 &mateksys3901l0x.velocityY,
99 &mateksys3901l0x.motion_quality,
100 &mateksys3901l0x.distancemm,
101 &mateksys3901l0x.distance_compensated,
102 &mateksys3901l0x.distancemm_quality);
103}
104
105#endif
106
111{
112 mateksys3901l0x.device = &((MATEKSYS_3901_L0X_PORT).device);
113 mateksys3901l0x.parse_crc = 0;
114 mateksys3901l0x.motion_quality = 0;
115 mateksys3901l0x.motionX_temp = 0;
116 mateksys3901l0x.motionX = 0;
117 mateksys3901l0x.motionY_temp = 0;
118 mateksys3901l0x.motionY = 0;
119 mateksys3901l0x.distancemm_quality = 0;
120 mateksys3901l0x.distancemm_temp = 0;
121 mateksys3901l0x.distancemm = 0;
122 mateksys3901l0x.distance_compensated = 0;
126
127#if PERIODIC_TELEMETRY
129#endif
130}
131
136{
137 mateksys3901l0x.scaler_x = scalex;
138}
139
144{
145 mateksys3901l0x.scaler_y = scaley;
146}
147
152{
153 while (mateksys3901l0x.parse_status != MATEKSYS_3901_L0X_INITIALIZE && mateksys3901l0x.device->char_available(mateksys3901l0x.device->periph)) {
154 mateksys3901l0x_parse(mateksys3901l0x.device->get_byte(mateksys3901l0x.device->periph));
155 }
156}
157
162{
163
164 switch (mateksys3901l0x.parse_status) {
166 break;
167
168 case MATEKSYS_3901_L0X_PARSE_HEAD: // MSP general header $
169 if (byte == 0x24) {
170 mateksys3901l0x.parse_status++;
171 }
172 break;
173
174 case MATEKSYS_3901_L0X_PARSE_HEAD2: // MSPv2 identifier X
175 if (byte == 0x58) {
176 mateksys3901l0x.parse_status++;
177 } else {
179 }
180 break;
181
182 case MATEKSYS_3901_L0X_PARSE_DIRECTION: // direction <
183 if (byte == 0x3C) {
184 mateksys3901l0x.parse_status++;
185 } else {
187 }
188 break;
189
190 case MATEKSYS_3901_L0X_PARSE_LENGTH: // set to 0
191 if (byte == 0x00) {
192 mateksys3901l0x.parse_crc += byte;
193 mateksys3901l0x.parse_status++;
194 } else {
196 }
197 break;
198
199 case MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1: // 0x01 = rangefinder; 0x02 = opticalflow
200 if (byte == 0x01 || byte == 0x02) {
201 mateksys3901l0x.sensor_id = byte;
202 mateksys3901l0x.parse_crc += byte;
203 mateksys3901l0x.parse_status++;
204 } else {
206 }
207 break;
208
209 case MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2: // sensor id pointer
210 if (byte == 0x1F) {
211 mateksys3901l0x.parse_status++;
212 mateksys3901l0x.parse_crc += byte;
213 } else {
215 }
216 break;
217
218 case MATEKSYS_3901_L0X_PARSE_SIZE: // two fixed sizes are expected if message is
219 if (byte == 0x05 || byte == 0x09) {
220 mateksys3901l0x.parse_status++;
221 mateksys3901l0x.parse_crc += byte;
222 } else {
224 }
225 break;
226
227 case MATEKSYS_3901_L0X_PARSE_POINTER: // should be zero, used to redirect to motion parsing or lidar parsing
228 if (mateksys3901l0x.sensor_id == 0x01) {
230 } else if (mateksys3901l0x.sensor_id == 0x02) {
232 } else {
234 }
235 break;
236
237 // rangefinder data parsing
239 mateksys3901l0x.distancemm_quality = byte;
240 mateksys3901l0x.parse_crc += byte;
241 mateksys3901l0x.parse_status++;
242 break;
243
245 if (mateksys3901l0x.distancemm_quality <= MATEKSYS_3901_L0X_DISTANCE_THRES) {
247 } else {
248 mateksys3901l0x.distancemm_temp = byte;
249 mateksys3901l0x.parse_crc += byte;
250 mateksys3901l0x.parse_status++;
251 }
252 break;
253
255 mateksys3901l0x.distancemm_temp |= (byte << 8);
256 mateksys3901l0x.parse_crc += byte;
257 mateksys3901l0x.parse_status++;
258 break;
259
261 mateksys3901l0x.distancemm_temp |= (byte << 16);
262 mateksys3901l0x.parse_crc += byte;
263 mateksys3901l0x.parse_status++;
264 break;
265
267 mateksys3901l0x.distancemm_temp |= (byte << 24);
268 mateksys3901l0x.parse_crc += byte;
270 break;
271
272 // optical flow data parsing
274 mateksys3901l0x.motion_quality = byte;
275 mateksys3901l0x.parse_crc += byte;
276 mateksys3901l0x.parse_status++;
277 break;
278
280 if (mateksys3901l0x.motion_quality <= MATEKSYS_3901_L0X_MOTION_THRES) {
282 } else {
283 mateksys3901l0x.motionY_temp = byte;
284 mateksys3901l0x.parse_crc += byte;
285 mateksys3901l0x.parse_status++;
286 }
287 break;
288
290 mateksys3901l0x.motionY_temp |= (byte << 8);
291 mateksys3901l0x.parse_crc += byte;
292 mateksys3901l0x.parse_status++;
293 break;
294
296 mateksys3901l0x.motionY_temp |= (byte << 16);
297 mateksys3901l0x.parse_crc += byte;
298 mateksys3901l0x.parse_status++;
299 break;
300
302 mateksys3901l0x.motionY_temp |= (byte << 24);
303 mateksys3901l0x.parse_crc += byte;
304 mateksys3901l0x.parse_status++;
305 break;
306
308 mateksys3901l0x.motionX_temp = byte;
309 mateksys3901l0x.parse_crc += byte;
310 mateksys3901l0x.parse_status++;
311 break;
312
314 mateksys3901l0x.motionX_temp |= (byte << 8);
315 mateksys3901l0x.parse_crc += byte;
316 mateksys3901l0x.parse_status++;
317 break;
318
320 mateksys3901l0x.motionX_temp |= (byte << 16);
321 mateksys3901l0x.parse_crc += byte;
322 mateksys3901l0x.parse_status++;
323 break;
324
326 mateksys3901l0x.motionX_temp |= (byte << 24);
327 mateksys3901l0x.parse_crc += byte;
328 mateksys3901l0x.parse_status++;
329 break;
330
332
333 // When the distance and motion info are valid (max values based on sensor specifications)...
334 if (mateksys3901l0x.distancemm_temp > 0 && mateksys3901l0x.distancemm_temp <= MATEKSYS_3901_L0X_MAX_DISTANCE && abs(mateksys3901l0x.motionX_temp) <= MATEKSYS_3901_L0X_MAX_FLOW && abs(mateksys3901l0x.motionY_temp) <= MATEKSYS_3901_L0X_MAX_FLOW) {
335
336 // pass temporary message and apply calibration parameters
337 mateksys3901l0x.motionX = mateksys3901l0x.motionX_temp * mateksys3901l0x.scaler_x;
338 mateksys3901l0x.motionY = mateksys3901l0x.motionY_temp * mateksys3901l0x.scaler_y;
339 mateksys3901l0x.distancemm = mateksys3901l0x.distancemm_temp;
340
341 // get from ground distance to altitude by compensating for body rotation
343
344 float phi = stateGetNedToBodyEulers_f()->phi;
345 float theta = stateGetNedToBodyEulers_f()->theta;
346 mateksys3901l0x.distance_compensated = ((mateksys3901l0x.distancemm * cos(phi)) * cos(theta)) * 0.001;
347
348 }
349
350 // estimate velocity and send it to telemetry (flow not compensated for gyro measurements)
351 mateksys3901l0x.velocityX = mateksys3901l0x.distance_compensated * sin(RadOfDeg(mateksys3901l0x.motionY)); // velocity in m/sec
352 mateksys3901l0x.velocityY = mateksys3901l0x.distance_compensated * sin(RadOfDeg(mateksys3901l0x.motionX)); // velocity in m/sec
353
354 // get ticks
356
357 // send AGL (if requested)
359 AbiSendMsgAGL(AGL_LIDAR_MATEKSYS_3901_L0X_ID,
360 mateksys3901l0x.time_usec,
361 mateksys3901l0x.distance_compensated);
362 }
363
364 // send optical flow (if requested)
366 AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID,
367 mateksys3901l0x.time_usec,
368 mateksys3901l0x.motionX, // motion in deg/sec
369 mateksys3901l0x.motionY, // motion in deg/sec
370 0,
371 0,
372 mateksys3901l0x.motion_quality,
373 0.0);
374 }
375 }
376 // Start reading again
378 break;
379
380 default:
381 // Error, return to start
383 break;
384
385 }
386}
Main include for ABI (AirBorneInterface).
#define AGL_LIDAR_MATEKSYS_3901_L0X_ID
#define FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static struct uart_periph * dev
float phi
in radians
float theta
in radians
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
#define MATEKSYS_3901_L0X_FLOW_X_SCALER
#define USE_MATEKSYS_3901_L0X_AGL
#define MATEKSYS_3901_L0X_DISTANCE_THRES
void mateksys_3901_l0x_scale_X(float scalex)
Scale the Flow X.
void mateksys3901l0x_init(void)
Initialization function.
static void mateksys3901l0x_parse(uint8_t byte)
Parse the sensor MSP output bytes 1 by 1.
#define MATEKSYS_3901_L0X_MAX_DISTANCE
#define MATEKSYS_3901_L0X_MOTION_THRES
#define USE_MATEKSYS_3901_L0X_OPTICAL_FLOW
void mateksys3901l0x_event(void)
Receive bytes from the UART port and parse them.
#define MATEKSYS_3901_L0X_MAX_FLOW
#define MATEKSYS_3901_L0X_COMPENSATE_ROTATION
#define MATEKSYS_3901_L0X_FLOW_Y_SCALER
void mateksys_3901_l0x_scale_Y(float scaley)
Scale the Flow Y.
static void mateksys3901l0x_send_optical_flow(struct transport_tx *trans, struct link_device *dev)
Downlink message flow and lidar (included velocity estimation, not yet tested).
struct Mateksys3901l0X mateksys3901l0x
Driver for the mateksys_3901_l0x sensor via MSP protocol output.
@ MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY
@ MATEKSYS_3901_L0X_PARSE_POINTER
@ MATEKSYS_3901_L0X_PARSE_MOTIONX_B4
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B1
@ MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2
@ MATEKSYS_3901_L0X_PARSE_CHECKSUM
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B2
@ MATEKSYS_3901_L0X_PARSE_HEAD
@ MATEKSYS_3901_L0X_PARSE_MOTIONX_B2
@ MATEKSYS_3901_L0X_PARSE_MOTIONX_B3
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B3
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B3
@ MATEKSYS_3901_L0X_INITIALIZE
@ MATEKSYS_3901_L0X_PARSE_LENGTH
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B2
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B4
@ MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY
@ MATEKSYS_3901_L0X_PARSE_DIRECTION
@ MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1
@ MATEKSYS_3901_L0X_PARSE_SIZE
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B1
@ MATEKSYS_3901_L0X_PARSE_HEAD2
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B4
@ MATEKSYS_3901_L0X_PARSE_MOTIONX_B1
API to get/set the generic vehicle states.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.