Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
cf_deck_multi_ranger.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
27 #include "peripherals/pca95x4.h"
30 #include "subsystems/abi.h"
32 
33 // By default, do early init to be compatible with the flow_deck
34 // Blocking i2c is only possible with ChibiOS
35 // This module should be called before other modules using the VL53L1X sensor
36 // without the possibility to turn off the sensor (i.e. flow_deck)
37 #ifndef MULTI_RANGER_EARLY_INIT
38 #define MULTI_RANGER_EARLY_INIT TRUE
39 #endif
40 
41 /* VL53L1X configuration */
42 // Time budget for single measurement
43 // Allowed values: 15, 20, 33, 50, 100, 200, 500
44 // see VL53L1X_SetTimingBudgetInMs
45 #ifndef MULTI_RANGER_TIMINGBUDGET_MS
46 #define MULTI_RANGER_TIMINGBUDGET_MS 100
47 #endif
48 
49 // Allowed values: 1 (short, max ~1.3m), 2 (long, max ~4m)
50 // see VL53L1X_SetDistanceMode
51 #ifndef MULTI_RANGER_DISTANCEMODE
52 #define MULTI_RANGER_DISTANCEMODE 2
53 #endif
54 
55 // Time between measurements
56 // Should be larger than or equal to timing budget
57 // see VL53L1X_SetInterMeasurementInMs
58 // Note: may be limited by module periodic frequency
59 #ifndef MULTI_RANGER_INTERMEASUREMENT_MS
60 #define MULTI_RANGER_INTERMEASUREMENT_MS MULTI_RANGER_TIMINGBUDGET_MS
61 #endif
62 #if MULTI_RANGER_INTERMEASUREMENT_MS < MULTI_RANGER_TIMINGBUDGET_MS
63 #warning MULTI_RANGER_INTERMEASUREMENT_MS should be greater than or equal to MULTI_RANGER_TIMINGBUDGET_MS
64 #endif
65 
66 // PCA I/O pins to enable sensors
67 #define MULTI_RANGER_PIN_FRONT PCA95X4_P4
68 #define MULTI_RANGER_PIN_BACK PCA95X4_P1
69 #define MULTI_RANGER_PIN_RIGHT PCA95X4_P2
70 #define MULTI_RANGER_PIN_LEFT PCA95X4_P6
71 #define MULTI_RANGER_PIN_UP PCA95X4_P0
72 #define MULTI_RANGER_PIN_ALL (MULTI_RANGER_PIN_FRONT | MULTI_RANGER_PIN_BACK | MULTI_RANGER_PIN_RIGHT | MULTI_RANGER_PIN_LEFT | MULTI_RANGER_PIN_UP)
73 
77 #ifdef MULTI_RANGER_EXTRA_DEV
78  MULTI_RANGER_CONF_EXTRA,
79 #endif
90 };
91 
99 };
100 
101 // Default orientation (azimuth, bearing) in rad of sensors relative to body
102 #ifndef MULTI_RANGER_ARRAY_ORIENTATION
103 #define MULTI_RANGER_ARRAY_ORIENTATION {{0.f, 0.f}, {0.f, M_PI}, {0.f, M_PI_2}, {0.f, -M_PI_2}, {M_PI_2, 0.f}}
104 #endif
106 
107 struct SingleRanger {
109  float distance;
110  float azimuth;
111  float bearing;
113 };
114 
117  // VL53L1X devices
119  // I/O expander
120  struct pca95x4 pca;
121 };
122 
124 
129 {
130 #ifndef SITL
132 #endif
133 }
134 
139 {
141 
142  // init I/O expander
143  pca95x4_init(&multi_ranger.pca, &(MULTI_RANGER_I2C_DEV), PCA95X4_DEFAULT_ADDRESS);
144 #if MULTI_RANGER_EARLY_INIT
145  pca95x4_configure(&multi_ranger.pca, ~(MULTI_RANGER_PIN_ALL), true); // configure output
146  pca95x4_set_output(&multi_ranger.pca, ~(MULTI_RANGER_PIN_ALL), true); // select none
147 #endif
148 
149  // init vl53l1x array
150  for (uint8_t i = 0; i < MULTI_RANGER_NB; i++) {
151  multi_ranger.ranger[i].dev.i2c_p = &(MULTI_RANGER_I2C_DEV);
154  multi_ranger.ranger[i].distance = 0.f;
157  }
158 }
159 
165 {
166 #ifndef SITL
167  uint16_t range_mm;
168  bool new_data = false;
169  bool ret = VL53L1X_NonBlocking_ReadDataEvent(&ranger->dev, &range_mm, &new_data);
170  if (new_data) {
171  ranger->distance = range_mm / 1000.f;
172  AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_MULTI_RANGER_DECK_ID, ranger->distance, ranger->azimuth, ranger->bearing);
173  }
174  return ret;
175 #endif
176 }
177 
182 {
183  switch (multi_ranger.status) {
184  case MULTI_RANGER_UNINIT:
185  pca95x4_configure(&multi_ranger.pca, ~(MULTI_RANGER_PIN_ALL), false); // configure output
187  break;
189  pca95x4_set_output(&multi_ranger.pca, MULTI_RANGER_PIN_FRONT, false); // select front
191  break;
196  break;
201  break;
206  break;
211  break;
215  break;
220  }
221  break;
226  }
227  break;
232  }
233  break;
238  }
239  break;
244  }
245  break;
246  default:
247  break;
248  }
249 }
250 
252 {
253  // call non blocking read/event functions
259 }
260 
262 {
263  float dist_array[MULTI_RANGER_NB];
264  for (int i = 0; i < MULTI_RANGER_NB; i++) {
265  dist_array[i] = multi_ranger.ranger[i].distance;
266  }
267  DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, MULTI_RANGER_NB, dist_array);
268 }
269 
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...
unsigned short uint16_t
Definition: types.h:16
PCA95X4 structure.
Definition: pca95x4.h:53
Driver for the 8-bit I/O expander based on i2c.
#define MULTI_RANGER_PIN_ALL
bool pca95x4_set_output(struct pca95x4 *dev, uint8_t mask, bool blocking)
Set output value.
Definition: pca95x4.c:60
static bool multi_ranger_read(struct SingleRanger *ranger UNUSED)
Read data from a device.
uint8_t last_wp UNUSED
Definition: navigation.c:92
Main include for ABI (AirBorneInterface).
void multi_ranger_event(void)
#define MULTI_RANGER_PIN_FRONT
void pca95x4_init(struct pca95x4 *dev, struct i2c_periph *i2c_p, uint8_t addr)
Init PCA95X4.
Definition: pca95x4.c:30
#define OBS_DETECTION_MULTI_RANGER_DECK_ID
VL53L1_Dev_t dev
sensor driver
float azimuth
azimuth [rad] relative to body frame
#define MULTI_RANGER_TIMINGBUDGET_MS
float bearing
bearing [rad] relative to body frame
#define PCA95X4_DEFAULT_ADDRESS
Definition: pca95x4.h:33
MultiRangerStatus
#define MULTI_RANGER_INTERMEASUREMENT_MS
#define VL53L1_DEFAULT_ADDRESS
bool VL53L1X_NonBlocking_RequestData(VL53L1_DEV dev)
Request a new reading.
void multi_ranger_init(void)
Module init.
#define MULTI_RANGER_PIN_BACK
float distance
raw distance measurement
uint8_t read_state
current reading state
bool VL53L1X_NonBlocking_IsIdle(VL53L1_DEV dev)
Test is read status is on idle.
#define MULTI_RANGER_PIN_LEFT
enum VL53L1_ReadStatus read_status
void multi_ranger_periodic(void)
Module periodic function.
struct i2c_transaction i2c_trans
MultiRangerDev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
struct SingleRanger ranger[MULTI_RANGER_NB]
sensor array
struct i2c_periph * i2c_p
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
bool pca95x4_configure(struct pca95x4 *dev, uint8_t val, bool blocking)
Configure PCA95X4.
Definition: pca95x4.c:42
unsigned char uint8_t
Definition: types.h:14
static const float multi_ranger_array_orientation[][2]
Functions definition.
void multi_ranger_report(void)
static struct cf_deck_multi_ranger multi_ranger
#define MULTI_RANGER_PIN_RIGHT
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:255
static void multi_ranger_boot_device(VL53L1_Dev_t *dev UNUSED)
Boot a device.
#define MULTI_RANGER_DISTANCEMODE
#define MULTI_RANGER_ARRAY_ORIENTATION
Non-blocking runtime functions for the VL53L1X.
enum MultiRangerStatus status