Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
tfmini.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Freek van Tienen <freek.v.tienen@gmail.com>
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 
27 #include "tfmini.h"
28 #include "mcu_periph/uart.h"
29 #include "subsystems/abi.h"
30 
31 // State interface for rotation compensation
32 #include "state.h"
33 
34 // Messages
35 #include "pprzlink/messages.h"
37 
38 struct TFMini tfmini = {
40 };
41 
42 static void tfmini_parse(uint8_t byte);
43 
44 #if PERIODIC_TELEMETRY
46 
50 static void tfmini_send_lidar(struct transport_tx *trans, struct link_device *dev)
51 {
52  pprz_msg_send_LIDAR(trans, dev, AC_ID,
53  &tfmini.distance,
54  &tfmini.mode,
55  &tfmini.parse_status);
56 }
57 
58 #endif
59 
63 void tfmini_init(void)
64 {
65  tfmini.device = &((TFMINI_PORT).device);
66 
67  tfmini.update_agl = USE_TFMINI_AGL;
68  tfmini.compensate_rotation = TFMINI_COMPENSATE_ROTATION;
69 
70  tfmini.strength = 0;
71  tfmini.distance = 0;
73 
74 #if PERIODIC_TELEMETRY
76 #endif
77 }
78 
83 void tfmini_event(void)
84 {
85  while (tfmini.parse_status != TFMINI_INITIALIZE && tfmini.device->char_available(tfmini.device->periph)) {
86  tfmini_parse(tfmini.device->get_byte(tfmini.device->periph));
87  }
88 }
89 
94 {
95  switch (tfmini.parse_status) {
96  case TFMINI_INITIALIZE:
97  break;
98  case TFMINI_PARSE_HEAD:
99  if (byte == 0x59) {
100  tfmini.parse_crc = byte;
101  tfmini.parse_status++;
102  }
103  break;
104  case TFMINI_PARSE_HEAD2:
105  if (byte == 0x59) {
106  tfmini.parse_crc += byte;
107  tfmini.parse_status++;
108  } else {
110  }
111  break;
112 
113  case TFMINI_PARSE_DIST_L:
114  tfmini.raw_dist = byte;
115  tfmini.parse_crc += byte;
116  tfmini.parse_status++;
117  break;
118  case TFMINI_PARSE_DIST_H:
119  tfmini.raw_dist |= (byte << 8);
120  tfmini.parse_crc += byte;
121  tfmini.parse_status++;
122  break;
123 
125  tfmini.raw_strength = byte;
126  tfmini.parse_crc += byte;
127  tfmini.parse_status++;
128  break;
130  tfmini.raw_strength |= (byte << 8);
131  tfmini.parse_crc += byte;
132  tfmini.parse_status++;
133  break;
134 
135  case TFMINI_PARSE_MODE:
136  tfmini.raw_mode = byte;
137  tfmini.parse_crc += byte;
138  tfmini.parse_status++;
139  break;
140  case TFMINI_PARSE_BYTE7:
141  tfmini.parse_crc += byte;
142  tfmini.parse_status++;
143  break;
144 
146  // When the CRC matches
147  if (tfmini.parse_crc == byte) {
148  uint32_t now_ts = get_sys_time_usec();
149  tfmini.distance = tfmini.raw_dist / 100.f;
150  tfmini.strength = tfmini.raw_strength;
151  tfmini.mode = tfmini.raw_mode;
152 
153  // When the distance is valid
154  if (tfmini.distance != 0xFFFF) {
155  // compensate AGL measurement for body rotation
156  if (tfmini.compensate_rotation) {
157  float phi = stateGetNedToBodyEulers_f()->phi;
158  float theta = stateGetNedToBodyEulers_f()->theta;
159  float gain = (float)fabs((double)(cosf(phi) * cosf(theta)));
160  tfmini.distance = tfmini.distance / gain;
161  }
162 
163  // send message (if requested)
164  if (tfmini.update_agl) {
165  AbiSendMsgAGL(AGL_LIDAR_TFMINI_ID, now_ts, tfmini.distance);
166  }
167  }
168  }
169 
170  // Start reading again
172  break;
173  }
174 }
void tfmini_event(void)
Lidar event function Receive bytes from the UART port and parse them.
Definition: tfmini.c:83
Definition: tfmini.h:46
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
struct link_device * device
Definition: tfmini.h:47
void tfmini_init(void)
Initialization function.
Definition: tfmini.c:63
float phi
in radians
struct TFMini tfmini
Definition: tfmini.c:38
static void tfmini_send_lidar(struct transport_tx *trans, struct link_device *dev)
Downlink message lidar.
Definition: tfmini.c:50
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
Periodic telemetry system header (includes downlink utility and generated code).
enum TFMiniParseStatus parse_status
Definition: tfmini.h:48
Main include for ABI (AirBorneInterface).
uint16_t strength
Definition: tfmini.h:54
uint16_t raw_strength
Definition: tfmini.h:51
uint8_t parse_crc
Definition: tfmini.h:49
uint16_t raw_dist
Definition: tfmini.h:50
float theta
in radians
float distance
Definition: tfmini.h:55
unsigned long uint32_t
Definition: types.h:18
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
static void tfmini_parse(uint8_t byte)
Parse the lidar bytes 1 by 1.
Definition: tfmini.c:93
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define AGL_LIDAR_TFMINI_ID
bool compensate_rotation
Definition: tfmini.h:58
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
#define byte
driver for the TFMini lidar
bool update_agl
Definition: tfmini.h:57
uint8_t raw_mode
Definition: tfmini.h:52
uint8_t mode
Definition: tfmini.h:56
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46