Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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/pca95xx.h"
30 #include "modules/core/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 PCA95XX_P4
68 #define MULTI_RANGER_PIN_BACK PCA95XX_P1
69 #define MULTI_RANGER_PIN_RIGHT PCA95XX_P2
70 #define MULTI_RANGER_PIN_LEFT PCA95XX_P6
71 #define MULTI_RANGER_PIN_UP PCA95XX_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 pca95xx pca;
121 };
122 
123 static struct cf_deck_multi_ranger multi_ranger;
124 
129 {
130 #ifndef SITL
132 #endif
133 }
134 
139 {
141 
142  // init I/O expander
143  pca95xx_init(&multi_ranger.pca, &(MULTI_RANGER_I2C_DEV), PCA95XX_DEFAULT_ADDRESS);
144 #if MULTI_RANGER_EARLY_INIT
145  pca95xx_configure(&multi_ranger.pca, ~(MULTI_RANGER_PIN_ALL), true); // configure output
146  pca95xx_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  pca95xx_configure(&multi_ranger.pca, ~(MULTI_RANGER_PIN_ALL), false); // configure output
187  break;
189  pca95xx_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 
Main include for ABI (AirBorneInterface).
#define OBS_DETECTION_MULTI_RANGER_DECK_ID
void multi_ranger_report(void)
#define MULTI_RANGER_INTERMEASUREMENT_MS
float distance
raw distance measurement
#define MULTI_RANGER_PIN_LEFT
static const float multi_ranger_array_orientation[][2]
MultiRangerDev
@ MULTI_RANGER_BACK
@ MULTI_RANGER_FRONT
@ MULTI_RANGER_NB
@ MULTI_RANGER_LEFT
@ MULTI_RANGER_UP
@ MULTI_RANGER_RIGHT
enum MultiRangerStatus status
void multi_ranger_init(void)
Module init.
#define MULTI_RANGER_PIN_FRONT
float bearing
bearing [rad] relative to body frame
uint8_t read_state
current reading state
static void multi_ranger_boot_device(VL53L1_Dev_t *dev UNUSED)
Boot a device.
MultiRangerStatus
@ MULTI_RANGER_CONF_RIGHT
@ MULTI_RANGER_CONF_FRONT
@ MULTI_RANGER_CONF_LEFT
@ MULTI_RANGER_READ_RIGHT
@ MULTI_RANGER_CONF_IO
@ MULTI_RANGER_UNINIT
@ MULTI_RANGER_CONF_BACK
@ MULTI_RANGER_READ_FRONT
@ MULTI_RANGER_READ_LEFT
@ MULTI_RANGER_CONF_UP
@ MULTI_RANGER_READ_UP
@ MULTI_RANGER_READ_BACK
VL53L1_Dev_t dev
sensor driver
#define MULTI_RANGER_PIN_RIGHT
static bool multi_ranger_read(struct SingleRanger *ranger UNUSED)
Read data from a device.
struct SingleRanger ranger[MULTI_RANGER_NB]
sensor array
void multi_ranger_event(void)
#define MULTI_RANGER_ARRAY_ORIENTATION
#define MULTI_RANGER_TIMINGBUDGET_MS
void multi_ranger_periodic(void)
Module periodic function.
#define MULTI_RANGER_PIN_BACK
#define MULTI_RANGER_PIN_ALL
static struct cf_deck_multi_ranger multi_ranger
#define MULTI_RANGER_DISTANCEMODE
float azimuth
azimuth [rad] relative to body frame
uint8_t last_wp UNUSED
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
void pca95xx_init(struct pca95xx *dev, struct i2c_periph *i2c_p, uint8_t addr)
Init PCA95XX.
Definition: pca95xx.c:30
bool pca95xx_configure(struct pca95xx *dev, uint8_t val, bool blocking)
Configure PCA95XX.
Definition: pca95xx.c:42
bool pca95xx_set_output(struct pca95xx *dev, uint8_t mask, bool blocking)
Set output value.
Definition: pca95xx.c:60
Driver for the 8-bit I/O expander based on i2c.
#define PCA95XX_DEFAULT_ADDRESS
Definition: pca95xx.h:33
PCA95XX structure.
Definition: pca95xx.h:53
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
struct i2c_transaction i2c_trans
enum VL53L1_ReadStatus read_status
#define VL53L1_DEFAULT_ADDRESS
struct i2c_periph * i2c_p
@ VL53L1_READ_IDLE
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
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
Functions definition.
bool VL53L1X_NonBlocking_RequestData(VL53L1_DEV dev)
Request a new reading.
bool VL53L1X_NonBlocking_IsIdle(VL53L1_DEV dev)
Test is read status is on idle.
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...
Non-blocking runtime functions for the VL53L1X.