Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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,
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 
94 static void tfmini_parse(uint8_t byte)
95 {
96  switch (tfmini.parse_status) {
97  case TFMINI_INITIALIZE:
98  break;
99  case TFMINI_PARSE_HEAD:
100  if (byte == 0x59) {
103  }
104  break;
105  case TFMINI_PARSE_HEAD2:
106  if (byte == 0x59) {
107  tfmini.parse_crc += byte;
109  } else {
111  }
112  break;
113 
114  case TFMINI_PARSE_DIST_L:
115  tfmini.raw_dist = byte;
116  tfmini.parse_crc += byte;
118  break;
119  case TFMINI_PARSE_DIST_H:
120  tfmini.raw_dist |= (byte << 8);
121  tfmini.parse_crc += byte;
123  break;
124 
127  tfmini.parse_crc += byte;
129  break;
131  tfmini.raw_strength |= (byte << 8);
132  tfmini.parse_crc += byte;
134  break;
135 
136  case TFMINI_PARSE_MODE:
137  tfmini.raw_mode = byte;
138  tfmini.parse_crc += byte;
140  break;
141  case TFMINI_PARSE_BYTE7:
142  tfmini.parse_crc += byte;
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;
153 
154  // When the distance is valid
155  if (tfmini.distance != 0xFFFF) {
156  // compensate AGL measurement for body 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 }
TFMini::raw_strength
uint16_t raw_strength
Definition: tfmini.h:51
TFMini::raw_mode
uint8_t raw_mode
Definition: tfmini.h:52
AGL_LIDAR_TFMINI_ID
#define AGL_LIDAR_TFMINI_ID
Definition: abi_sender_ids.h:169
tfmini
struct TFMini tfmini
Definition: tfmini.c:38
TFMINI_PARSE_BYTE7
@ TFMINI_PARSE_BYTE7
Definition: tfmini.h:42
abi.h
status
uint8_t status
Definition: nps_radio_control_spektrum.c:101
TFMINI_PARSE_HEAD2
@ TFMINI_PARSE_HEAD2
Definition: tfmini.h:36
tfmini_init
void tfmini_init(void)
Initialization function.
Definition: tfmini.c:64
TFMini::strength
uint16_t strength
Definition: tfmini.h:54
TFMINI_PARSE_CHECKSUM
@ TFMINI_PARSE_CHECKSUM
Definition: tfmini.h:43
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
TFMini::mode
uint8_t mode
Definition: tfmini.h:56
TFMINI_PARSE_STRENGTH_L
@ TFMINI_PARSE_STRENGTH_L
Definition: tfmini.h:39
TFMini
Definition: tfmini.h:46
TFMINI_PARSE_DIST_H
@ TFMINI_PARSE_DIST_H
Definition: tfmini.h:38
TFMini::raw_dist
uint16_t raw_dist
Definition: tfmini.h:50
TFMINI_PARSE_HEAD
@ TFMINI_PARSE_HEAD
Definition: tfmini.h:35
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
TFMini::update_agl
bool update_agl
Definition: tfmini.h:57
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
tfmini_event
void tfmini_event(void)
Lidar event function Receive bytes from the UART port and parse them.
Definition: tfmini.c:84
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
TFMINI_PARSE_DIST_L
@ TFMINI_PARSE_DIST_L
Definition: tfmini.h:37
uint8_t
unsigned char uint8_t
Definition: types.h:14
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
TFMini::parse_crc
uint8_t parse_crc
Definition: tfmini.h:49
tfmini_parse
static void tfmini_parse(uint8_t byte)
Parse the lidar bytes 1 by 1.
Definition: tfmini.c:94
TFMINI_PARSE_STRENGTH_H
@ TFMINI_PARSE_STRENGTH_H
Definition: tfmini.h:40
TFMini::compensate_rotation
bool compensate_rotation
Definition: tfmini.h:58
TFMINI_PARSE_MODE
@ TFMINI_PARSE_MODE
Definition: tfmini.h:41
TFMini::distance
float distance
Definition: tfmini.h:55
TFMini::device
struct link_device * device
Definition: tfmini.h:47
state.h
TFMINI_INITIALIZE
@ TFMINI_INITIALIZE
Definition: tfmini.h:34
TFMini::parse_status
enum TFMiniParseStatus parse_status
Definition: tfmini.h:48
tfmini.h
driver for the TFMini lidar
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
tfmini_send_lidar
static void tfmini_send_lidar(struct transport_tx *trans, struct link_device *dev)
Downlink message lidar.
Definition: tfmini.c:50