mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-01 13:11:35 +08:00
ST ISM330DLC IMU driver
This commit is contained in:
Daniel Agar
@@ -117,6 +117,7 @@
|
||||
#define DRV_DEVTYPE_BMI088 0x66
|
||||
#define DRV_DEVTYPE_BMP388 0x67
|
||||
#define DRV_DEVTYPE_DPS310 0x68
|
||||
#define DRV_DEVTYPE_ST_ISM330DLC 0x69
|
||||
|
||||
/*
|
||||
* ioctl() definitions
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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
|
||||
)
|
||||
@@ -0,0 +1,391 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
using namespace ST_ISM330DLC;
|
||||
|
||||
static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
|
||||
|
||||
ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
|
||||
SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
|
||||
_px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation),
|
||||
_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation)
|
||||
{
|
||||
set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
|
||||
_px4_accel.set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
|
||||
_px4_gyro.set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
|
||||
|
||||
_px4_accel.set_sample_rate(ST_ISM330DLC::LA_ODR);
|
||||
_px4_gyro.set_sample_rate(ST_ISM330DLC::G_ODR);
|
||||
|
||||
_px4_accel.set_update_rate(1000000 / _fifo_interval);
|
||||
_px4_gyro.set_update_rate(1000000 / _fifo_interval);
|
||||
}
|
||||
|
||||
ISM330DLC::~ISM330DLC()
|
||||
{
|
||||
Stop();
|
||||
|
||||
if (_dma_data_buffer != nullptr) {
|
||||
board_dma_free(_dma_data_buffer, FIFO::SIZE);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
int
|
||||
ISM330DLC::probe()
|
||||
{
|
||||
if (RegisterRead(Register::WHO_AM_I) == ISM330DLC_WHO_AM_I) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
bool
|
||||
ISM330DLC::Init()
|
||||
{
|
||||
if (SPI::init() != PX4_OK) {
|
||||
PX4_ERR("SPI::init failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!Reset()) {
|
||||
PX4_ERR("reset failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
// allocate DMA capable buffer
|
||||
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
|
||||
|
||||
if (_dma_data_buffer == nullptr) {
|
||||
PX4_ERR("DMA alloc failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
Start();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
ISM330DLC::Reset()
|
||||
{
|
||||
for (int i = 0; i < 5; i++) {
|
||||
// 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));
|
||||
|
||||
const bool reset_done = ((RegisterRead(Register::CTRL3_C) & CTRL3_C_BIT::SW_RESET) == 0);
|
||||
|
||||
// reset done once data is ready
|
||||
if (reset_done) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
Stop();
|
||||
|
||||
ResetFIFO();
|
||||
|
||||
#if defined(GPIO_SPI2_DRDY1_ISM330) && false // TODO: enable
|
||||
// Setup data ready on rising edge
|
||||
px4_arch_gpiosetevent(GPIO_SPI2_DRDY1_ISM330, 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);
|
||||
#endif
|
||||
}
|
||||
|
||||
void
|
||||
ISM330DLC::Stop()
|
||||
{
|
||||
#if defined(GPIO_SPI2_DRDY1_ISM330) && false // TODO: enable
|
||||
// Disable data ready callback
|
||||
px4_arch_gpiosetevent(GPIO_SPI2_DRDY1_ISM330, false, false, false, nullptr, nullptr);
|
||||
|
||||
RegisterWrite(Register::INT1_CTRL, 0);
|
||||
#else
|
||||
ScheduleClear();
|
||||
#endif
|
||||
}
|
||||
|
||||
void
|
||||
ISM330DLC::Run()
|
||||
{
|
||||
perf_count(_interval_perf);
|
||||
|
||||
// Number of unread words (16-bit axes) stored in FIFO.
|
||||
const hrt_abstime timestamp_fifo_level = hrt_absolute_time();
|
||||
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_ERR("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 > 8) {
|
||||
// not technically an overflow, but more samples than we expected
|
||||
perf_count(_fifo_overflow_perf);
|
||||
ResetFIFO();
|
||||
return;
|
||||
}
|
||||
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
|
||||
struct ISM_Report {
|
||||
uint8_t cmd;
|
||||
FIFO::DATA f[8]; // we never transfer more than 8 samples
|
||||
};
|
||||
ISM_Report *report = (ISM_Report *)_dma_data_buffer;
|
||||
memset(report, 0, transfer_size);
|
||||
report->cmd = static_cast<uint8_t>(Register::FIFO_DATA_OUT_L) | DIR_READ;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
|
||||
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
static constexpr uint32_t gyro_dt = 1000000 / ST_ISM330DLC::G_ODR;
|
||||
|
||||
// estimate timestamp of first sample in the FIFO from number of samples and fill rate
|
||||
const hrt_abstime timestamp_sample = timestamp_fifo_level - ((samples - 1) * gyro_dt);
|
||||
|
||||
PX4Accelerometer::FIFOSample accel{};
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.samples = samples;
|
||||
accel.dt = gyro_dt;
|
||||
|
||||
PX4Gyroscope::FIFOSample gyro{};
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
gyro.dt = gyro_dt;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->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 = (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::PrintInfo()
|
||||
{
|
||||
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);
|
||||
|
||||
_px4_accel.print_status();
|
||||
_px4_gyro.print_status();
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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/px4_config.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
using ST_ISM330DLC::Register;
|
||||
|
||||
class ISM330DLC : public device::SPI, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
ISM330DLC(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
|
||||
virtual ~ISM330DLC();
|
||||
|
||||
bool Init();
|
||||
void Start();
|
||||
void Stop();
|
||||
bool Reset();
|
||||
void PrintInfo();
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
|
||||
private:
|
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||
void DataReady();
|
||||
|
||||
void Run() override;
|
||||
|
||||
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();
|
||||
|
||||
|
||||
uint8_t *_dma_data_buffer{nullptr};
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
static constexpr uint32_t _fifo_interval{1000}; // 1000 us sample interval
|
||||
|
||||
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};
|
||||
|
||||
};
|
||||
@@ -0,0 +1,210 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
@@ -0,0 +1,153 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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>
|
||||
|
||||
namespace ism330dlc
|
||||
{
|
||||
|
||||
ISM330DLC *g_dev{nullptr};
|
||||
|
||||
int start(enum Rotation rotation);
|
||||
int stop();
|
||||
int status();
|
||||
void usage();
|
||||
|
||||
int start(enum Rotation rotation)
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
PX4_WARN("already started");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// create the driver
|
||||
g_dev = new ISM330DLC(PX4_SPI_BUS_SENSORS2, PX4_SPIDEV_ISM330, rotation); // v5x TODO: board manifest
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
PX4_ERR("driver start failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!g_dev->Init()) {
|
||||
PX4_ERR("driver init failed");
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int stop()
|
||||
{
|
||||
if (g_dev == nullptr) {
|
||||
PX4_WARN("driver not running");
|
||||
}
|
||||
|
||||
g_dev->Stop();
|
||||
delete g_dev;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int reset()
|
||||
{
|
||||
if (g_dev == nullptr) {
|
||||
PX4_WARN("driver not running");
|
||||
}
|
||||
|
||||
return g_dev->Reset();
|
||||
}
|
||||
|
||||
int status()
|
||||
{
|
||||
if (g_dev == nullptr) {
|
||||
PX4_WARN("driver not running");
|
||||
return -1;
|
||||
}
|
||||
|
||||
g_dev->PrintInfo();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void usage()
|
||||
{
|
||||
PX4_INFO("missing command: try 'start', 'stop', 'reset', 'status'");
|
||||
PX4_INFO("options:");
|
||||
PX4_INFO(" -R rotation");
|
||||
}
|
||||
|
||||
} // namespace ism330dlc
|
||||
|
||||
extern "C" __EXPORT int ism330dlc_main(int argc, char *argv[])
|
||||
{
|
||||
enum Rotation rotation = ROTATION_NONE;
|
||||
int myoptind = 1;
|
||||
int ch = 0;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
/* start options */
|
||||
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'R':
|
||||
rotation = (enum Rotation)atoi(myoptarg);
|
||||
break;
|
||||
|
||||
default:
|
||||
ism330dlc::usage();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
const char *verb = argv[myoptind];
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return ism330dlc::start(rotation);
|
||||
|
||||
} else if (!strcmp(verb, "stop")) {
|
||||
return ism330dlc::stop();
|
||||
|
||||
} else if (!strcmp(verb, "status")) {
|
||||
return ism330dlc::status();
|
||||
|
||||
} else if (!strcmp(verb, "reset")) {
|
||||
return ism330dlc::reset();
|
||||
}
|
||||
|
||||
ism330dlc::usage();
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user