Paparazzi UAS  v6.0_unstable-38-g4d1c433-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
sonar_vl53l1x.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Tom van Dijk <tomvand@users.noreply.github.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 
23 #include "sonar_vl53l1x.h"
24 
25 #include "mcu_periph/i2c.h"
27 #include "subsystems/abi.h"
28 
29 #ifdef SITL
30 #include "state.h"
31 #endif
32 
34 
35 
36 #ifndef SONAR_VL53L1X_I2C_ADDR
37 #define SONAR_VL53L1X_I2C_ADDR VL53L1_DEFAULT_ADDRESS
38 #endif
39 
40 // Signed distance offset in mm
41 #ifndef SONAR_VL53L1X_OFFSET
42 #define SONAR_VL53L1X_OFFSET 0
43 #endif
44 
45 
46 /* VL53L1X configuration */
47 // Time budget for single measurement
48 // Allowed values: 15, 20, 33, 50, 100, 200, 500
49 // see VL53L1X_SetTimingBudgetInMs
50 #ifndef SONAR_VL53L1X_TIMINGBUDGET_MS
51 #define SONAR_VL53L1X_TIMINGBUDGET_MS 100
52 #endif
53 
54 // Allowed values: 1 (short, max ~1.3m), 2 (long, max ~4m)
55 // see VL53L1X_SetDistanceMode
56 #ifndef SONAR_VL53L1X_DISTANCEMODE
57 #define SONAR_VL53L1X_DISTANCEMODE 2
58 #endif
59 
60 // Time between measurements
61 // Should be larger than or equal to timing budget
62 // see VL53L1X_SetInterMeasurementInMs
63 // Note: may be limited by module periodic frequency
64 #ifndef SONAR_VL53L1X_INTERMEASUREMENT_MS
65 #define SONAR_VL53L1X_INTERMEASUREMENT_MS SONAR_VL53L1X_TIMINGBUDGET_MS
66 #endif
67 #if SONAR_VL53L1X_INTERMEASUREMENT_MS < SONAR_VL53L1X_TIMINGBUDGET_MS
68 #warning SONAR_VL53L1X_INTERMEASUREMENT_MS should be greater than or equal to SONAR_VL53L1X_TIMINGBUDGET_MS
69 #endif
70 
71 
73 
74 
75 static void sonar_vl53l1x_publish(uint16_t range_mm)
76 {
77  float range_ofs_m = (range_mm + sonar_vl53l1x.offset_mm) * 1.0e-3f;
78 
79  // Send ABI message
80  uint32_t now_ts = get_sys_time_usec();
81  AbiSendMsgAGL(AGL_VL53L1X_ID, now_ts, range_ofs_m);
82 
83 #ifdef SENSOR_SYNC_SEND_SONAR
84  // Send Telemetry report
85  DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &range_mm, &range_ofs_m);
86 #endif
87 }
88 
89 
91 {
92  // Set up structs
93  sonar_vl53l1x.dev.i2c_p = &SONAR_VL53L1X_I2C_DEV;
96 
97  /* Initialize sensor */
98 #ifndef SITL
100 #endif
101 }
102 
103 
105 {
106 #ifndef SITL
109  }
110 #else // SITL
111  float range_mm = stateGetPositionEnu_f()->z * 1.0e3f;
112  if (range_mm > 0.0f && range_mm < 5.0f) {
113  sonar_vl53l1x_publish(range_mm)
114  }
115 #endif // SITL
116 }
117 
119 {
120 #ifndef SITL
121  uint16_t range_mm;
122  bool new_data = false;
123  VL53L1X_NonBlocking_ReadDataEvent(&sonar_vl53l1x.dev, &range_mm, &new_data);
124  if (new_data) {
125  sonar_vl53l1x_publish(range_mm);
126  }
127 #endif
128 }
bool VL53L1X_NonBlocking_ReadDataEvent(VL53L1_DEV dev, uint16_t *distance_mm, bool *new_data)
Implement non-blocking read sequence The data reading actually starts when the read_state is set to V...
void sonar_vl53l1x_event(void)
Main include for ABI (AirBorneInterface).
#define SONAR_VL53L1X_DISTANCEMODE
Definition: sonar_vl53l1x.c:57
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
struct sonar_vl53l1x_dev sonar_vl53l1x
Definition: sonar_vl53l1x.c:72
static void sonar_vl53l1x_publish(uint16_t range_mm)
Definition: sonar_vl53l1x.c:75
#define SONAR_VL53L1X_TIMINGBUDGET_MS
Definition: sonar_vl53l1x.c:51
bool VL53L1X_NonBlocking_RequestData(VL53L1_DEV dev)
Request a new reading.
void sonar_vl53l1x_read(void)
bool VL53L1X_NonBlocking_IsIdle(VL53L1_DEV dev)
Test is read status is on idle.
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
struct i2c_transaction i2c_trans
#define SONAR_VL53L1X_I2C_ADDR
Definition: sonar_vl53l1x.c:37
struct i2c_periph * i2c_p
VL53L1_Dev_t dev
Definition: sonar_vl53l1x.h:29
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
void sonar_vl53l1x_init(void)
Definition: sonar_vl53l1x.c:90
API to get/set the generic vehicle states.
#define SONAR_VL53L1X_INTERMEASUREMENT_MS
Definition: sonar_vl53l1x.c:65
#define AGL_VL53L1X_ID
float z
in meters
#define SONAR_VL53L1X_OFFSET
Definition: sonar_vl53l1x.c:42
void VL53L1X_BootDevice(VL53L1_DEV dev, uint16_t TimingBudgetInMs, uint16_t DistanceMode, uint32_t InterMeasurementInMs)
Implement boot sequence of VL53L1 device as described in documentation See VL53L1X_SetTimingBudgetInM...
Definition: vl53l1x_api.c:262
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
Non-blocking runtime functions for the VL53L1X.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
Architecture independent I2C (Inter-Integrated Circuit Bus) API.