Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
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 {
53  pprz_msg_send_LIDAR(trans, dev, AC_ID,
54  &tfmini.distance,
55  &tfmini.mode,
56  &status);
57 }
58 
59 #endif
60 
64 void tfmini_init(void)
65 {
66  tfmini.device = &((TFMINI_PORT).device);
67 
68  tfmini.update_agl = USE_TFMINI_AGL;
69  tfmini.compensate_rotation = TFMINI_COMPENSATE_ROTATION;
70 
71  tfmini.strength = 0;
72  tfmini.distance = 0;
74 
75 #if PERIODIC_TELEMETRY
77 #endif
78 }
79 
84 void tfmini_event(void)
85 {
86  while (tfmini.parse_status != TFMINI_INITIALIZE && tfmini.device->char_available(tfmini.device->periph)) {
87  tfmini_parse(tfmini.device->get_byte(tfmini.device->periph));
88  }
89 }
90 
95 {
96  switch (tfmini.parse_status) {
97  case TFMINI_INITIALIZE:
98  break;
99  case TFMINI_PARSE_HEAD:
100  if (byte == 0x59) {
101  tfmini.parse_crc = byte;
102  tfmini.parse_status++;
103  }
104  break;
105  case TFMINI_PARSE_HEAD2:
106  if (byte == 0x59) {
107  tfmini.parse_crc += byte;
108  tfmini.parse_status++;
109  } else {
111  }
112  break;
113 
114  case TFMINI_PARSE_DIST_L:
115  tfmini.raw_dist = byte;
116  tfmini.parse_crc += byte;
117  tfmini.parse_status++;
118  break;
119  case TFMINI_PARSE_DIST_H:
120  tfmini.raw_dist |= (byte << 8);
121  tfmini.parse_crc += byte;
122  tfmini.parse_status++;
123  break;
124 
126  tfmini.raw_strength = byte;
127  tfmini.parse_crc += byte;
128  tfmini.parse_status++;
129  break;
131  tfmini.raw_strength |= (byte << 8);
132  tfmini.parse_crc += byte;
133  tfmini.parse_status++;
134  break;
135 
136  case TFMINI_PARSE_MODE:
137  tfmini.raw_mode = byte;
138  tfmini.parse_crc += byte;
139  tfmini.parse_status++;
140  break;
141  case TFMINI_PARSE_BYTE7:
142  tfmini.parse_crc += byte;
143  tfmini.parse_status++;
144  break;
145 
147  // When the CRC matches
148  if (tfmini.parse_crc == byte) {
149  uint32_t now_ts = get_sys_time_usec();
150  tfmini.distance = tfmini.raw_dist / 100.f;
151  tfmini.strength = tfmini.raw_strength;
152  tfmini.mode = tfmini.raw_mode;
153 
154  // When the distance is valid
155  if (tfmini.distance != 0xFFFF) {
156  // compensate AGL measurement for body rotation
157  if (tfmini.compensate_rotation) {
158  float phi = stateGetNedToBodyEulers_f()->phi;
159  float theta = stateGetNedToBodyEulers_f()->theta;
160  float gain = (float)fabs((double)(cosf(phi) * cosf(theta)));
161  tfmini.distance = tfmini.distance * gain;
162  }
163 
164  // send message (if requested)
165  if (tfmini.update_agl) {
166  AbiSendMsgAGL(AGL_LIDAR_TFMINI_ID, now_ts, tfmini.distance);
167  }
168  }
169  }
170 
171  // Start reading again
173  break;
174 
175  default:
176  // Error, return to start
178  break;
179  }
180 }
void tfmini_event(void)
Lidar event function Receive bytes from the UART port and parse them.
Definition: tfmini.c:84
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
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:64
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
uint8_t status
#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:94
#define AGL_LIDAR_TFMINI_ID
bool compensate_rotation
Definition: tfmini.h:58
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:71
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78