zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

drivers: remove ism330dlc 

not used anymore
release/1.12
Beat Küng 4 years ago committed by Daniel Agar
parent
9f5dee2ecf
commit
daaf8b61dc
7 changed files with 0 additions and 837 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      src/drivers/drv_sensor.h
  2. 1
      src/drivers/imu/st/CMakeLists.txt
  3. 46
      src/drivers/imu/st/ism330dlc/CMakeLists.txt
  4. 342
      src/drivers/imu/st/ism330dlc/ISM330DLC.cpp
  5. 120
      src/drivers/imu/st/ism330dlc/ISM330DLC.hpp
  6. 210
      src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp
  7. 117
      src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp

1
src/drivers/drv_sensor.h
Unescape View File

@ -126,7 +126,6 @@ @@ -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
Unescape View File

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

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

@ -1,46 +0,0 @@ @@ -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
Unescape View File

@ -1,342 +0,0 @@ @@ -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
Unescape View File

@ -1,120 +0,0 @@ @@ -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
Unescape View File

@ -1,210 +0,0 @@ @@ -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
Unescape View File

@ -1,117 +0,0 @@ @@ -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;
}
Write Preview
Loading…
Cancel
Save