Paparazzi UAS  v5.18.0_stable
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 "mateksys_3901_l0x.h"
29 #include "mcu_periph/uart.h"
30 #include "subsystems/abi.h"
31 
32 // State interface for rotation compensation
33 #include "state.h"
34 
35 // Messages
36 #include "pprzlink/messages.h"
38 
39 
40 // Define configuration parameters
41 #ifndef MATEKSYS_3901_L0X_MOTION_THRES
42 #define MATEKSYS_3901_L0X_MOTION_THRES
43 #endif
44 
45 #ifndef MATEKSYS_3901_L0X_DISTANCE_THRES
46 #define MATEKSYS_3901_L0X_DISTANCE_THRES
47 #endif
48 
49 #ifndef USE_MATEKSYS_3901_L0X_AGL
50 #define USE_MATEKSYS_3901_L0X_AGL
51 #endif
52 
53 #ifndef USE_MATEKSYS_3901_L0X_OPTICAL_FLOW
54 #define USE_MATEKSYS_3901_L0X_OPTICAL_FLOW
55 #endif
56 
57 #ifndef MATEKSYS_3901_L0X_COMPENSATE_ROTATION
58 #define MATEKSYS_3901_L0X_COMPENSATE_ROTATION
59 #endif
60 
63 };
64 
65 static void mateksys3901l0x_parse(uint8_t byte);
66 
67 #if PERIODIC_TELEMETRY
69 
73 static void mateksys3901l0x_send_optical_flow(struct transport_tx *trans, struct link_device *dev)
74 {
75  pprz_msg_send_OPTICAL_FLOW(trans, dev, AC_ID,
85 }
86 
87 #endif
88 
93 {
94  mateksys3901l0x.device = &((MATEKSYS_3901_L0X_PORT).device);
102 
103 #if PERIODIC_TELEMETRY
105 #endif
106 }
107 
112 {
115  }
116 }
117 
122 {
123 
124  switch (mateksys3901l0x.parse_status) {
126  break;
127 
128  case MATEKSYS_3901_L0X_PARSE_HEAD: // MSP general header $
129  if (byte == 0x24) {
131  }
132  break;
133 
134  case MATEKSYS_3901_L0X_PARSE_HEAD2: // MSPv2 identifier X
135  if (byte == 0x58) {
137  } else {
139  }
140  break;
141 
142  case MATEKSYS_3901_L0X_PARSE_DIRECTION: // direction <
143  if (byte == 0x3C) {
145  } else {
147  }
148  break;
149 
150  case MATEKSYS_3901_L0X_PARSE_LENGTH: // set to 0
151  if (byte == 0x00) {
154  } else {
156  }
157  break;
158 
159  case MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1: // 0x01 = rangefinder; 0x02 = opticalflow
160  if (byte == 0x01 || byte == 0x02) {
164  } else {
166  }
167  break;
168 
169  case MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2: // sensor id pointer
170  if (byte == 0x1F) {
173  } else {
175  }
176  break;
177 
178  case MATEKSYS_3901_L0X_PARSE_SIZE: // two fixed sizes are expected if message is
179  if (byte == 0x05 || byte == 0x09) {
182  } else {
184  }
185  break;
186 
187  case MATEKSYS_3901_L0X_PARSE_POINTER: // should be zero, used to redirect to motion parsing or lidar parsing
188  if (mateksys3901l0x.sensor_id == 0x01) {
190  } else if (mateksys3901l0x.sensor_id == 0x02) {
192  } else {
194  }
195  break;
196 
197  // rangefinder data parsing
202  break;
203 
207  } else {
211  }
212  break;
213 
215  mateksys3901l0x.distancemm |= (byte << 8);
218  break;
219 
221  mateksys3901l0x.distancemm |= (byte << 16);
224  break;
225 
227  mateksys3901l0x.distancemm |= (byte << 24);
230  break;
231 
232  // optical flow data parsing
237  break;
238 
242  } else {
246  }
247  break;
248 
250  mateksys3901l0x.motionY |= (byte << 8);
253  break;
254 
256  mateksys3901l0x.motionY |= (byte << 16);
259  break;
260 
262  mateksys3901l0x.motionY |= (byte << 24);
265  break;
266 
271  break;
272 
274  mateksys3901l0x.motionX |= (byte << 8);
277  break;
278 
280  mateksys3901l0x.motionX |= (byte << 16);
283  break;
284 
286  mateksys3901l0x.motionX |= (byte << 24);
289  break;
290 
292 
293  // When the distance and motion info are valid (max values based on sensor specifications)...
294  if (mateksys3901l0x.distancemm > 0 && mateksys3901l0x.distancemm <= 3000 && abs(mateksys3901l0x.motionX) <= 300 && abs(mateksys3901l0x.motionY) <= 300) {
295  // ... compensate AGL measurement for body rotation
297  float phi = stateGetNedToBodyEulers_f()->phi;
298  float theta = stateGetNedToBodyEulers_f()->theta;
299  float gain = (float)fabs((double)(cosf(phi) * cosf(theta)));
301  }
302 
303  // send messages with no error measurements
307 
308  // estimate velocity and send it to telemetry
311 
312  // get ticks
313  uint32_t now_ts = get_sys_time_usec();
314  mateksys3901l0x.time_sec = now_ts*1e-6;
315 
316  // send AGL (if requested)
318  AbiSendMsgAGL(AGL_LIDAR_MATEKSYS_3901_L0X_ID,
319  now_ts,
321  }
322 
323  // send optical flow (if requested)
325  AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID,
326  now_ts,
329  0,
330  0,
332  0.0);
333  }
334  }
335  // Start reading again
337  break;
338 
339  default:
340  // Error, return to start
342  break;
343 
344  }
345 }
MATEKSYS_3901_L0X_DISTANCE_THRES
#define MATEKSYS_3901_L0X_DISTANCE_THRES
Definition: mateksys_3901_l0x.c:46
Mateksys3901l0X::parse_status
enum Mateksys3901l0XParseStatus parse_status
Definition: mateksys_3901_l0x.h:76
Mateksys3901l0X::distancemm
int32_t distancemm
Definition: mateksys_3901_l0x.h:85
MATEKSYS_3901_L0X_PARSE_CHECKSUM
@ MATEKSYS_3901_L0X_PARSE_CHECKSUM
Definition: mateksys_3901_l0x.h:71
mateksys3901l0x_init
void mateksys3901l0x_init(void)
Initialization function.
Definition: mateksys_3901_l0x.c:92
MATEKSYS_3901_L0X_PARSE_MOTIONY_B3
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B3
Definition: mateksys_3901_l0x.h:65
Mateksys3901l0X::motionY_clean
int32_t motionY_clean
Definition: mateksys_3901_l0x.h:83
Mateksys3901l0X::motion_quality
uint8_t motion_quality
Definition: mateksys_3901_l0x.h:79
USE_MATEKSYS_3901_L0X_AGL
#define USE_MATEKSYS_3901_L0X_AGL
Definition: mateksys_3901_l0x.c:50
MATEKSYS_3901_L0X_PARSE_DISTANCE_B1
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B1
Definition: mateksys_3901_l0x.h:58
MATEKSYS_3901_L0X_PARSE_LENGTH
@ MATEKSYS_3901_L0X_PARSE_LENGTH
Definition: mateksys_3901_l0x.h:52
abi.h
MATEKSYS_3901_L0X_PARSE_HEAD2
@ MATEKSYS_3901_L0X_PARSE_HEAD2
Definition: mateksys_3901_l0x.h:50
Mateksys3901l0X::sensor_id
uint8_t sensor_id
Definition: mateksys_3901_l0x.h:78
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
uint32_t
unsigned long uint32_t
Definition: types.h:18
MATEKSYS_3901_L0X_PARSE_DISTANCE_B2
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B2
Definition: mateksys_3901_l0x.h:59
MATEKSYS_3901_L0X_PARSE_DISTANCE_B4
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B4
Definition: mateksys_3901_l0x.h:61
Mateksys3901l0X::velocityY
float velocityY
Definition: mateksys_3901_l0x.h:88
MATEKSYS_3901_L0X_PARSE_DISTANCE_B3
@ MATEKSYS_3901_L0X_PARSE_DISTANCE_B3
Definition: mateksys_3901_l0x.h:60
MATEKSYS_3901_L0X_PARSE_POINTER
@ MATEKSYS_3901_L0X_PARSE_POINTER
Definition: mateksys_3901_l0x.h:56
MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY
@ MATEKSYS_3901_L0X_PARSE_DISTANCEQUALITY
Definition: mateksys_3901_l0x.h:57
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:68
telemetry.h
Mateksys3901l0X::device
struct link_device * device
Definition: mateksys_3901_l0x.h:75
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:70
FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID
#define FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID
Definition: abi_sender_ids.h:375
MATEKSYS_3901_L0X_INITIALIZE
@ MATEKSYS_3901_L0X_INITIALIZE
Definition: mateksys_3901_l0x.h:48
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Mateksys3901l0X::motionX_clean
int32_t motionX_clean
Definition: mateksys_3901_l0x.h:82
mateksys3901l0x
struct Mateksys3901l0X mateksys3901l0x
Definition: mateksys_3901_l0x.c:61
MATEKSYS_3901_L0X_PARSE_MOTIONX_B3
@ MATEKSYS_3901_L0X_PARSE_MOTIONX_B3
Definition: mateksys_3901_l0x.h:69
MATEKSYS_3901_L0X_PARSE_DIRECTION
@ MATEKSYS_3901_L0X_PARSE_DIRECTION
Definition: mateksys_3901_l0x.h:51
uint8_t
unsigned char uint8_t
Definition: types.h:14
Mateksys3901l0X::distance_clean
float distance_clean
Definition: mateksys_3901_l0x.h:86
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:46
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:68
Mateksys3901l0X
Definition: mateksys_3901_l0x.h:74
MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1
@ MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1
Definition: mateksys_3901_l0x.h:53
AGL_LIDAR_MATEKSYS_3901_L0X_ID
#define AGL_LIDAR_MATEKSYS_3901_L0X_ID
Definition: abi_sender_ids.h:185
MATEKSYS_3901_L0X_PARSE_MOTIONY_B1
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B1
Definition: mateksys_3901_l0x.h:63
MATEKSYS_3901_L0X_PARSE_MOTIONY_B2
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B2
Definition: mateksys_3901_l0x.h:64
Mateksys3901l0X::distancemm_quality
uint8_t distancemm_quality
Definition: mateksys_3901_l0x.h:84
MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY
@ MATEKSYS_3901_L0X_PARSE_MOTIONQUALITY
Definition: mateksys_3901_l0x.h:62
Mateksys3901l0X::parse_crc
uint8_t parse_crc
Definition: mateksys_3901_l0x.h:89
MATEKSYS_3901_L0X_PARSE_MOTIONX_B1
@ MATEKSYS_3901_L0X_PARSE_MOTIONX_B1
Definition: mateksys_3901_l0x.h:67
MATEKSYS_3901_L0X_PARSE_SIZE
@ MATEKSYS_3901_L0X_PARSE_SIZE
Definition: mateksys_3901_l0x.h:55
Mateksys3901l0X::velocityX
float velocityX
Definition: mateksys_3901_l0x.h:87
MATEKSYS_3901_L0X_PARSE_HEAD
@ MATEKSYS_3901_L0X_PARSE_HEAD
Definition: mateksys_3901_l0x.h:49
mateksys3901l0x_event
void mateksys3901l0x_event(void)
Receive bytes from the UART port and parse them.
Definition: mateksys_3901_l0x.c:111
MATEKSYS_3901_L0X_PARSE_MOTIONY_B4
@ MATEKSYS_3901_L0X_PARSE_MOTIONY_B4
Definition: mateksys_3901_l0x.h:66
Mateksys3901l0X::time_sec
float time_sec
Definition: mateksys_3901_l0x.h:77
MATEKSYS_3901_L0X_MOTION_THRES
#define MATEKSYS_3901_L0X_MOTION_THRES
Definition: mateksys_3901_l0x.c:42
Mateksys3901l0X::motionX
int32_t motionX
Definition: mateksys_3901_l0x.h:80
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:54
mateksys3901l0x_parse
static void mateksys3901l0x_parse(uint8_t byte)
Parse the sensor MSP output bytes 1 by 1.
Definition: mateksys_3901_l0x.c:121
MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2
@ MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2
Definition: mateksys_3901_l0x.h:54
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:73
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:58