Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
28 #include <stdlib.h>
29 #include "mateksys_3901_l0x.h"
30 #include "mcu_periph/uart.h"
31 #include "subsystems/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 
80 };
81 
83 
84 #if PERIODIC_TELEMETRY
86 
90 static 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;
123  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
124  mateksys3901l0x.scaler_x = MATEKSYS_3901_L0X_FLOW_X_SCALER;
125  mateksys3901l0x.scaler_y = MATEKSYS_3901_L0X_FLOW_Y_SCALER;
126 
127 #if PERIODIC_TELEMETRY
129 #endif
130 }
131 
135 void mateksys_3901_l0x_scale_X(float scalex)
136 {
137  mateksys3901l0x.scaler_x = scalex;
138 }
139 
143 void mateksys_3901_l0x_scale_Y(float scaley)
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 {
178  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
179  }
180  break;
181 
182  case MATEKSYS_3901_L0X_PARSE_DIRECTION: // direction <
183  if (byte == 0x3C) {
184  mateksys3901l0x.parse_status++;
185  } else {
186  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
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 {
195  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
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 {
205  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
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 {
214  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
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 {
223  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
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 {
233  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
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) {
246  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
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) {
281  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
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
355  mateksys3901l0x.time_usec = get_sys_time_usec();
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
377  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
378  break;
379 
380  default:
381  // Error, return to start
382  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
383  break;
384 
385  }
386 }
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
enum Mateksys3901l0XParseStatus parse_status
#define MATEKSYS_3901_L0X_DISTANCE_THRES
#define AGL_LIDAR_MATEKSYS_3901_L0X_ID
float phi
in radians
#define USE_MATEKSYS_3901_L0X_AGL
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
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)
#define MATEKSYS_3901_L0X_COMPENSATE_ROTATION
Periodic telemetry system header (includes downlink utility and generated code).
Main include for ABI (AirBorneInterface).
void mateksys_3901_l0x_scale_Y(float scaley)
Scale the Flow Y.
struct link_device * device
float theta
in radians
uint8_t distancemm_quality
Driver for the mateksys_3901_l0x sensor via MSP protocol output.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void mateksys3901l0x_event(void)
Receive bytes from the UART port and parse them.
struct Mateksys3901l0X mateksys3901l0x
#define MATEKSYS_3901_L0X_MAX_DISTANCE
API to get/set the generic vehicle states.
#define byte
#define MATEKSYS_3901_L0X_MOTION_THRES
#define USE_MATEKSYS_3901_L0X_OPTICAL_FLOW
#define FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID
void mateksys3901l0x_init(void)
Initialization function.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
#define MATEKSYS_3901_L0X_FLOW_Y_SCALER
#define MATEKSYS_3901_L0X_MAX_FLOW
static void mateksys3901l0x_parse(uint8_t byte)
Parse the sensor MSP output bytes 1 by 1.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
void mateksys_3901_l0x_scale_X(float scalex)
Scale the Flow X.
#define MATEKSYS_3901_L0X_FLOW_X_SCALER
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98