Paparazzi UAS  v5.18.0_stable-1-g6993852-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 "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 
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,
76  &mateksys3901l0x.time_sec,
77  &mateksys3901l0x.sensor_id,
78  &mateksys3901l0x.motionX_clean,
79  &mateksys3901l0x.motionY_clean,
80  &mateksys3901l0x.velocityX,
81  &mateksys3901l0x.velocityY,
82  &mateksys3901l0x.motion_quality,
83  &mateksys3901l0x.distance_clean,
84  &mateksys3901l0x.distancemm_quality);
85 }
86 
87 #endif
88 
93 {
94  mateksys3901l0x.device = &((MATEKSYS_3901_L0X_PORT).device);
95  mateksys3901l0x.parse_crc = 0;
96  mateksys3901l0x.motion_quality = 0;
97  mateksys3901l0x.motionX = 0;
98  mateksys3901l0x.motionY = 0;
99  mateksys3901l0x.distancemm_quality = 0;
100  mateksys3901l0x.distancemm = 0;
101  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
102 
103 #if PERIODIC_TELEMETRY
105 #endif
106 }
107 
112 {
113  while (mateksys3901l0x.parse_status != MATEKSYS_3901_L0X_INITIALIZE && mateksys3901l0x.device->char_available(mateksys3901l0x.device->periph)) {
114  mateksys3901l0x_parse(mateksys3901l0x.device->get_byte(mateksys3901l0x.device->periph));
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) {
130  mateksys3901l0x.parse_status++;
131  }
132  break;
133 
134  case MATEKSYS_3901_L0X_PARSE_HEAD2: // MSPv2 identifier X
135  if (byte == 0x58) {
136  mateksys3901l0x.parse_status++;
137  } else {
138  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
139  }
140  break;
141 
142  case MATEKSYS_3901_L0X_PARSE_DIRECTION: // direction <
143  if (byte == 0x3C) {
144  mateksys3901l0x.parse_status++;
145  } else {
146  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
147  }
148  break;
149 
150  case MATEKSYS_3901_L0X_PARSE_LENGTH: // set to 0
151  if (byte == 0x00) {
152  mateksys3901l0x.parse_crc += byte;
153  mateksys3901l0x.parse_status++;
154  } else {
155  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
156  }
157  break;
158 
159  case MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B1: // 0x01 = rangefinder; 0x02 = opticalflow
160  if (byte == 0x01 || byte == 0x02) {
161  mateksys3901l0x.sensor_id = byte;
162  mateksys3901l0x.parse_crc += byte;
163  mateksys3901l0x.parse_status++;
164  } else {
165  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
166  }
167  break;
168 
169  case MATEKSYS_3901_L0X_PARSE_FUNCTION_ID_B2: // sensor id pointer
170  if (byte == 0x1F) {
171  mateksys3901l0x.parse_status++;
172  mateksys3901l0x.parse_crc += byte;
173  } else {
174  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
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) {
180  mateksys3901l0x.parse_status++;
181  mateksys3901l0x.parse_crc += byte;
182  } else {
183  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
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 {
193  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
194  }
195  break;
196 
197  // rangefinder data parsing
199  mateksys3901l0x.distancemm_quality = byte;
200  mateksys3901l0x.parse_crc += byte;
201  mateksys3901l0x.parse_status++;
202  break;
203 
205  if (mateksys3901l0x.distancemm_quality <= MATEKSYS_3901_L0X_DISTANCE_THRES) {
206  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
207  } else {
208  mateksys3901l0x.distancemm = byte;
209  mateksys3901l0x.parse_crc += byte;
210  mateksys3901l0x.parse_status++;
211  }
212  break;
213 
215  mateksys3901l0x.distancemm |= (byte << 8);
216  mateksys3901l0x.parse_crc += byte;
217  mateksys3901l0x.parse_status++;
218  break;
219 
221  mateksys3901l0x.distancemm |= (byte << 16);
222  mateksys3901l0x.parse_crc += byte;
223  mateksys3901l0x.parse_status++;
224  break;
225 
227  mateksys3901l0x.distancemm |= (byte << 24);
228  mateksys3901l0x.parse_crc += byte;
230  break;
231 
232  // optical flow data parsing
234  mateksys3901l0x.motion_quality = byte;
235  mateksys3901l0x.parse_crc += byte;
236  mateksys3901l0x.parse_status++;
237  break;
238 
240  if (mateksys3901l0x.motion_quality <= MATEKSYS_3901_L0X_MOTION_THRES) {
241  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
242  } else {
243  mateksys3901l0x.motionY = byte;
244  mateksys3901l0x.parse_crc += byte;
245  mateksys3901l0x.parse_status++;
246  }
247  break;
248 
250  mateksys3901l0x.motionY |= (byte << 8);
251  mateksys3901l0x.parse_crc += byte;
252  mateksys3901l0x.parse_status++;
253  break;
254 
256  mateksys3901l0x.motionY |= (byte << 16);
257  mateksys3901l0x.parse_crc += byte;
258  mateksys3901l0x.parse_status++;
259  break;
260 
262  mateksys3901l0x.motionY |= (byte << 24);
263  mateksys3901l0x.parse_crc += byte;
264  mateksys3901l0x.parse_status++;
265  break;
266 
268  mateksys3901l0x.motionX = byte;
269  mateksys3901l0x.parse_crc += byte;
270  mateksys3901l0x.parse_status++;
271  break;
272 
274  mateksys3901l0x.motionX |= (byte << 8);
275  mateksys3901l0x.parse_crc += byte;
276  mateksys3901l0x.parse_status++;
277  break;
278 
280  mateksys3901l0x.motionX |= (byte << 16);
281  mateksys3901l0x.parse_crc += byte;
282  mateksys3901l0x.parse_status++;
283  break;
284 
286  mateksys3901l0x.motionX |= (byte << 24);
287  mateksys3901l0x.parse_crc += byte;
288  mateksys3901l0x.parse_status++;
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)));
300  mateksys3901l0x.distancemm = mateksys3901l0x.distancemm * gain;
301  }
302 
303  // send messages with no error measurements
304  mateksys3901l0x.distance_clean = mateksys3901l0x.distancemm/1000.f;
305  mateksys3901l0x.motionX_clean = mateksys3901l0x.motionX;
306  mateksys3901l0x.motionY_clean = mateksys3901l0x.motionY;
307 
308  // estimate velocity and send it to telemetry
309  mateksys3901l0x.velocityX = mateksys3901l0x.distance_clean * sin(RadOfDeg(mateksys3901l0x.motionX_clean));
310  mateksys3901l0x.velocityY = mateksys3901l0x.distance_clean * sin(RadOfDeg(mateksys3901l0x.motionY_clean));
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,
320  mateksys3901l0x.distance_clean);
321  }
322 
323  // send optical flow (if requested)
325  AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_MATEKSYS_3901_L0X_ID,
326  now_ts,
327  mateksys3901l0x.motionX_clean,
328  mateksys3901l0x.motionY_clean,
329  0,
330  0,
331  mateksys3901l0x.motion_quality,
332  0.0);
333  }
334  }
335  // Start reading again
336  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
337  break;
338 
339  default:
340  // Error, return to start
341  mateksys3901l0x.parse_status = MATEKSYS_3901_L0X_PARSE_HEAD;
342  break;
343 
344  }
345 }
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).
struct link_device * device
float theta
in radians
uint8_t distancemm_quality
Driver for the mateksys_3901_l0x sensor via MSP protocol output.
unsigned long uint32_t
Definition: types.h:18
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void mateksys3901l0x_event(void)
Receive bytes from the UART port and parse them.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
struct Mateksys3901l0X mateksys3901l0x
unsigned char uint8_t
Definition: types.h:14
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:68
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