dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

drivers: remove ism330dlc 

not used anymore
This commit is contained in:
Beat Küng 2021-01-12 14:12:17 +01:00 committed by Daniel Agar
parent 9f5dee2ecf
commit daaf8b61dc
7 changed files with 0 additions and 837 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/drivers/drv_sensor.h
View File

@ -126,7 +126,6 @@
#define DRV_GYR_DEVTYPE_BMI088 0x66
#define DRV_BARO_DEVTYPE_BMP388 0x67
#define DRV_BARO_DEVTYPE_DPS310 0x68
#define DRV_IMU_DEVTYPE_ST_ISM330DLC 0x69
#define DRV_ACC_DEVTYPE_BMI088 0x6a
#define DRV_OSD_DEVTYPE_ATXXXX 0x6b
#define DRV_FLOW_DEVTYPE_PMW3901 0x6c

1
src/drivers/imu/st/CMakeLists.txt
View File

@ -31,5 +31,4 @@
#
############################################################################
add_subdirectory(ism330dlc)
add_subdirectory(lsm9ds1)

46
src/drivers/imu/st/ism330dlc/CMakeLists.txt
View File

@ -1,46 +0,0 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__imu__st__ism330dlc
MAIN ism330dlc
SRCS
ism330dlc_main.cpp
ISM330DLC.cpp
ISM330DLC.hpp
ST_ISM330DLC_Registers.hpp
DEPENDS
drivers_accelerometer
drivers_gyroscope
px4_work_queue
)

342
src/drivers/imu/st/ism330dlc/ISM330DLC.cpp
View File

@ -1,342 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ISM330DLC.hpp"
using namespace time_literals;
static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval
ISM330DLC::ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) :
SPI(DRV_IMU_DEVTYPE_ST_ISM330DLC, MODULE_NAME, bus, device, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_drdy_gpio(drdy_gpio),
_px4_accel(get_device_id(), rotation),
_px4_gyro(get_device_id(), rotation)
{
}
ISM330DLC::~ISM330DLC()
{
perf_free(_interval_perf);
perf_free(_transfer_perf);
perf_free(_fifo_empty_perf);
perf_free(_fifo_overflow_perf);
perf_free(_fifo_reset_perf);
perf_free(_drdy_count_perf);
perf_free(_drdy_interval_perf);
}
void ISM330DLC::exit_and_cleanup()
{
if (_drdy_gpio != 0) {
// Disable data ready callback
//px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr);
RegisterWrite(Register::INT1_CTRL, 0);
}
I2CSPIDriverBase::exit_and_cleanup();
}
int ISM330DLC::probe()
{
const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
if (whoami != ISM330DLC_WHO_AM_I) {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
return PX4_ERROR;
}
return PX4_OK;
}
int ISM330DLC::init()
{
int ret = SPI::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("SPI::init failed (%i)", ret);
return ret;
}
if (!Reset()) {
PX4_ERR("reset failed");
return PX4_ERROR;
}
Start();
return PX4_OK;
}
bool ISM330DLC::Reset()
{
// CTRL3_C: SW_RESET
// Note: When the FIFO is used, the IF_INC and BDU bits must be equal to 1.
RegisterSetBits(Register::CTRL3_C, CTRL3_C_BIT::BDU | CTRL3_C_BIT::IF_INC | CTRL3_C_BIT::SW_RESET);
usleep(50); // Wait 50 μs (or wait until the SW_RESET bit of the CTRL3_C register returns to 0).
// Accelerometer configuration
// CTRL1_XL: Accelerometer 16 G range and ODR 6.66 kHz
RegisterWrite(Register::CTRL1_XL, CTRL1_XL_BIT::ODR_XL_6_66KHZ | CTRL1_XL_BIT::FS_XL_16);
_px4_accel.set_scale(0.488f * (CONSTANTS_ONE_G / 1000.0f)); // 0.488 mg/LSB
_px4_accel.set_range(16.0f * CONSTANTS_ONE_G);
// Gyroscope configuration
// CTRL2_G: Gyroscope 2000 degrees/second and ODR 6.66 kHz
// CTRL6_C: Gyroscope low-pass filter bandwidth 937 Hz (maximum)
RegisterWrite(Register::CTRL2_G, CTRL2_G_BIT::ODR_G_6_66KHZ | CTRL2_G_BIT::FS_G_2000);
RegisterSetBits(Register::CTRL6_C, CTRL6_C_BIT::FTYPE_GYRO_LPF_BW_937_HZ);
_px4_gyro.set_scale(math::radians(70.0f / 1000.0f)); // 70 mdps/LSB
_px4_gyro.set_range(math::radians(2000.0f));
// reset is considered done once data is ready
const bool reset_done = ((RegisterRead(Register::CTRL3_C) & CTRL3_C_BIT::SW_RESET) == 0);
return reset_done;
}
void ISM330DLC::ResetFIFO()
{
perf_count(_fifo_reset_perf);
// FIFO_CTRL5 - disable FIFO
RegisterWrite(Register::FIFO_CTRL5, 0);
// CTRL5_C: rounding mode gyro + accel
RegisterWrite(Register::CTRL5_C, CTRL5_C_BIT::ROUNDING_GYRO_ACCEL);
// FIFO_CTRL3: full gyro and accel data to FIFO
RegisterWrite(Register::FIFO_CTRL3, FIFO_CTRL3_BIT::DEC_FIFO_GYRO | FIFO_CTRL3_BIT::DEC_FIFO_XL);
// FIFO_CTRL5: FIFO ODR is set to 6.66 kHz, and FIFO continuous mode enabled
RegisterWrite(Register::FIFO_CTRL5, FIFO_CTRL5_BIT::ODR_FIFO_6_66_KHZ | FIFO_CTRL5_BIT::FIFO_MODE_CONTINUOUS);
}
uint8_t ISM330DLC::RegisterRead(Register reg)
{
uint8_t cmd[2] {};
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
void ISM330DLC::RegisterWrite(Register reg, uint8_t value)
{
uint8_t cmd[2] { (uint8_t)reg, value };
transfer(cmd, cmd, sizeof(cmd));
}
void ISM330DLC::RegisterSetBits(Register reg, uint8_t setbits)
{
uint8_t val = RegisterRead(reg);
if (!(val & setbits)) {
val |= setbits;
RegisterWrite(reg, val);
}
}
void ISM330DLC::RegisterClearBits(Register reg, uint8_t clearbits)
{
uint8_t val = RegisterRead(reg);
if (val & clearbits) {
val &= !clearbits;
RegisterWrite(reg, val);
}
}
int ISM330DLC::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
ISM330DLC *dev = reinterpret_cast<ISM330DLC *>(arg);
dev->DataReady();
return 0;
}
void ISM330DLC::DataReady()
{
// _time_data_ready = hrt_absolute_time();
// perf_count(_drdy_count_perf);
// perf_count(_drdy_interval_perf);
// make another measurement
ScheduleNow();
}
void ISM330DLC::Start()
{
ResetFIFO();
if (_drdy_gpio != 0 && false) { // TODO: enable
// Setup data ready on rising edge
//px4_arch_gpiosetevent(_drdy_gpio, true, true, false, &ISM330DLC::DataReadyInterruptCallback, this);
// FIFO threshold level setting
// FIFO_CTRL1: FTH_[7:0]
// FIFO_CTRL2: FTH_[10:8]
const uint8_t fifo_threshold = 12;
RegisterWrite(Register::FIFO_CTRL1, fifo_threshold);
// INT1: FIFO full, overrun, or threshold
RegisterWrite(Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FULL_FLAG | INT1_CTRL_BIT::INT1_FIFO_OVR |
INT1_CTRL_BIT::INT1_FTH);
} else {
ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
}
}
void ISM330DLC::RunImpl()
{
perf_count(_interval_perf);
// use timestamp from the data ready interrupt if available,
// otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
hrt_absolute_time();
// Number of unread words (16-bit axes) stored in FIFO.
const uint8_t fifo_words = RegisterRead(Register::FIFO_STATUS1);
// check for FIFO status
const uint8_t FIFO_STATUS2 = RegisterRead(Register::FIFO_STATUS2);
if (FIFO_STATUS2 & FIFO_STATUS2_BIT::OVER_RUN) {
// overflow
perf_count(_fifo_overflow_perf);
ResetFIFO();
return;
} else if (FIFO_STATUS2 & FIFO_STATUS2_BIT::FIFO_EMPTY) {
// fifo empty could indicate a problem, reset
perf_count(_fifo_empty_perf);
ResetFIFO();
return;
}
// FIFO pattern: indicates Next reading from FIFO output registers (Gx, Gy, Gz, XLx, XLy, XLz)
const uint8_t fifo_pattern = RegisterRead(Register::FIFO_STATUS3);
if (fifo_pattern != 0) {
PX4_DEBUG("check FIFO pattern: %d", fifo_pattern);
}
// Transfer data
// only transfer out complete sets of gyro + accel
const int samples = (fifo_words / 2) / sizeof(FIFO::DATA);
if (samples < 1) {
perf_count(_fifo_empty_perf);
return;
} else if (samples > 16) {
// not technically an overflow, but more samples than we expected
perf_count(_fifo_overflow_perf);
ResetFIFO();
return;
}
FIFOTransferBuffer buffer{};
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
perf_begin(_transfer_perf);
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
perf_end(_transfer_perf);
return;
}
perf_end(_transfer_perf);
sensor_accel_fifo_s accel{};
accel.timestamp_sample = timestamp_sample;
accel.samples = samples;
accel.dt = 1000000 / ST_ISM330DLC::LA_ODR;
sensor_gyro_fifo_s gyro{};
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = 1000000 / ST_ISM330DLC::G_ODR;
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = buffer.f[i];
// sensor Z is up (RHC), flip y & z for publication
gyro.x[i] = combine(fifo_sample.OUTX_L_G, fifo_sample.OUTX_H_G);
gyro.y[i] = -combine(fifo_sample.OUTY_L_G, fifo_sample.OUTY_H_G);
gyro.z[i] = -combine(fifo_sample.OUTZ_L_G, fifo_sample.OUTZ_H_G);
accel.x[i] = combine(fifo_sample.OUTX_L_XL, fifo_sample.OUTX_H_XL);
accel.y[i] = -combine(fifo_sample.OUTY_L_XL, fifo_sample.OUTY_H_XL);
accel.z[i] = -combine(fifo_sample.OUTZ_L_XL, fifo_sample.OUTZ_H_XL);
}
// get current temperature at 1 Hz
if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) {
uint8_t temperature_buf[3] {};
temperature_buf[0] = static_cast<uint8_t>(Register::OUT_TEMP_L) | DIR_READ;
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
return;
}
// 16 bits in two’s complement format with a sensitivity of 256 LSB/°C. The output zero level corresponds to 25 °C.
const int16_t OUT_TEMP = combine(temperature_buf[1], temperature_buf[2]);
const float temperature = ((float)OUT_TEMP / 256.0f) + 25.0f;
_px4_accel.set_temperature(temperature);
_px4_gyro.set_temperature(temperature);
}
_px4_gyro.updateFIFO(gyro);
_px4_accel.updateFIFO(accel);
}
void ISM330DLC::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_interval_perf);
perf_print_counter(_transfer_perf);
perf_print_counter(_fifo_empty_perf);
perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_count_perf);
perf_print_counter(_drdy_interval_perf);
}

120
src/drivers/imu/st/ism330dlc/ISM330DLC.hpp
View File

@ -1,120 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ISM330DLC.hpp
*
* Driver for the ST ISM330DLC connected via SPI.
*
*/
#pragma once
#include "ST_ISM330DLC_Registers.hpp"
#include <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
using namespace ST_ISM330DLC;
class ISM330DLC : public device::SPI, public I2CSPIDriver<ISM330DLC>
{
public:
ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
~ISM330DLC() override;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void print_status() override;
void RunImpl();
int init() override;
void Start();
bool Reset();
protected:
void custom_method(const BusCLIArguments &cli) override;
void exit_and_cleanup() override;
private:
// Sensor Configuration
static constexpr uint32_t GYRO_RATE{ST_ISM330DLC::G_ODR};
static constexpr uint32_t ACCEL_RATE{ST_ISM330DLC::LA_ODR};
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
// Transfer data
struct FIFOTransferBuffer {
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA_OUT_L) | DIR_READ};
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
};
// ensure no struct padding
static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
int probe() override;
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);
void RegisterSetBits(Register reg, uint8_t setbits);
void RegisterClearBits(Register reg, uint8_t clearbits);
void ResetFIFO();
const spi_drdy_gpio_t _drdy_gpio;
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")};
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")};
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")};
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")};
perf_counter_t _drdy_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": drdy count")};
perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")};
hrt_abstime _time_data_ready{0};
hrt_abstime _time_last_temperature_update{0};
};

210
src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp
View File

@ -1,210 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ST_ISM330DLC_registers.hpp
*
* ST ISM330DLC registers.
*
*/
#pragma once
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
static constexpr uint8_t Bit2 = (1 << 2);
static constexpr uint8_t Bit3 = (1 << 3);
static constexpr uint8_t Bit4 = (1 << 4);
static constexpr uint8_t Bit5 = (1 << 5);
static constexpr uint8_t Bit6 = (1 << 6);
static constexpr uint8_t Bit7 = (1 << 7);
namespace ST_ISM330DLC
{
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t ISM330DLC_WHO_AM_I = 0b01101010; // Who I am ID
static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency
static constexpr uint32_t LA_ODR = 6664; // Linear acceleration output data rate
static constexpr uint32_t G_ODR = 6664; // Angular rate output data rate
enum class
Register : uint8_t {
FIFO_CTRL1 = 0x06, // FIFO threshold level setting.
FIFO_CTRL3 = 0x08, // FIFO control register (r/w).
FIFO_CTRL5 = 0x0A,
INT1_CTRL = 0x0D,
INT2_CTRL = 0x0E,
WHO_AM_I = 0x0F,
CTRL1_XL = 0x10, // Linear acceleration sensor control register 1 (r/w).
CTRL2_G = 0x11, // Angular rate sensor control register 2 (r/w).
CTRL3_C = 0x12, // Control register 3 (r/w).
CTRL4_C = 0x13,
CTRL5_C = 0x14, // Control register 5 (r/w).
CTRL6_C = 0x15, // Angular rate sensor control register 6 (r/w).
OUT_TEMP_L = 0x20,
OUT_TEMP_H = 0x21,
FIFO_STATUS1 = 0x3A, // FIFO status control register (r)
FIFO_STATUS2 = 0x3B, // FIFO status control register (r)
FIFO_STATUS3 = 0x3C, // FIFO status control register (r)
FIFO_DATA_OUT_L = 0x3E, // FIFO data output (first byte)
FIFO_DATA_OUT_H = 0x3F, // FIFO data output (second byte)
};
// FIFO_CTRL3
enum
FIFO_CTRL3_BIT : uint8_t {
DEC_FIFO_GYRO = Bit3, // Gyro no decimation
DEC_FIFO_XL = Bit0, // Accel no decimation
};
// FIFO_CTRL5
enum
FIFO_CTRL5_BIT : uint8_t {
ODR_FIFO_6_66_KHZ = Bit6 | Bit4, // FIFO ODR is set to 6.66 kHz
FIFO_MODE_CONTINUOUS = Bit2 | Bit1, // Continuous mode. If the FIFO is full, the new sample overwrites the older one.
};
// INT1_CTRL
enum
INT1_CTRL_BIT : uint8_t {
INT1_FULL_FLAG = Bit5,
INT1_FIFO_OVR = Bit4,
INT1_FTH = Bit3,
INT1_DRDY_G = Bit1,
INT1_DRDY_XL = Bit0,
};
// INT2_CTRL
enum
INT2_CTRL_BIT : uint8_t {
INT2_FULL_FLAG = Bit5,
INT2_FIFO_OVR = Bit4,
INT2_FTH = Bit3,
INT2_DRDY_G = Bit1,
INT2_DRDY_XL = Bit0,
};
// CTRL1_XL
enum
CTRL1_XL_BIT : uint8_t {
ODR_XL_6_66KHZ = Bit7 | Bit5, // 6.66 kHz Output data rate and power mode selection
FS_XL_16 = Bit2, // FS_XL 01: ±16 g
};
// CTRL2_G
enum
CTRL2_G_BIT : uint8_t {
ODR_G_6_66KHZ = Bit7 | Bit5,
FS_G_2000 = Bit3 | Bit2,
};
// CTRL3_C
enum
CTRL3_C_BIT : uint8_t {
BDU = Bit7,
IF_INC = Bit2,
SW_RESET = Bit0
};
// CTRL4_C
enum
CTRL4_C_BIT : uint8_t {
INT2_on_INT1 = Bit5,
};
// CTRL5_C
enum
CTRL5_C_BIT : uint8_t {
ROUNDING_GYRO_ACCEL = Bit1 | Bit0, // ROUNDING[2:0] - 011 Gyroscope + accelerometer
};
// CTRL6_C
enum
CTRL6_C_BIT : uint8_t {
FTYPE_GYRO_LPF_BW_937_HZ = Bit1 | Bit0
};
// FIFO_STATUS2
enum
FIFO_STATUS2_BIT : uint8_t {
OVER_RUN = Bit6,
FIFO_EMPTY = Bit4,
};
namespace FIFO
{
static constexpr size_t SIZE = 4096;
// Saving data in the FIFO buffer is organized in four FIFO data sets consisting of 6 bytes each
// each FIFO sample is composed of 16 bits
struct DATA {
uint8_t OUTX_L_G;
uint8_t OUTX_H_G;
uint8_t OUTY_L_G;
uint8_t OUTY_H_G;
uint8_t OUTZ_L_G;
uint8_t OUTZ_H_G;
uint8_t OUTX_L_XL;
uint8_t OUTX_H_XL;
uint8_t OUTY_L_XL;
uint8_t OUTY_H_XL;
uint8_t OUTZ_L_XL;
uint8_t OUTZ_H_XL;
};
static_assert(sizeof(DATA) == 12);
}
} // namespace ST_ISM330DLC

117
src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp
View File

@ -1,117 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ISM330DLC.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
void
ISM330DLC::print_usage()
{
PRINT_MODULE_USAGE_NAME("ism330dlc", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_COMMAND("reset");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
I2CSPIDriverBase *ISM330DLC::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
ISM330DLC *instance = new ISM330DLC(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
if (OK != instance->init()) {
delete instance;
return nullptr;
}
return instance;
}
void ISM330DLC::custom_method(const BusCLIArguments &cli)
{
Reset();
}
extern "C" int ism330dlc_main(int argc, char *argv[])
{
int ch;
using ThisDriver = ISM330DLC;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = ST_ISM330DLC::SPI_SPEED;
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
}
}
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ST_ISM330DLC);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
if (!strcmp(verb, "reset")) {
return ThisDriver::module_custom_method(cli, iterator);
}
ThisDriver::print_usage();
return -1;
}