Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-02 05:17:03 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Multi ranger deck integration (#2524)

Browse Source 
* [driver] factorize some vl53l1x function and use event loop

- the reading of data is still not blocking, but using the
  event loop to perform all reading transactions in a row instead of
  splitting them in several periodic calls
- move boot and reading sequence to the peripheral driver
- add an auto address increment option to support several sensors

[vl53l1] change boot return status to void

* [module] support for the multi-ranger deck for crazyflie

- add driver of the pca95x4 I/O expander
- read and publish data of the 5 vl53l1x sensor of the deck

* [conf] update config of crazyflie

- update example in Enac folder
- allow multi-ranger deck with forcefield module
- use same IMU defaults than original crazyflie settings

* [bug] syslink send_message buffer was 2 bytes too short

* [sonar] poll new data at 50 Hz
This commit is contained in:
Gautier Hattenberger
2020-04-22 18:00:48 +02:00
committed by GitHub
parent 66a0f367a7
commit cf5a989072
17 changed files with 752 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
+47 -4
View File
@@ -9,7 +9,7 @@
<configure name="RTOS_DEBUG" value="0"/> <configure name="RTOS_DEBUG" value="0"/>
<target name="ap" board="crazyflie_2.1"> <target name="ap" board="crazyflie_2.1">
<module name="gps" type="datalink"/> <!--module name="gps" type="datalink"/-->
</target> </target>
<target name="nps" board="pc"> <target name="nps" board="pc">
@@ -30,11 +30,12 @@
<define name="IMU_BMI088_Z_SIGN" value="-1"/> <define name="IMU_BMI088_Z_SIGN" value="-1"/>
</module> </module>
<module name="stabilization" type="int_quat"/> <module name="stabilization" type="int_quat"/>
<!--module name="stabilization" type="indi_simple"/-->
<module name="ahrs" type="madgwick"> <module name="ahrs" type="madgwick">
<configure name="AHRS_PROPAGATE_FREQUENCY" value="1000"/> <configure name="AHRS_PROPAGATE_FREQUENCY" value="1000"/>
<!--configure name="USE_MAGNETOMETER" value="FALSE"/--> <!--configure name="USE_MAGNETOMETER" value="FALSE"/-->
</module> </module>
<module name="ins"/> <module name="ins" type="extended"/>
<module name="sys_mon"/> <module name="sys_mon"/>
<module name="air_data"/> <module name="air_data"/>
<module name="baro_bmp3"> <module name="baro_bmp3">
@@ -43,6 +44,14 @@
<define name="BMP3_SLAVE_ADDR" value="BMP3_I2C_ADDR_ALT"/> <define name="BMP3_SLAVE_ADDR" value="BMP3_I2C_ADDR_ALT"/>
</module> </module>
<module name="cf_deck" type="multi_ranger"/>
<module name="range_forcefield">
<define name="RANGE_FORCEFIELD_MAX_VEL" value="0.1"/>
</module>
<module name="sonar" type="vl53l1x"/>
<define name="SENSOR_SYNC_SEND_SONAR"/>
<define name="NO_GPS_NEEDED_FOR_NAV" value="TRUE"/>
</firmware> </firmware>
<firmware name="test_chibios"> <firmware name="test_chibios">
@@ -181,12 +190,45 @@
<define name="PSI_DDGAIN" value="300"/> <define name="PSI_DDGAIN" value="300"/>
</section> </section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.0034724f"/>
<define name="G1_Q" value="0.0052575f"/>
<define name="G1_R" value="-0.0015942f"/>
<define name="G2_R" value="-0.11281f"/>
<define name="FILTER_ROLL_RATE" value="FALSE"/>
<define name="FILTER_PITCH_RATE" value="FALSE"/>
<define name="FILTER_YAW_RATE" value="FALSE"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="3.57f"/>
<define name="REF_ERR_Q" value="3.57f"/>
<define name="REF_ERR_R" value="3.57f"/>
<define name="REF_RATE_P" value="14.0"/>
<define name="REF_RATE_Q" value="14.0"/>
<define name="REF_RATE_R" value="14.0"/>
<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="8.0f"/>
<define name="FILT_CUTOFF_R" value="8.0f"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03149f"/>
<define name="ACT_DYN_Q" value="0.03149f"/>
<define name="ACT_DYN_R" value="0.03149f"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_"> <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="150"/> <define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/> <define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/> <define name="HOVER_KI" value="20"/>
<!-- NOMINAL_HOVER_THROTTLE sets a fixed value instead of the adaptive estimation --> <!-- NOMINAL_HOVER_THROTTLE sets a fixed value instead of the adaptive estimation -->
<!--define name="NOMINAL_HOVER_THROTTLE" value="0.5"/--> <define name="NOMINAL_HOVER_THROTTLE" value="0.75"/>
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
@@ -204,8 +246,9 @@
</section> </section>
<section name="AUTOPILOT"> <section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/> <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/> <define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/> <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section> </section>
conf/modules/cf_deck_multi_ranger.xml
+28
View File
@@ -0,0 +1,28 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="cf_deck_multi_ranger" dir="range_finder">
<doc>
<description>
Multi-ranger deck from Bitcraze for Crazyflie drones
</description>
<configure name="MULTI_RANGER_I2C_DEV" value="i2c1" description="I2C device to use for the multi_ranger deck"/>
</doc>
<header>
<file name="cf_deck_multi_ranger.h"/>
</header>
<init fun="multi_ranger_init()"/>
<periodic fun="multi_ranger_periodic()" freq="50.0" autorun="TRUE"/>
<periodic fun="multi_ranger_report()" freq="5.0" autorun="TRUE"/>
<event fun="multi_ranger_event()"/>
<makefile target="ap">
<configure name="MULTI_RANGER_I2C_DEV" default="i2c1" case="lower|upper"/>
<define name="MULTI_RANGER_I2C_DEV" value="$(MULTI_RANGER_I2C_DEV_LOWER)"/>
<define name="USE_$(MULTI_RANGER_I2C_DEV_UPPER)"/>
<define name="VL53L1X_AUTO_INCR_ADDR"/>
<file name="cf_deck_multi_ranger.c"/>
<file name="pca95x4.c" dir="peripherals"/>
<file name="vl53l1_platform.c" dir="peripherals"/>
<file name="vl53l1x_api.c" dir="peripherals"/>
<file name="vl53l1x_nonblocking.c" dir="peripherals"/>
</makefile>
</module>
conf/modules/range_forcefield.xml
+1 -1
View File
@@ -23,7 +23,7 @@
</dl_settings> </dl_settings>
</settings> </settings>
<depends>laser_range_array</depends> <depends>laser_range_array|cf_deck_multi_ranger</depends>
<header> <header>
<file name="range_forcefield.h"/> <file name="range_forcefield.h"/>
conf/modules/sonar_vl53l1x.xml
+2 -1
View File
@@ -27,7 +27,8 @@
</header> </header>
<init fun="sonar_vl53l1x_init()"/> <init fun="sonar_vl53l1x_init()"/>
<periodic fun="sonar_vl53l1x_read()" period="0.020"/> <periodic fun="sonar_vl53l1x_read()" freq="50."/>
<event fun="sonar_vl53l1x_event()"/>
<makefile target="ap|sim"> <makefile target="ap|sim">
<file name="sonar_vl53l1x.c"/> <file name="sonar_vl53l1x.c"/>
sw/airborne/modules/datalink/bitcraze/syslink_dl.c
+1 -1
View File
@@ -42,7 +42,7 @@ PPRZ_MUTEX(syslink_tx_mtx);
static void send_message(syslink_message_t *msg) static void send_message(syslink_message_t *msg)
{ {
syslink_compute_cksum(msg); syslink_compute_cksum(msg);
uint8_t buf[sizeof(syslink_message_t)]; uint8_t buf[sizeof(syslink_message_t)+2];
buf[0] = syslink_stx[0]; buf[0] = syslink_stx[0];
buf[1] = syslink_stx[1]; buf[1] = syslink_stx[1];
buf[2] = msg->type; buf[2] = msg->type;
sw/airborne/modules/range_finder/cf_deck_multi_ranger.c
+269
View File
@@ -0,0 +1,269 @@
/*
* Copyright (C) Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/** @file "modules/range_finder/cf_deck_multi_ranger.c"
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Multi-ranger deck from Bitcraze for Crazyflie drones
*/
#include "modules/range_finder/cf_deck_multi_ranger.h"
#include "peripherals/pca95x4.h"
#include "peripherals/vl53l1x_nonblocking.h"
#include "peripherals/vl53l1x_api.h"
#include "subsystems/abi.h"
#include "subsystems/datalink/downlink.h"
// By default, do early init to be compatible with the flow_deck
// Blocking i2c is only possible with ChibiOS
// This module should be called before other modules using the VL53L1X sensor
// without the possibility to turn off the sensor (i.e. flow_deck)
#ifndef MULTI_RANGER_EARLY_INIT
#define MULTI_RANGER_EARLY_INIT TRUE
#endif
/* VL53L1X configuration */
// Time budget for single measurement
// Allowed values: 15, 20, 33, 50, 100, 200, 500
// see VL53L1X_SetTimingBudgetInMs
#ifndef MULTI_RANGER_TIMINGBUDGET_MS
#define MULTI_RANGER_TIMINGBUDGET_MS 100
#endif
// Allowed values: 1 (short, max ~1.3m), 2 (long, max ~4m)
// see VL53L1X_SetDistanceMode
#ifndef MULTI_RANGER_DISTANCEMODE
#define MULTI_RANGER_DISTANCEMODE 2
#endif
// Time between measurements
// Should be larger than or equal to timing budget
// see VL53L1X_SetInterMeasurementInMs
// Note: may be limited by module periodic frequency
#ifndef MULTI_RANGER_INTERMEASUREMENT_MS
#define MULTI_RANGER_INTERMEASUREMENT_MS MULTI_RANGER_TIMINGBUDGET_MS
#endif
#if MULTI_RANGER_INTERMEASUREMENT_MS < MULTI_RANGER_TIMINGBUDGET_MS
#warning MULTI_RANGER_INTERMEASUREMENT_MS should be greater than or equal to MULTI_RANGER_TIMINGBUDGET_MS
#endif
// PCA I/O pins to enable sensors
#define MULTI_RANGER_PIN_FRONT PCA95X4_P4
#define MULTI_RANGER_PIN_BACK PCA95X4_P1
#define MULTI_RANGER_PIN_RIGHT PCA95X4_P2
#define MULTI_RANGER_PIN_LEFT PCA95X4_P6
#define MULTI_RANGER_PIN_UP PCA95X4_P0
#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)
enum MultiRangerStatus {
MULTI_RANGER_UNINIT,
MULTI_RANGER_CONF_IO,
#ifdef MULTI_RANGER_EXTRA_DEV
MULTI_RANGER_CONF_EXTRA,
#endif
MULTI_RANGER_CONF_FRONT,
MULTI_RANGER_CONF_BACK,
MULTI_RANGER_CONF_RIGHT,
MULTI_RANGER_CONF_LEFT,
MULTI_RANGER_CONF_UP,
MULTI_RANGER_READ_FRONT,
MULTI_RANGER_READ_BACK,
MULTI_RANGER_READ_RIGHT,
MULTI_RANGER_READ_LEFT,
MULTI_RANGER_READ_UP
};
enum MultiRangerDev {
MULTI_RANGER_FRONT = 0,
MULTI_RANGER_BACK,
MULTI_RANGER_RIGHT,
MULTI_RANGER_LEFT,
MULTI_RANGER_UP,
MULTI_RANGER_NB
};
// Default orientation (azimuth, bearing) in rad of sensors relative to body
#ifndef MULTI_RANGER_ARRAY_ORIENTATION
#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}}
#endif
static const float multi_ranger_array_orientation[][2] = MULTI_RANGER_ARRAY_ORIENTATION;
struct SingleRanger {
VL53L1_Dev_t dev; ///< sensor driver
float distance; ///< raw distance measurement
float azimuth; ///< azimuth [rad] relative to body frame
float bearing; ///< bearing [rad] relative to body frame
uint8_t read_state; ///< current reading state
};
struct cf_deck_multi_ranger {
enum MultiRangerStatus status;
// VL53L1X devices
struct SingleRanger ranger[MULTI_RANGER_NB]; ///< sensor array
// I/O expander
struct pca95x4 pca;
};
static struct cf_deck_multi_ranger multi_ranger;
/**
* Boot a device
*/
static void multi_ranger_boot_device(VL53L1_Dev_t *dev UNUSED)
{
#ifndef SITL
VL53L1X_BootDevice(dev, MULTI_RANGER_TIMINGBUDGET_MS, MULTI_RANGER_DISTANCEMODE, MULTI_RANGER_INTERMEASUREMENT_MS);
#endif
}
/**
* Module init
*/
void multi_ranger_init(void)
{
multi_ranger.status = MULTI_RANGER_UNINIT;
// init I/O expander
pca95x4_init(&multi_ranger.pca, &(MULTI_RANGER_I2C_DEV), PCA95X4_DEFAULT_ADDRESS);
#if MULTI_RANGER_EARLY_INIT
pca95x4_configure(&multi_ranger.pca, ~(MULTI_RANGER_PIN_ALL), true); // configure output
pca95x4_set_output(&multi_ranger.pca, ~(MULTI_RANGER_PIN_ALL), true); // select none
#endif
// init vl53l1x array
for (uint8_t i = 0; i < MULTI_RANGER_NB; i++) {
multi_ranger.ranger[i].dev.i2c_p = &(MULTI_RANGER_I2C_DEV);
multi_ranger.ranger[i].dev.i2c_trans.slave_addr = VL53L1_DEFAULT_ADDRESS;
multi_ranger.ranger[i].dev.read_status = VL53L1_READ_IDLE;
multi_ranger.ranger[i].distance = 0.f;
multi_ranger.ranger[i].azimuth = multi_ranger_array_orientation[i][0];
multi_ranger.ranger[i].bearing = multi_ranger_array_orientation[i][1];
}
}
/**
* Read data from a device
* @return true when read cycle is finished
*/
static bool multi_ranger_read(struct SingleRanger *ranger UNUSED)
{
#ifndef SITL
uint16_t range_mm;
bool new_data = false;
bool ret = VL53L1X_NonBlocking_ReadDataEvent(&ranger->dev, &range_mm, &new_data);
if (new_data) {
ranger->distance = range_mm / 1000.f;
AbiSendMsgOBSTACLE_DETECTION(42, ranger->distance, ranger->azimuth, ranger->bearing);
}
return ret;
#endif
}
/**
* Module periodic function
*/
void multi_ranger_periodic(void)
{
switch (multi_ranger.status) {
case MULTI_RANGER_UNINIT:
pca95x4_configure(&multi_ranger.pca, ~(MULTI_RANGER_PIN_ALL), false); // configure output
multi_ranger.status++;
break;
case MULTI_RANGER_CONF_IO:
pca95x4_set_output(&multi_ranger.pca, MULTI_RANGER_PIN_FRONT, false); // select front
multi_ranger.status++;
break;
case MULTI_RANGER_CONF_FRONT:
multi_ranger_boot_device(&multi_ranger.ranger[MULTI_RANGER_FRONT].dev);
pca95x4_set_output(&multi_ranger.pca, MULTI_RANGER_PIN_FRONT | MULTI_RANGER_PIN_BACK, false); // select back
multi_ranger.status++;
break;
case MULTI_RANGER_CONF_BACK:
multi_ranger_boot_device(&multi_ranger.ranger[MULTI_RANGER_BACK].dev);
pca95x4_set_output(&multi_ranger.pca, MULTI_RANGER_PIN_FRONT | MULTI_RANGER_PIN_BACK | MULTI_RANGER_PIN_RIGHT, false); // select right
multi_ranger.status++;
break;
case MULTI_RANGER_CONF_RIGHT:
multi_ranger_boot_device(&multi_ranger.ranger[MULTI_RANGER_RIGHT].dev);
pca95x4_set_output(&multi_ranger.pca, MULTI_RANGER_PIN_FRONT | MULTI_RANGER_PIN_BACK | MULTI_RANGER_PIN_RIGHT | MULTI_RANGER_PIN_LEFT, false); // select left
multi_ranger.status++;
break;
case MULTI_RANGER_CONF_LEFT:
multi_ranger_boot_device(&multi_ranger.ranger[MULTI_RANGER_LEFT].dev);
pca95x4_set_output(&multi_ranger.pca, MULTI_RANGER_PIN_ALL, false); // select up
multi_ranger.status++;
break;
case MULTI_RANGER_CONF_UP:
multi_ranger_boot_device(&multi_ranger.ranger[MULTI_RANGER_UP].dev);
multi_ranger.status++;
break;
case MULTI_RANGER_READ_FRONT:
if (VL53L1X_NonBlocking_IsIdle(&multi_ranger.ranger[MULTI_RANGER_FRONT].dev)) {
VL53L1X_NonBlocking_RequestData(&multi_ranger.ranger[MULTI_RANGER_FRONT].dev);
multi_ranger.status++;
}
break;
case MULTI_RANGER_READ_BACK:
if (VL53L1X_NonBlocking_IsIdle(&multi_ranger.ranger[MULTI_RANGER_BACK].dev)) {
VL53L1X_NonBlocking_RequestData(&multi_ranger.ranger[MULTI_RANGER_BACK].dev);
multi_ranger.status++;
}
break;
case MULTI_RANGER_READ_RIGHT:
if (VL53L1X_NonBlocking_IsIdle(&multi_ranger.ranger[MULTI_RANGER_RIGHT].dev)) {
VL53L1X_NonBlocking_RequestData(&multi_ranger.ranger[MULTI_RANGER_RIGHT].dev);
multi_ranger.status++;
}
break;
case MULTI_RANGER_READ_LEFT:
if (VL53L1X_NonBlocking_IsIdle(&multi_ranger.ranger[MULTI_RANGER_LEFT].dev)) {
VL53L1X_NonBlocking_RequestData(&multi_ranger.ranger[MULTI_RANGER_LEFT].dev);
multi_ranger.status++;
}
break;
case MULTI_RANGER_READ_UP:
if (VL53L1X_NonBlocking_IsIdle(&multi_ranger.ranger[MULTI_RANGER_UP].dev)) {
VL53L1X_NonBlocking_RequestData(&multi_ranger.ranger[MULTI_RANGER_UP].dev);
multi_ranger.status = MULTI_RANGER_READ_FRONT;
}
break;
default:
break;
}
}
void multi_ranger_event(void)
{
// call non blocking read/event functions
multi_ranger_read(&multi_ranger.ranger[MULTI_RANGER_FRONT]);
multi_ranger_read(&multi_ranger.ranger[MULTI_RANGER_BACK]);
multi_ranger_read(&multi_ranger.ranger[MULTI_RANGER_RIGHT]);
multi_ranger_read(&multi_ranger.ranger[MULTI_RANGER_LEFT]);
multi_ranger_read(&multi_ranger.ranger[MULTI_RANGER_UP]);
}
void multi_ranger_report(void)
{
float dist_array[MULTI_RANGER_NB];
for (int i = 0; i < MULTI_RANGER_NB; i++) {
dist_array[i] = multi_ranger.ranger[i].distance;
}
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, MULTI_RANGER_NB, dist_array);
}
sw/airborne/modules/range_finder/cf_deck_multi_ranger.h
+36
View File
@@ -0,0 +1,36 @@
/*
* Copyright (C) Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/** @file "modules/range_finder/cf_deck_multi_ranger.h"
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Multi-ranger deck from Bitcraze for Crazyflie drones
*/
#ifndef CF_DECK_MULTI_RANGER_H
#define CF_DECK_MULTI_RANGER_H
#include "std.h"
extern void multi_ranger_init(void);
extern void multi_ranger_periodic(void);
extern void multi_ranger_report(void);
extern void multi_ranger_event(void);
#endif // CF_DECK_MULTI_RANGER_H
sw/airborne/modules/sonar/sonar_vl53l1x.c
+16 -32
View File
@@ -34,7 +34,7 @@
#ifndef SONAR_VL53L1X_I2C_ADDR #ifndef SONAR_VL53L1X_I2C_ADDR
#define SONAR_VL53L1X_I2C_ADDR 0x52 #define SONAR_VL53L1X_I2C_ADDR VL53L1_DEFAULT_ADDRESS
#endif #endif
// Signed distance offset in mm // Signed distance offset in mm
@@ -96,17 +96,7 @@ void sonar_vl53l1x_init(void)
/* Initialize sensor */ /* Initialize sensor */
#ifndef SITL #ifndef SITL
uint8_t state; VL53L1X_BootDevice(&sonar_vl53l1x.dev, SONAR_VL53L1X_TIMINGBUDGET_MS, SONAR_VL53L1X_DISTANCEMODE, SONAR_VL53L1X_INTERMEASUREMENT_MS);
do {
VL53L1X_BootState(&sonar_vl53l1x.dev, &state);
} while (!state);
VL53L1X_SensorInit(&sonar_vl53l1x.dev);
/* Configure sensor */
VL53L1X_SetTimingBudgetInMs(&sonar_vl53l1x.dev, SONAR_VL53L1X_TIMINGBUDGET_MS);
VL53L1X_SetDistanceMode(&sonar_vl53l1x.dev, SONAR_VL53L1X_DISTANCEMODE);
VL53L1X_SetInterMeasurementInMs(&sonar_vl53l1x.dev, SONAR_VL53L1X_INTERMEASUREMENT_MS);
/* Start measurement */
VL53L1X_StartRanging(&sonar_vl53l1x.dev);
#endif #endif
} }
@@ -114,26 +104,8 @@ void sonar_vl53l1x_init(void)
void sonar_vl53l1x_read(void) void sonar_vl53l1x_read(void)
{ {
#ifndef SITL #ifndef SITL
uint8_t isDataReady; if (VL53L1X_NonBlocking_IsIdle(&sonar_vl53l1x.dev)) {
uint16_t range_mm; VL53L1X_NonBlocking_RequestData(&sonar_vl53l1x.dev);
switch (sonar_vl53l1x.read_state) {
case 0:
// Wait for data ready
if (!VL53L1X_NonBlocking_CheckForDataReady(&sonar_vl53l1x.dev, &isDataReady)) { return; } // Check in progress
sonar_vl53l1x.read_state++;
/* Falls through. */
case 1:
// Get ranging data
if (!VL53L1X_NonBlocking_GetDistance(&sonar_vl53l1x.dev, &range_mm)) { return; } // Read in progress
sonar_vl53l1x_publish(range_mm);
sonar_vl53l1x.read_state++;
/* Falls through. */
case 2:
// Clear interrupt
if (!VL53L1X_NonBlocking_ClearInterrupt(&sonar_vl53l1x.dev)) { return; } // Clear in progress
sonar_vl53l1x.read_state = 0;
break;
default: return;
} }
#else // SITL #else // SITL
float range_mm = stateGetPositionEnu_f()->z * 1.0e3f; float range_mm = stateGetPositionEnu_f()->z * 1.0e3f;
@@ -142,3 +114,15 @@ void sonar_vl53l1x_read(void)
} }
#endif // SITL #endif // SITL
} }
void sonar_vl53l1x_event(void)
{
#ifndef SITL
uint16_t range_mm;
bool new_data = false;
VL53L1X_NonBlocking_ReadDataEvent(&sonar_vl53l1x.dev, &range_mm, &new_data);
if (new_data) {
sonar_vl53l1x_publish(range_mm);
}
#endif
}
sw/airborne/modules/sonar/sonar_vl53l1x.h
+1
View File
@@ -34,5 +34,6 @@ extern struct sonar_vl53l1x_dev sonar_vl53l1x;
extern void sonar_vl53l1x_init(void); extern void sonar_vl53l1x_init(void);
extern void sonar_vl53l1x_read(void); extern void sonar_vl53l1x_read(void);
extern void sonar_vl53l1x_event(void);
#endif #endif
sw/airborne/peripherals/bmi088.h
+14 -14
View File
@@ -35,13 +35,13 @@
/// Default gyro full scale range +- 1000°/s /// Default gyro full scale range +- 1000°/s
#define BMI088_DEFAULT_GYRO_RANGE BMI088_GYRO_RANGE_1000 #define BMI088_DEFAULT_GYRO_RANGE BMI088_GYRO_RANGE_1000
/// Default gyro output rate /// Default gyro output rate
#define BMI088_DEFAULT_GYRO_ODR BMI088_GYRO_ODR_400_BW_47 #define BMI088_DEFAULT_GYRO_ODR BMI088_GYRO_ODR_1000_BW_116
/// Default accel full scale range +- 6g /// Default accel full scale range +- 6g
#define BMI088_DEFAULT_ACCEL_RANGE BMI088_ACCEL_RANGE_6G #define BMI088_DEFAULT_ACCEL_RANGE BMI088_ACCEL_RANGE_12G
/// Default accel output rate /// Default accel output rate
#define BMI088_DEFAULT_ACCEL_ODR BMI088_ACCEL_ODR_400 #define BMI088_DEFAULT_ACCEL_ODR BMI088_ACCEL_ODR_1600
/// Default accel bandwidth /// Default accel bandwidth
#define BMI088_DEFAULT_ACCEL_BW BMI088_ACCEL_BW_NORMAL #define BMI088_DEFAULT_ACCEL_BW BMI088_ACCEL_BW_OSR4
/** default gyro sensitivy from the datasheet /** default gyro sensitivy from the datasheet
* sens = 1/ [LSB/(deg/s)] * pi/180 * 2^INT32_RATE_FRAC * sens = 1/ [LSB/(deg/s)] * pi/180 * 2^INT32_RATE_FRAC
@@ -71,21 +71,21 @@ extern const int32_t BMI088_GYRO_SENS_FRAC[5][2];
/** default accel sensitivy from the datasheet /** default accel sensitivy from the datasheet
* sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC * sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC
* ex: BMI with 8g has 4096 LSB/g * ex: BMI with 6g has 5460 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525 * sens = 9.81 [m/s^2] / 5460 [LSB/g] * 2^INT32_ACCEL_FRAC = 1.83982
*/ */
// FIXME // FIXME
#define BMI088_ACCEL_SENS_3G 0.919687 #define BMI088_ACCEL_SENS_3G 0.919912
#define BMI088_ACCEL_SENS_3G_NUM 9197 #define BMI088_ACCEL_SENS_3G_NUM 9199
#define BMI088_ACCEL_SENS_3G_DEN 10000 #define BMI088_ACCEL_SENS_3G_DEN 10000
#define BMI088_ACCEL_SENS_6G 1.83937 #define BMI088_ACCEL_SENS_6G 1.83982
#define BMI088_ACCEL_SENS_6G_NUM 18394 #define BMI088_ACCEL_SENS_6G_NUM 18398
#define BMI088_ACCEL_SENS_6G_DEN 10000 #define BMI088_ACCEL_SENS_6G_DEN 10000
#define BMI088_ACCEL_SENS_12G 3.67875 #define BMI088_ACCEL_SENS_12G 3.67965
#define BMI088_ACCEL_SENS_12G_NUM 36787 #define BMI088_ACCEL_SENS_12G_NUM 36797
#define BMI088_ACCEL_SENS_12G_DEN 10000 #define BMI088_ACCEL_SENS_12G_DEN 10000
#define BMI088_ACCEL_SENS_24G 7.3575 #define BMI088_ACCEL_SENS_24G 7.3593
#define BMI088_ACCEL_SENS_24G_NUM 7357 #define BMI088_ACCEL_SENS_24G_NUM 7359
#define BMI088_ACCEL_SENS_24G_DEN 1000 #define BMI088_ACCEL_SENS_24G_DEN 1000
// Get default sensitivity from a table // Get default sensitivity from a table
sw/airborne/peripherals/pca95x4.c
+76
View File
@@ -0,0 +1,76 @@
/*
* Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file peripherals/pca95x4.c
*
* Driver for the 8-bit I/O expander based on i2c
*/
#include "peripherals/pca95x4.h"
// Init function
void pca95x4_init(struct pca95x4 *dev, struct i2c_periph *i2c_p, uint8_t addr)
{
/* set i2c_peripheral */
dev->i2c_p = i2c_p;
/* slave address */
dev->i2c_trans.slave_addr = addr;
/* set initial status: Done */
dev->i2c_trans.status = I2CTransDone;
}
// Configure function
bool pca95x4_configure(struct pca95x4 *dev, uint8_t val, bool blocking)
{
if (dev->i2c_trans.status != I2CTransDone &&
dev->i2c_trans.status != I2CTransSuccess &&
dev->i2c_trans.status != I2CTransFailed) {
return false; // previous transaction not finished
}
// send config value
dev->i2c_trans.buf[0] = PCA95X4_CONFIG_REG;
dev->i2c_trans.buf[1] = val;
if (blocking) {
return i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2);
} else {
return i2c_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2);
}
}
// Set output function
bool pca95x4_set_output(struct pca95x4 *dev, uint8_t mask, bool blocking)
{
if (dev->i2c_trans.status != I2CTransDone &&
dev->i2c_trans.status != I2CTransSuccess &&
dev->i2c_trans.status != I2CTransFailed) {
return false; // previous transaction not finished
}
// send mask value
dev->i2c_trans.buf[0] = PCA95X4_OUTPUT_REG;
dev->i2c_trans.buf[1] = mask;
if (blocking) {
return i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2);
} else {
return i2c_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2);
}
}
sw/airborne/peripherals/pca95x4.h
+82
View File
@@ -0,0 +1,82 @@
/*
* Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file peripherals/pca95x4.h
*
* Driver for the 8-bit I/O expander based on i2c
*/
#ifndef PCA95X4_H
#define PCA95X4_H
#include "std.h"
#include "mcu_periph/i2c.h"
#define PCA95X4_DEFAULT_ADDRESS 0x40
#define PCA95X4_INPUT_REG (0x00)
#define PCA95X4_OUTPUT_REG (0x01)
#define PCA95X4_POL_REG (0x02)
#define PCA95X4_CONFIG_REG (0x03)
#define PCA95X4_P0 (1 << 0)
#define PCA95X4_P1 (1 << 1)
#define PCA95X4_P2 (1 << 2)
#define PCA95X4_P3 (1 << 3)
#define PCA95X4_P4 (1 << 4)
#define PCA95X4_P5 (1 << 5)
#define PCA95X4_P6 (1 << 6)
#define PCA95X4_P7 (1 << 7)
#define PCA95X4_CLEAR_ALL 0x00
/** PCA95X4 structure
*/
struct pca95x4 {
struct i2c_periph *i2c_p;
struct i2c_transaction i2c_trans;
};
/** Init PCA95X4
* @param [in] dev address to pca95x4 device
* @param [in] i2c_p addres of i2c bus
* @param [in] addr i2c address
*/
extern void pca95x4_init(struct pca95x4 *dev, struct i2c_periph *i2c_p, uint8_t addr);
/** Configure PCA95X4
* @param [in] dev address to pca95x4 device
* @param [in] val value to write to confi register
* @param [in] blocking true for blocking i2c transaction
* @return false if i2c was not ready or transaction submit fails or timeout (blocking)
*/
extern bool pca95x4_configure(struct pca95x4 *dev, uint8_t val, bool blocking);
/** Set output value
* @param [in] dev address to pca95x4 device
* @param [in] mask output pins to set
* @param [in] blocking true for blocking i2c transaction
* @return false if i2c was not ready or transaction submit fails or timeout (blocking)
*/
extern bool pca95x4_set_output(struct pca95x4 *dev, uint8_t mask, bool blocking);
#endif
sw/airborne/peripherals/vl53l1_platform.h
+9
View File
@@ -37,7 +37,15 @@ extern "C"
{ {
#endif #endif
#define VL53L1_DEFAULT_ADDRESS 0x52
enum VL53L1_ReadStatus {
VL53L1_READ_IDLE,
VL53L1_READ_DATA_READY,
VL53L1_READ_STATUS,
VL53L1_READ_DISTANCE,
VL53L1_CLEAR_INT
};
typedef struct { typedef struct {
struct i2c_periph *i2c_p; struct i2c_periph *i2c_p;
@@ -49,6 +57,7 @@ typedef struct {
uint8_t state; uint8_t state;
uint8_t IntPol; uint8_t IntPol;
} nonblocking; } nonblocking;
enum VL53L1_ReadStatus read_status;
} VL53L1_Dev_t; } VL53L1_Dev_t;
typedef VL53L1_Dev_t *VL53L1_DEV; typedef VL53L1_Dev_t *VL53L1_DEV;
sw/airborne/peripherals/vl53l1x_api.c
+25
View File
@@ -203,6 +203,11 @@ static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
255, 255, 11, 12 255, 255, 11, 12
}; };
#if VL53L1X_AUTO_INCR_ADDR
// start after default address
static uint8_t auto_incr_addr = VL53L1_DEFAULT_ADDRESS + 2;
#endif
VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion) VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion)
{ {
VL53L1X_ERROR Status = 0; VL53L1X_ERROR Status = 0;
@@ -240,9 +245,29 @@ VL53L1X_ERROR VL53L1X_SensorInit(VL53L1_DEV dev)
status = VL53L1X_StopRanging(dev); status = VL53L1X_StopRanging(dev);
status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */
#if VL53L1X_AUTO_INCR_ADDR
status = VL53L1X_SetI2CAddress(dev, auto_incr_addr);
auto_incr_addr += 2; // auto increment by 2 (+1 on 7 bits address)
#endif
return status; return status;
} }
void VL53L1X_BootDevice(VL53L1_DEV dev, uint16_t TimingBudgetInMs, uint16_t DistanceMode, uint32_t InterMeasurementInMs)
{
uint8_t state;
do {
VL53L1X_BootState(dev, &state);
} while (!state);
VL53L1X_SensorInit(dev);
/* Configure sensor */
VL53L1X_SetTimingBudgetInMs(dev, TimingBudgetInMs);
VL53L1X_SetDistanceMode(dev, DistanceMode);
VL53L1X_SetInterMeasurementInMs(dev, InterMeasurementInMs);
/* Start measurement */
VL53L1X_StartRanging(dev);
}
VL53L1X_ERROR VL53L1X_ClearInterrupt(VL53L1_DEV dev) VL53L1X_ERROR VL53L1X_ClearInterrupt(VL53L1_DEV dev)
{ {
VL53L1X_ERROR status = 0; VL53L1X_ERROR status = 0;
sw/airborne/peripherals/vl53l1x_api.h
+46 -40
View File
@@ -77,47 +77,47 @@
typedef int8_t VL53L1X_ERROR; typedef int8_t VL53L1X_ERROR;
#define SOFT_RESET 0x0000 #define SOFT_RESET 0x0000
#define VL53L1_I2C_SLAVE__DEVICE_ADDRESS 0x0001 #define VL53L1_I2C_SLAVE__DEVICE_ADDRESS 0x0001
#define VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND 0x0008 #define VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND 0x0008
#define ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS 0x0016 #define ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS 0x0016
#define ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS 0x0018 #define ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS 0x0018
#define ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS 0x001A #define ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS 0x001A
#define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E #define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E
#define MM_CONFIG__INNER_OFFSET_MM 0x0020 #define MM_CONFIG__INNER_OFFSET_MM 0x0020
#define MM_CONFIG__OUTER_OFFSET_MM 0x0022 #define MM_CONFIG__OUTER_OFFSET_MM 0x0022
#define GPIO_HV_MUX__CTRL 0x0030 #define GPIO_HV_MUX__CTRL 0x0030
#define GPIO__TIO_HV_STATUS 0x0031 #define GPIO__TIO_HV_STATUS 0x0031
#define SYSTEM__INTERRUPT_CONFIG_GPIO 0x0046 #define SYSTEM__INTERRUPT_CONFIG_GPIO 0x0046
#define PHASECAL_CONFIG__TIMEOUT_MACROP 0x004B #define PHASECAL_CONFIG__TIMEOUT_MACROP 0x004B
#define RANGE_CONFIG__TIMEOUT_MACROP_A_HI 0x005E #define RANGE_CONFIG__TIMEOUT_MACROP_A_HI 0x005E
#define RANGE_CONFIG__VCSEL_PERIOD_A 0x0060 #define RANGE_CONFIG__VCSEL_PERIOD_A 0x0060
#define RANGE_CONFIG__VCSEL_PERIOD_B 0x0063 #define RANGE_CONFIG__VCSEL_PERIOD_B 0x0063
#define RANGE_CONFIG__TIMEOUT_MACROP_B_HI 0x0061 #define RANGE_CONFIG__TIMEOUT_MACROP_B_HI 0x0061
#define RANGE_CONFIG__TIMEOUT_MACROP_B_LO 0x0062 #define RANGE_CONFIG__TIMEOUT_MACROP_B_LO 0x0062
#define RANGE_CONFIG__SIGMA_THRESH 0x0064 #define RANGE_CONFIG__SIGMA_THRESH 0x0064
#define RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS 0x0066 #define RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS 0x0066
#define RANGE_CONFIG__VALID_PHASE_HIGH 0x0069 #define RANGE_CONFIG__VALID_PHASE_HIGH 0x0069
#define VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD 0x006C #define VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD 0x006C
#define SYSTEM__THRESH_HIGH 0x0072 #define SYSTEM__THRESH_HIGH 0x0072
#define SYSTEM__THRESH_LOW 0x0074 #define SYSTEM__THRESH_LOW 0x0074
#define SD_CONFIG__WOI_SD0 0x0078 #define SD_CONFIG__WOI_SD0 0x0078
#define SD_CONFIG__INITIAL_PHASE_SD0 0x007A #define SD_CONFIG__INITIAL_PHASE_SD0 0x007A
#define ROI_CONFIG__USER_ROI_CENTRE_SPAD 0x007F #define ROI_CONFIG__USER_ROI_CENTRE_SPAD 0x007F
#define ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE 0x0080 #define ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE 0x0080
#define SYSTEM__SEQUENCE_CONFIG 0x0081 #define SYSTEM__SEQUENCE_CONFIG 0x0081
#define VL53L1_SYSTEM__GROUPED_PARAMETER_HOLD 0x0082 #define VL53L1_SYSTEM__GROUPED_PARAMETER_HOLD 0x0082
#define SYSTEM__INTERRUPT_CLEAR 0x0086 #define SYSTEM__INTERRUPT_CLEAR 0x0086
#define SYSTEM__MODE_START 0x0087 #define SYSTEM__MODE_START 0x0087
#define VL53L1_RESULT__RANGE_STATUS 0x0089 #define VL53L1_RESULT__RANGE_STATUS 0x0089
#define VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0 0x008C #define VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0 0x008C
#define RESULT__AMBIENT_COUNT_RATE_MCPS_SD 0x0090 #define RESULT__AMBIENT_COUNT_RATE_MCPS_SD 0x0090
#define VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0 0x0096 #define VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0 0x0096
#define VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0 0x0098 #define VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0 0x0098
#define VL53L1_RESULT__OSC_CALIBRATE_VAL 0x00DE #define VL53L1_RESULT__OSC_CALIBRATE_VAL 0x00DE
#define VL53L1_FIRMWARE__SYSTEM_STATUS 0x00E5 #define VL53L1_FIRMWARE__SYSTEM_STATUS 0x00E5
#define VL53L1_IDENTIFICATION__MODEL_ID 0x010F #define VL53L1_IDENTIFICATION__MODEL_ID 0x010F
#define VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD 0x013E #define VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD 0x013E
/**************************************** /****************************************
* PRIVATE define do not edit * PRIVATE define do not edit
@@ -161,6 +161,12 @@ VL53L1X_ERROR VL53L1X_SetI2CAddress(VL53L1_DEV, uint8_t new_address);
*/ */
VL53L1X_ERROR VL53L1X_SensorInit(VL53L1_DEV dev); VL53L1X_ERROR VL53L1X_SensorInit(VL53L1_DEV dev);
/**
* @brief Implement boot sequence of VL53L1 device as described in documentation
* See VL53L1X_SetTimingBudgetInMs, VL53L1X_SetDistanceMode and VL53L1X_SetInterMeasurementInMs for params details
*/
void VL53L1X_BootDevice(VL53L1_DEV dev, uint16_t TimingBudgetInMs, uint16_t DistanceMode, uint32_t InterMeasurementInMs);
/** /**
* @brief This function clears the interrupt, to be called after a ranging data reading * @brief This function clears the interrupt, to be called after a ranging data reading
* to arm the interrupt for the next data ready event. * to arm the interrupt for the next data ready event.
sw/airborne/peripherals/vl53l1x_nonblocking.c
+75
View File
@@ -30,6 +30,11 @@
#include <assert.h> #include <assert.h>
// possible GetRangeStatus return value as in api.c GetRangeStatus
static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
255, 255, 9, 13, 255, 255, 255, 255, 10, 6,
255, 255, 11, 12
};
// Returns true upon completion // Returns true upon completion
static bool VL53L1_NonBlocking_WriteMulti(VL53L1_DEV dev, uint16_t index, uint8_t *pdata, uint32_t count) static bool VL53L1_NonBlocking_WriteMulti(VL53L1_DEV dev, uint16_t index, uint8_t *pdata, uint32_t count)
{ {
@@ -106,6 +111,19 @@ bool VL53L1X_NonBlocking_CheckForDataReady(VL53L1_DEV dev, uint8_t *isDataReady)
} }
} }
// Returns true upon completion
bool VL53L1X_NonBlocking_GetRangeStatus(VL53L1_DEV dev, uint8_t *rangeStatus)
{
uint8_t RgSt;
*rangeStatus = 255;
if (!VL53L1_NonBlocking_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt, 1)) { return false; }
RgSt = RgSt & 0x1F;
if (RgSt < 24) {
*rangeStatus = status_rtn[RgSt];
}
return true;
}
// Returns true upon completion // Returns true upon completion
bool VL53L1X_NonBlocking_GetDistance(VL53L1_DEV dev, uint16_t *distance) bool VL53L1X_NonBlocking_GetDistance(VL53L1_DEV dev, uint16_t *distance)
{ {
@@ -122,3 +140,60 @@ bool VL53L1X_NonBlocking_ClearInterrupt(VL53L1_DEV dev)
uint8_t data = 0x01; uint8_t data = 0x01;
return VL53L1_NonBlocking_WriteMulti(dev, SYSTEM__INTERRUPT_CLEAR, &data, 1); return VL53L1_NonBlocking_WriteMulti(dev, SYSTEM__INTERRUPT_CLEAR, &data, 1);
} }
bool VL53L1X_NonBlocking_ReadDataEvent(VL53L1_DEV dev, uint16_t *distance_mm, bool *new_data)
{
uint8_t isDataReady, rangeStatus;
*new_data = false;
switch (dev->read_status) {
case VL53L1_READ_IDLE:
// Idle, do nothing
return false;
case VL53L1_READ_DATA_READY:
// Wait for data ready
if (!VL53L1X_NonBlocking_CheckForDataReady(dev, &isDataReady)) {
return false; // Check in progress
}
dev->read_status++;
/* Falls through. */
case VL53L1_READ_STATUS:
// Get range status
if (!VL53L1X_NonBlocking_GetRangeStatus(dev, &rangeStatus)) {
return false; // Read status in progress
}
if (rangeStatus != 0) {
dev->read_status = VL53L1_CLEAR_INT; // wrong measurement, clear interrupt and back to idle
return true; // end cycle
}
dev->read_status++;
/* Falls through. */
case VL53L1_READ_DISTANCE:
// Get ranging data
if (!VL53L1X_NonBlocking_GetDistance(dev, distance_mm)) {
return false; // Read in progress
}
*new_data = true;
dev->read_status++;
/* Falls through. */
case VL53L1_CLEAR_INT:
// Clear interrupt
if (!VL53L1X_NonBlocking_ClearInterrupt(dev)) {
return false; // Clear in progress
}
dev->read_status = VL53L1_READ_IDLE;
return true; // Cycle is done
default:
return false;
}
}
bool VL53L1X_NonBlocking_IsIdle(VL53L1_DEV dev)
{
return dev->read_status == VL53L1_READ_IDLE;
}
bool VL53L1X_NonBlocking_RequestData(VL53L1_DEV dev)
{
dev->read_status = VL53L1_READ_DATA_READY;
return true;
}
sw/airborne/peripherals/vl53l1x_nonblocking.h
+24
View File
@@ -39,6 +39,13 @@
*/ */
bool VL53L1X_NonBlocking_CheckForDataReady(VL53L1_DEV dev, uint8_t *isDataReady); bool VL53L1X_NonBlocking_CheckForDataReady(VL53L1_DEV dev, uint8_t *isDataReady);
/**
* @brief This function returns the range status to discard wrong measurments
* @param : rangeStatus (0:no error, 1:sigma failed, 2:signal failed, ..., 7:wrap-around)
* @return: TRUE upon completion
*/
bool VL53L1X_NonBlocking_GetRangeStatus(VL53L1_DEV dev, uint8_t *rangeStatus);
/** /**
* @brief This function returns the distance measured by the sensor in mm * @brief This function returns the distance measured by the sensor in mm
* @return: TRUE upon completion * @return: TRUE upon completion
@@ -52,5 +59,22 @@ bool VL53L1X_NonBlocking_GetDistance(VL53L1_DEV dev, uint16_t *distance);
*/ */
bool VL53L1X_NonBlocking_ClearInterrupt(VL53L1_DEV dev); bool VL53L1X_NonBlocking_ClearInterrupt(VL53L1_DEV dev);
/** @brief Implement non-blocking read sequence
* The data reading actually starts when the read_state is set to VL53L1_READ_DATA_READY using VL53L1X_NonBlocking_RequestData()
* @param : distance measurement in millimeter
* @param : flag when a new data is available
* @return: TRUE when a complete read sequence is finished
*/
bool VL53L1X_NonBlocking_ReadDataEvent(VL53L1_DEV dev, uint16_t *distance_mm, bool *new_data);
/** @brief Test is read status is on idle
* @return: TRUE if idle
*/
bool VL53L1X_NonBlocking_IsIdle(VL53L1_DEV dev);
/** @brief Request a new reading
* @return: TRUE if request successful
*/
bool VL53L1X_NonBlocking_RequestData(VL53L1_DEV dev);
#endif // VL53L1X_NONBLOCKING_H #endif // VL53L1X_NONBLOCKING_H