Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "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 
80 };
81 
82 static void mateksys3901l0x_parse(uint8_t byte);
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,
103 }
104 
105 #endif
106 
111 {
112  mateksys3901l0x.device = &((MATEKSYS_3901_L0X_PORT).device);
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 {
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) {
171  }
172  break;
173 
174  case MATEKSYS_3901_L0X_PARSE_HEAD2: // MSPv2 identifier X
175  if (byte == 0x58) {
177  } else {
179  }
180  break;
181 
182  case MATEKSYS_3901_L0X_PARSE_DIRECTION: // direction <
183  if (byte == 0x3C) {
185  } else {
187  }
188  break;
189 
190  case MATEKSYS_3901_L0X_PARSE_LENGTH: // set to 0
191  if (byte == 0x00) {
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) {
204  } else {
206  }
207  break;
208 
209  case MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2: // sensor id pointer
210  if (byte == 0x1F) {
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) {
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
242  break;
243 
247  } else {
251  }
252  break;
253 
255  mateksys3901l0x.distancemm_temp |= (byte << 8);
258  break;
259 
261  mateksys3901l0x.distancemm_temp |= (byte << 16);
264  break;
265 
267  mateksys3901l0x.distancemm_temp |= (byte << 24);
270  break;
271 
272  // optical flow data parsing
277  break;
278 
282  } else {
286  }
287  break;
288 
290  mateksys3901l0x.motionY_temp |= (byte << 8);
293  break;
294 
296  mateksys3901l0x.motionY_temp |= (byte << 16);
299  break;
300 
302  mateksys3901l0x.motionY_temp |= (byte << 24);
305  break;
306 
311  break;
312 
314  mateksys3901l0x.motionX_temp |= (byte << 8);
317  break;
318 
320  mateksys3901l0x.motionX_temp |= (byte << 16);
323  break;
324 
326  mateksys3901l0x.motionX_temp |= (byte << 24);
329  break;
330 
332 
333  // When the distance and motion info are valid (max values based on sensor specifications)...
335 
336  // pass temporary message and apply calibration parameters
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)
353 
354  // get ticks
356 
357  // send AGL (if requested)
359  AbiSendMsgAGL(AGL_LIDAR_MATEKSYS_3901_L0X_ID,
362  }
363 
364  // send optical flow (if requested)
366  AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID,
368  mateksys3901l0x.motionX, // motion in deg/sec
369  mateksys3901l0x.motionY, // motion in deg/sec
370  0,
371  0,
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.
Definition: sys_time_arch.c:71
float phi
in radians
float theta
in radians
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
#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.
enum Mateksys3901l0XParseStatus parse_status
uint8_t distancemm_quality
@ 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
struct link_device * device
#define byte
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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.
Definition: vl53l1_types.h:98