Paparazzi UAS  v5.18.0_stable
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 "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 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 
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 
cf_deck_multi_ranger
Definition: cf_deck_multi_ranger.c:115
uint16_t
unsigned short uint16_t
Definition: types.h:16
pca95xx_set_output
bool pca95xx_set_output(struct pca95xx *dev, uint8_t mask, bool blocking)
Set output value.
Definition: pca95xx.c:60
MULTI_RANGER_READ_BACK
@ MULTI_RANGER_READ_BACK
Definition: cf_deck_multi_ranger.c:86
pca95xx_configure
bool pca95xx_configure(struct pca95xx *dev, uint8_t val, bool blocking)
Configure PCA95XX.
Definition: pca95xx.c:42
MultiRangerDev
MultiRangerDev
Definition: cf_deck_multi_ranger.c:92
MULTI_RANGER_UP
@ MULTI_RANGER_UP
Definition: cf_deck_multi_ranger.c:97
multi_ranger_read
static bool multi_ranger_read(struct SingleRanger *ranger UNUSED)
Read data from a device.
Definition: cf_deck_multi_ranger.c:164
VL53L1X_BootDevice
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
abi.h
MULTI_RANGER_LEFT
@ MULTI_RANGER_LEFT
Definition: cf_deck_multi_ranger.c:96
MULTI_RANGER_CONF_BACK
@ MULTI_RANGER_CONF_BACK
Definition: cf_deck_multi_ranger.c:81
MULTI_RANGER_READ_RIGHT
@ MULTI_RANGER_READ_RIGHT
Definition: cf_deck_multi_ranger.c:87
cf_deck_multi_ranger::pca
struct pca95xx pca
Definition: cf_deck_multi_ranger.c:120
MULTI_RANGER_READ_UP
@ MULTI_RANGER_READ_UP
Definition: cf_deck_multi_ranger.c:89
MULTI_RANGER_PIN_RIGHT
#define MULTI_RANGER_PIN_RIGHT
Definition: cf_deck_multi_ranger.c:69
vl53l1x_nonblocking.h
Non-blocking runtime functions for the VL53L1X.
MULTI_RANGER_PIN_BACK
#define MULTI_RANGER_PIN_BACK
Definition: cf_deck_multi_ranger.c:68
multi_ranger_boot_device
static void multi_ranger_boot_device(VL53L1_Dev_t *dev UNUSED)
Boot a device.
Definition: cf_deck_multi_ranger.c:128
SingleRanger::bearing
float bearing
bearing [rad] relative to body frame
Definition: cf_deck_multi_ranger.c:111
vl53l1x_api.h
Functions definition.
MULTI_RANGER_READ_FRONT
@ MULTI_RANGER_READ_FRONT
Definition: cf_deck_multi_ranger.c:85
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:96
multi_ranger_report
void multi_ranger_report(void)
Definition: cf_deck_multi_ranger.c:261
multi_ranger_event
void multi_ranger_event(void)
Definition: cf_deck_multi_ranger.c:251
MULTI_RANGER_PIN_ALL
#define MULTI_RANGER_PIN_ALL
Definition: cf_deck_multi_ranger.c:72
SingleRanger::dev
VL53L1_Dev_t dev
sensor driver
Definition: cf_deck_multi_ranger.c:108
PCA95XX_DEFAULT_ADDRESS
#define PCA95XX_DEFAULT_ADDRESS
Definition: pca95xx.h:33
VL53L1_Dev_t::read_status
enum VL53L1_ReadStatus read_status
Definition: vl53l1_platform.h:60
SingleRanger::read_state
uint8_t read_state
current reading state
Definition: cf_deck_multi_ranger.c:112
cf_deck_multi_ranger::status
enum MultiRangerStatus status
Definition: cf_deck_multi_ranger.c:116
VL53L1_DEFAULT_ADDRESS
#define VL53L1_DEFAULT_ADDRESS
Definition: vl53l1_platform.h:40
VL53L1_READ_IDLE
@ VL53L1_READ_IDLE
Definition: vl53l1_platform.h:43
MULTI_RANGER_RIGHT
@ MULTI_RANGER_RIGHT
Definition: cf_deck_multi_ranger.c:95
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
VL53L1X_NonBlocking_RequestData
bool VL53L1X_NonBlocking_RequestData(VL53L1_DEV dev)
Request a new reading.
Definition: vl53l1x_nonblocking.c:195
VL53L1_Dev_t::i2c_trans
struct i2c_transaction i2c_trans
Definition: vl53l1_platform.h:52
uint8_t
unsigned char uint8_t
Definition: types.h:14
pca95xx_init
void pca95xx_init(struct pca95xx *dev, struct i2c_periph *i2c_p, uint8_t addr)
Init PCA95XX.
Definition: pca95xx.c:30
MultiRangerStatus
MultiRangerStatus
Definition: cf_deck_multi_ranger.c:74
SingleRanger::distance
float distance
raw distance measurement
Definition: cf_deck_multi_ranger.c:109
MULTI_RANGER_CONF_FRONT
@ MULTI_RANGER_CONF_FRONT
Definition: cf_deck_multi_ranger.c:80
MULTI_RANGER_DISTANCEMODE
#define MULTI_RANGER_DISTANCEMODE
Definition: cf_deck_multi_ranger.c:52
MULTI_RANGER_READ_LEFT
@ MULTI_RANGER_READ_LEFT
Definition: cf_deck_multi_ranger.c:88
MULTI_RANGER_TIMINGBUDGET_MS
#define MULTI_RANGER_TIMINGBUDGET_MS
Definition: cf_deck_multi_ranger.c:46
i2c_transaction::slave_addr
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
VL53L1X_NonBlocking_ReadDataEvent
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...
Definition: vl53l1x_nonblocking.c:144
OBS_DETECTION_MULTI_RANGER_DECK_ID
#define OBS_DETECTION_MULTI_RANGER_DECK_ID
Definition: abi_sender_ids.h:458
multi_ranger_init
void multi_ranger_init(void)
Module init.
Definition: cf_deck_multi_ranger.c:138
SingleRanger
Definition: cf_deck_multi_ranger.c:107
MULTI_RANGER_NB
@ MULTI_RANGER_NB
Definition: cf_deck_multi_ranger.c:98
pca95xx.h
MULTI_RANGER_INTERMEASUREMENT_MS
#define MULTI_RANGER_INTERMEASUREMENT_MS
Definition: cf_deck_multi_ranger.c:60
VL53L1X_NonBlocking_IsIdle
bool VL53L1X_NonBlocking_IsIdle(VL53L1_DEV dev)
Test is read status is on idle.
Definition: vl53l1x_nonblocking.c:190
MULTI_RANGER_CONF_IO
@ MULTI_RANGER_CONF_IO
Definition: cf_deck_multi_ranger.c:76
cf_deck_multi_ranger::ranger
struct SingleRanger ranger[MULTI_RANGER_NB]
sensor array
Definition: cf_deck_multi_ranger.c:118
multi_ranger_array_orientation
static const float multi_ranger_array_orientation[][2]
Definition: cf_deck_multi_ranger.c:105
MULTI_RANGER_ARRAY_ORIENTATION
#define MULTI_RANGER_ARRAY_ORIENTATION
Definition: cf_deck_multi_ranger.c:103
MULTI_RANGER_CONF_UP
@ MULTI_RANGER_CONF_UP
Definition: cf_deck_multi_ranger.c:84
MULTI_RANGER_PIN_FRONT
#define MULTI_RANGER_PIN_FRONT
Definition: cf_deck_multi_ranger.c:67
MULTI_RANGER_FRONT
@ MULTI_RANGER_FRONT
Definition: cf_deck_multi_ranger.c:93
multi_ranger_periodic
void multi_ranger_periodic(void)
Module periodic function.
Definition: cf_deck_multi_ranger.c:181
MULTI_RANGER_PIN_LEFT
#define MULTI_RANGER_PIN_LEFT
Definition: cf_deck_multi_ranger.c:70
VL53L1_Dev_t::i2c_p
struct i2c_periph * i2c_p
Definition: vl53l1_platform.h:51
multi_ranger
static struct cf_deck_multi_ranger multi_ranger
Definition: cf_deck_multi_ranger.c:123
SingleRanger::azimuth
float azimuth
azimuth [rad] relative to body frame
Definition: cf_deck_multi_ranger.c:110
VL53L1_Dev_t
Definition: vl53l1_platform.h:50
MULTI_RANGER_CONF_RIGHT
@ MULTI_RANGER_CONF_RIGHT
Definition: cf_deck_multi_ranger.c:82
MULTI_RANGER_UNINIT
@ MULTI_RANGER_UNINIT
Definition: cf_deck_multi_ranger.c:75
MULTI_RANGER_CONF_LEFT
@ MULTI_RANGER_CONF_LEFT
Definition: cf_deck_multi_ranger.c:83
pca95xx
PCA95XX structure.
Definition: pca95xx.h:53
cf_deck_multi_ranger.h
MULTI_RANGER_BACK
@ MULTI_RANGER_BACK
Definition: cf_deck_multi_ranger.c:94