Paparazzi UAS  v6.2_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 }
MATEKSYS_3901_L0X_DISTANCE_THRES
#define MATEKSYS_3901_L0X_DISTANCE_THRES
Definition: mateksys_3901_l0x.c:47
Mateksys3901l0X::parse_status
enum Mateksys3901l0XParseStatus parse_status
Definition: mateksys_3901_l0x.h:74
MATEKSYS_3901_L0X_PARSE_CHECKSUM
@ MATEKSYS_3901_L0X_PARSE_CHECKSUM
Definition: mateksys_3901_l0x.h:69
mateksys3901l0x_init
void mateksys3901l0x_init(void)
Initialization function.
Definition: mateksys_3901_l0x.c:110
MATEKSYS_3901_L0X_PARSE_MOTIONY_B3
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B3
Definition: mateksys_3901_l0x.h:63
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
Mateksys3901l0X::motion_quality
uint8_t motion_quality
Definition: mateksys_3901_l0x.h:77
USE_MATEKSYS_3901_L0X_AGL
#define USE_MATEKSYS_3901_L0X_AGL
Definition: mateksys_3901_l0x.c:59
MATEKSYS_3901_L0X_FLOW_Y_SCALER
#define MATEKSYS_3901_L0X_FLOW_Y_SCALER
Definition: mateksys_3901_l0x.c:75
MATEKSYS_3901_L0X_PARSE_DISTANCE_B1
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B1
Definition: mateksys_3901_l0x.h:56
MATEKSYS_3901_L0X_PARSE_LENGTH
@ MATEKSYS_3901_L0X_PARSE_LENGTH
Definition: mateksys_3901_l0x.h:50
MATEKSYS_3901_L0X_MAX_FLOW
#define MATEKSYS_3901_L0X_MAX_FLOW
Definition: mateksys_3901_l0x.c:51
Mateksys3901l0X::scaler_x
float scaler_x
Definition: mateksys_3901_l0x.h:89
abi.h
MATEKSYS_3901_L0X_PARSE_HEAD2
@ MATEKSYS_3901_L0X_PARSE_HEAD2
Definition: mateksys_3901_l0x.h:48
Mateksys3901l0X::scaler_y
float scaler_y
Definition: mateksys_3901_l0x.h:90
Mateksys3901l0X::sensor_id
uint8_t sensor_id
Definition: mateksys_3901_l0x.h:76
Mateksys3901l0X::time_usec
float time_usec
Definition: mateksys_3901_l0x.h:75
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
MATEKSYS_3901_L0X_PARSE_DISTANCE_B2
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B2
Definition: mateksys_3901_l0x.h:57
Mateksys3901l0X::motionY_temp
int32_t motionY_temp
Definition: mateksys_3901_l0x.h:80
MATEKSYS_3901_L0X_PARSE_DISTANCE_B4
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B4
Definition: mateksys_3901_l0x.h:59
Mateksys3901l0X::velocityY
float velocityY
Definition: mateksys_3901_l0x.h:87
MATEKSYS_3901_L0X_PARSE_DISTANCE_B3
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B3
Definition: mateksys_3901_l0x.h:58
MATEKSYS_3901_L0X_PARSE_POINTER
@ MATEKSYS_3901_L0X_PARSE_POINTER
Definition: mateksys_3901_l0x.h:54
MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY
@ MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY
Definition: mateksys_3901_l0x.h:55
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
mateksys_3901_l0x_scale_Y
void mateksys_3901_l0x_scale_Y(float scaley)
Scale the Flow Y.
Definition: mateksys_3901_l0x.c:143
telemetry.h
Mateksys3901l0X::device
struct link_device * device
Definition: mateksys_3901_l0x.h:73
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
MATEKSYS_3901_L0X_PARSE_MOTIONX_B4
@ MATEKSYS_3901_L0X_PARSE_MOTIONX_B4
Definition: mateksys_3901_l0x.h:68
FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID
#define FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID
Definition: abi_sender_ids.h:391
MATEKSYS_3901_L0X_INITIALIZE
@ MATEKSYS_3901_L0X_INITIALIZE
Definition: mateksys_3901_l0x.h:46
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
mateksys3901l0x
struct Mateksys3901l0X mateksys3901l0x
Definition: mateksys_3901_l0x.c:78
MATEKSYS_3901_L0X_PARSE_MOTIONX_B3
@ MATEKSYS_3901_L0X_PARSE_MOTIONX_B3
Definition: mateksys_3901_l0x.h:67
MATEKSYS_3901_L0X_PARSE_DIRECTION
@ MATEKSYS_3901_L0X_PARSE_DIRECTION
Definition: mateksys_3901_l0x.h:49
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
byte
#define byte
Definition: nps_fdm_crrcsim.c:74
MATEKSYS_3901_L0X_PARSE_MOTIONX_B2
@ MATEKSYS_3901_L0X_PARSE_MOTIONX_B2
Definition: mateksys_3901_l0x.h:66
Mateksys3901l0X
Definition: mateksys_3901_l0x.h:72
MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1
@ MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1
Definition: mateksys_3901_l0x.h:51
AGL_LIDAR_MATEKSYS_3901_L0X_ID
#define AGL_LIDAR_MATEKSYS_3901_L0X_ID
Definition: abi_sender_ids.h:193
MATEKSYS_3901_L0X_PARSE_MOTIONY_B1
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B1
Definition: mateksys_3901_l0x.h:61
MATEKSYS_3901_L0X_MAX_DISTANCE
#define MATEKSYS_3901_L0X_MAX_DISTANCE
Definition: mateksys_3901_l0x.c:55
MATEKSYS_3901_L0X_PARSE_MOTIONY_B2
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B2
Definition: mateksys_3901_l0x.h:62
Mateksys3901l0X::distancemm_quality
uint8_t distancemm_quality
Definition: mateksys_3901_l0x.h:82
MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY
@ MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY
Definition: mateksys_3901_l0x.h:60
Mateksys3901l0X::parse_crc
uint8_t parse_crc
Definition: mateksys_3901_l0x.h:88
Mateksys3901l0X::motionX_temp
int32_t motionX_temp
Definition: mateksys_3901_l0x.h:78
MATEKSYS_3901_L0X_PARSE_MOTIONX_B1
@ MATEKSYS_3901_L0X_PARSE_MOTIONX_B1
Definition: mateksys_3901_l0x.h:65
MATEKSYS_3901_L0X_PARSE_SIZE
@ MATEKSYS_3901_L0X_PARSE_SIZE
Definition: mateksys_3901_l0x.h:53
Mateksys3901l0X::velocityX
float velocityX
Definition: mateksys_3901_l0x.h:86
Mateksys3901l0X::distancemm_temp
int32_t distancemm_temp
Definition: mateksys_3901_l0x.h:83
MATEKSYS_3901_L0X_PARSE_HEAD
@ MATEKSYS_3901_L0X_PARSE_HEAD
Definition: mateksys_3901_l0x.h:47
mateksys3901l0x_event
void mateksys3901l0x_event(void)
Receive bytes from the UART port and parse them.
Definition: mateksys_3901_l0x.c:151
MATEKSYS_3901_L0X_PARSE_MOTIONY_B4
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B4
Definition: mateksys_3901_l0x.h:64
mateksys_3901_l0x_scale_X
void mateksys_3901_l0x_scale_X(float scalex)
Scale the Flow X.
Definition: mateksys_3901_l0x.c:135
MATEKSYS_3901_L0X_MOTION_THRES
#define MATEKSYS_3901_L0X_MOTION_THRES
Definition: mateksys_3901_l0x.c:43
Mateksys3901l0X::distancemm
float distancemm
Definition: mateksys_3901_l0x.h:84
Mateksys3901l0X::motionX
int32_t motionX
Definition: mateksys_3901_l0x.h:79
Mateksys3901l0X::motionY
int32_t motionY
Definition: mateksys_3901_l0x.h:81
state.h
USE_MATEKSYS_3901_L0X_OPTICAL_FLOW
#define USE_MATEKSYS_3901_L0X_OPTICAL_FLOW
Definition: mateksys_3901_l0x.c:63
MATEKSYS_3901_L0X_FLOW_X_SCALER
#define MATEKSYS_3901_L0X_FLOW_X_SCALER
Definition: mateksys_3901_l0x.c:71
mateksys3901l0x_parse
static void mateksys3901l0x_parse(uint8_t byte)
Parse the sensor MSP output bytes 1 by 1.
Definition: mateksys_3901_l0x.c:161
MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2
@ MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2
Definition: mateksys_3901_l0x.h:52
mateksys3901l0x_send_optical_flow
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)
Definition: mateksys_3901_l0x.c:90
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
mateksys_3901_l0x.h
Driver for the mateksys_3901_l0x sensor via MSP protocol output.
MATEKSYS_3901_L0X_COMPENSATE_ROTATION
#define MATEKSYS_3901_L0X_COMPENSATE_ROTATION
Definition: mateksys_3901_l0x.c:67
Mateksys3901l0X::distance_compensated
float distance_compensated
Definition: mateksys_3901_l0x.h:85