Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
tfmini_i2c.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 OpenUAS <noreply@openuas.org>
3  * Thanks to Michal Podhradsky for SF11 work done
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  *
22  */
23 
24 
29 #include "generated/airframe.h"
31 #include "subsystems/abi.h"
32 
33 #if PERIODIC_TELEMETRY
35 #include "pprzlink/messages.h"
36 #endif
37 
38 #ifdef SENSOR_SYNC_SEND
40 #endif
41 
42 #ifdef SITL
43 #include "state.h"
44 #endif
45 
46 #ifndef AGL_LIDAR_TFMINI_I2C_ID
47 #define AGL_LIDAR_TFMINI_I2C_ID AGL_LIDAR_TFMINI_ID
48 #endif
49 
50 // Check if an I2C device is selected
51 #ifndef TFMINI_I2C_DEV
52 #error TFMINI_I2C_DEV needs to be defined
53 #endif
54 PRINT_CONFIG_VAR(TFMINI_I2C_DEV)
55 
56 /* TODO: not all possible sensor features are implemented(Yet), only basic functionality to get distance */
57 
58 // Default base address on 8 bits
59 #ifndef TFMINI_I2C_ADDR
60 #define TFMINI_I2C_ADDR 0x10
61 #endif
62 
63 #define TFMINI_I2C_REG_ADDR 0x26
64 #define TFMINI_I2C_REG_VAL 0x10
65 #define TFMINI_I2C_READ_ADDR 0x10
66 
67 #define DISTANCE_MODE_REG_ADDR 0x50
68 #define DISTANCE_MODE_SHORT 0x00 // for 0.3-2m
69 #define DISTANCE_MODE_MIDDLE 0x03 // for 0.5-5m
70 #define DISTANCE_MODE_LONG 0x07 // for 1-12m
71 
72 #define DETECTION_PATTERN_REG_ADDR 0x51
73 #define AUTOMATIC_DETECTION_PATTERN 0x00
74 #define FIXED_DETECTION_PATTERN 0x01
75 
76 #define SETTING_OF_RANGE_LIMIT_REG_ADDR 0x55
77 #define RANGE_LIMIT_DISABLED 0x00
78 #define RANGE_LIMIT_ENABLED 0x01
79 
80 #define RANGE_OUTPUT_LIMIT_THRESHOLD_REG_ADDR_L 0x56
81 #define RANGE_OUTPUT_LIMIT_THRESHOLD_REG_ADDR_H 0x57
82 
83 //Settable 0x0000~0xFFFF
84 //Unit: mm12000(DEC)
85 #define LOWER_LIMIT_OF_SIGNAL_STRENGTH_THRESHOLD_REG_ADDR_L 0x58
86 #define LOWER_LIMIT_OF_SIGNAL_STRENGTH_THRESHOLD_REG_ADDR_H 0x59
87 
88 //settable 0x0000~0xFFFF
89 //When Strength is lower than that value, the distance output is 0xFFFF, and is not credible to be used as flag.20(DEC)
90 #define UPPER_LIMIT_OF_SIGNAL_STRENGTH_THRESHOLD_REG_ADDR_L 0x5A
91 #define UPPER_LIMIT_OF_SIGNAL_STRENGTH_THRESHOLD_REG_ADDR_H 0x5B
92 
93 //Settable 0x0000~0xFFFF
94 #define OUTPUT_VALUE_OF_SIGNAL_STRENGTH_THRESHOLD_AT_THE_HIGHEST_POINT_REG_ADDR_L 0x5C
95 #define OUTPUT_VALUE_OF_SIGNAL_STRENGTH_THRESHOLD_AT_THE_HIGHEST_POINT_REG_ADDR_H 0x5D
96 
97 //Settable 0x0000~0xFFFF
98 #define UNIT_OF_DISTANCE_REG_ADDR 0x66
99 #define UNIT_OF_DISTANCE_MM 0x00 //output millimeter (mm)
100 #define UNIT_OF_DISTANCE_CM 0x01 // outout centimeter (cm)
101 
102 #define SLAVE_ADDRESS_REG_ADDR 0x26 //settable 0x10~0x78 default 0x10
103 
104 #define TRIGGER_MODE_REG_ADDR 0x27
105 #define USE_EXTERNAL_TRIGGER 0 // max100Hz default 0
106 #define EXTERNAL_TRIGGER_REG_ADDR 0x01 //set to 0x01 Command for one single measurement
107 
108 #define DEVICE_RESET_ADDR 0x70 //setable 0x02 //All settings are reset to the default(excluding slave address and trigger mode)
109 
111 #ifndef TFMINI_I2C_OFFSET
112 #define TFMINI_I2C_OFFSET 0.0f
113 #endif
114 
116 #ifndef TFMINI_I2C_MIN_RANGE
117 #define TFMINI_I2C_MIN_RANGE 0.3f //Default for TFMini is 30cm(old firmware?), for TFMini Plus 10cm, sensors cannot measure a range closer than that
118 #endif
119 
121 #ifndef TFMINI_I2C_MAX_RANGE
122 #define TFMINI_I2C_MAX_RANGE 6.0f //Reasonable default for it is maximum value for both inside and outside lighting conditions in day environment
123 #endif
124 
126 
130 static void tfmini_i2c_send_lidar(struct transport_tx *trans, struct link_device *dev)
131 {
132  uint8_t trans_status = tfmini_i2c.trans.status;
133  pprz_msg_send_LIDAR(trans, dev, AC_ID,
134  &tfmini_i2c.dist,
136  &trans_status);
137 }
138 
142 void tfmini_i2c_init(void)
143 {
146 
147  tfmini_i2c.raw_dist = 0;
148  tfmini_i2c.strength = 0; //Not implemented
149  tfmini_i2c.dist = 0.0f;
151  tfmini_i2c.update_agl = USE_TFMINI_I2C_AGL;
152  tfmini_i2c.compensate_rotation = TFMINI_I2C_COMPENSATE_ROTATION;
153 
155 
156 #if PERIODIC_TELEMETRY
158 #endif
159 }
160 
168 {
169  switch (tfmini_i2c.trans.status) {
170  case I2CTransPending:
171  // wait and do nothing
172  break;
173  case I2CTransRunning:
174  // wait and do nothing
175  break;
176  case I2CTransSuccess:
177  // set to done
179  break;
180  case I2CTransFailed:
181  // set to done
183  break;
184  case I2CTransDone:
185  // do nothing
186  break;
187  default:
188  break;
189  }
190 }
191 
197 {
198 
199 #ifndef SITL
200  switch (tfmini_i2c.status) {
201  case TFMINI_I2C_ACQUIRE:
203  tfmini_i2c.trans.buf[0] = 0x01; // sets register pointer to results register
204  tfmini_i2c.trans.buf[1] = 0x02;
205  tfmini_i2c.trans.buf[2] = 0x07;
206  if (i2c_blocking_transceive(&TFMINI_I2C_DEV, &tfmini_i2c.trans, tfmini_i2c.addr, 3, 7)) {
208  }
209  }; //fallthrough
210  case TFMINI_I2C_PARSE: {
212  uint32_t now_ts = get_sys_time_usec();
213 
214  tfmini_i2c.dist = ((float)(tfmini_i2c.raw_dist))*0.01f;
215 
217 
219 
220  //Compensate AGL measurement for body rotation
222  float phi = stateGetNedToBodyEulers_f()->phi;
223  float theta = stateGetNedToBodyEulers_f()->theta;
224  float gain = (float)fabs((double)(cosf(phi) * cosf(theta)));
225  tfmini_i2c.dist = tfmini_i2c.dist * gain;
226  }
227 
228  // Datasheet states FFFF is the output if a valid distance value could not be aquired but depend on not implemented on setting
229  //if (tfmini.dist != 0xFFFF) {
230  if (tfmini_i2c.update_agl) {
231  AbiSendMsgAGL(AGL_LIDAR_TFMINI_I2C_ID, now_ts, tfmini_i2c.dist);
232  }
233  //}
234 
236  break;
237  }
238  default:
239  break;
240  }
241 #else // SITL
243 #endif // SITL
244 }
245 
246 //Sonar message can be misused used to debug raw and scaled
248 {
249 #ifdef SENSOR_SYNC_SEND
251 #endif
252 }
uint16_t
unsigned short uint16_t
Definition: types.h:16
i2c_transaction::buf
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
TFMiniI2C::status
enum TFMiniI2CStatus status
Definition: tfmini_i2c.h:45
TFMINI_I2C_OFFSET
#define TFMINI_I2C_OFFSET
Ranger offset value for what considered the distance should be zero, e.g. high landing gear to tarmac...
Definition: tfmini_i2c.c:112
tfmini_i2c_send_lidar
static void tfmini_i2c_send_lidar(struct transport_tx *trans, struct link_device *dev)
Send measured value and status information so it can be read back in e.g.
Definition: tfmini_i2c.c:130
TFMiniI2C::addr
uint8_t addr
Definition: tfmini_i2c.h:44
abi.h
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
tfmini_i2c
struct TFMiniI2C tfmini_i2c
Definition: tfmini_i2c.c:125
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
TFMiniI2C::update_agl
bool update_agl
Do or don't update AGL ABI message.
Definition: tfmini_i2c.h:50
TFMINI_I2C_ADDR
#define TFMINI_I2C_ADDR
Definition: tfmini_i2c.c:60
I2CTransFailed
@ I2CTransFailed
transaction failed
Definition: i2c.h:58
TFMiniI2C::raw_dist
uint16_t raw_dist
raw non scaled value from sensor
Definition: tfmini_i2c.h:46
uint32_t
unsigned long uint32_t
Definition: types.h:18
i2c_blocking_transceive
bool i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r)
Submit a write/read transaction and wait for it to complete.
Definition: i2c.c:403
TFMINI_I2C_PARSE
@ TFMINI_I2C_PARSE
Definition: tfmini_i2c.h:39
tfmini_i2c.h
Driver for the TFMini ranging device when used via I2C bus.
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
I2CTransSuccess
@ I2CTransSuccess
transaction successfully finished by I2C driver
Definition: i2c.h:57
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
TFMiniI2C::strength
uint16_t strength
strength of reflect signal, not implemented ATM
Definition: tfmini_i2c.h:47
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
TFMiniI2C
Definition: tfmini_i2c.h:42
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
TFMiniI2C::trans
struct i2c_transaction trans
Definition: tfmini_i2c.h:43
I2CTransRunning
@ I2CTransRunning
transaction is currently ongoing
Definition: i2c.h:56
uint8_t
unsigned char uint8_t
Definition: types.h:14
AGL_LIDAR_TFMINI_I2C_ID
#define AGL_LIDAR_TFMINI_I2C_ID
Definition: tfmini_i2c.c:47
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
i2c_transaction::status
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
tfmini_i2c_event
void tfmini_i2c_event(void)
Ranger event function Basically just check the progress of the transation to prevent overruns during ...
Definition: tfmini_i2c.c:167
TFMINI_I2C_ACQUIRE
@ TFMINI_I2C_ACQUIRE
Definition: tfmini_i2c.h:38
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
tfmini_i2c_init
void tfmini_i2c_init(void)
Set the default values at initialization.
Definition: tfmini_i2c.c:142
TFMINI_I2C_MAX_RANGE
#define TFMINI_I2C_MAX_RANGE
The maximum range for the device to be able to measure.
Definition: tfmini_i2c.c:122
I2CTransPending
@ I2CTransPending
transaction is pending in queue
Definition: i2c.h:55
tfmini_i2c_periodic
void tfmini_i2c_periodic(void)
Get the ranger current distance value.
Definition: tfmini_i2c.c:196
I2CTransDone
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
state.h
TFMiniI2C::offset
float offset
offset to what one considers a zero distance to sensor in [m]
Definition: tfmini_i2c.h:49
TFMiniI2C::compensate_rotation
bool compensate_rotation
Definition: tfmini_i2c.h:51
tfmini_i2c_downlink
void tfmini_i2c_downlink(void)
Definition: tfmini_i2c.c:247
TFMINI_I2C_MIN_RANGE
#define TFMINI_I2C_MIN_RANGE
The minimum range for the device to be able to measure.
Definition: tfmini_i2c.c:117
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
TFMiniI2C::dist
float dist
Sacled distance measured in [m].
Definition: tfmini_i2c.h:48