yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

ST ISM330DLC IMU driver

Browse Source
This commit is contained in:
Daniel Agar
2019-10-14 20:09:39 -04:00
parent eb560e8a0f
commit 1a395fb6d9
24 changed files with 1675 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
boards/px4/fmu-v5x/default.cmake
View File

@@ -32,8 +32,8 @@ px4_add_board(
imu/adis16477
imu/adis16497
imu/bmi088
# TBD imu/ism330dlc - needs bus selection
imu/mpu6000
imu/st/ism330dlc
irlock
lights/blinkm
lights/rgbled

4
boards/px4/fmu-v5x/src/spi.cpp
View File

@@ -335,9 +335,7 @@ __EXPORT void board_spi_reset(int mask_ms)
stm32_configgpio(GPIO_SPI2_SCK_OFF);
stm32_configgpio(GPIO_SPI2_MISO_OFF);
stm32_configgpio(GPIO_SPI2_MOSI_OFF);
#if BOARD_USE_DRDY
stm32_configgpio(GPIO_DRDY_OFF_SPI2_DRDY1_ISM330);
#endif
/* set the sensor rail off */
stm32_gpiowrite(GPIO_VDD_3V3_SENSORS2_EN, 0);
}
@@ -412,9 +410,7 @@ __EXPORT void board_spi_reset(int mask_ms)
stm32_configgpio(GPIO_SPI2_SCK);
stm32_configgpio(GPIO_SPI2_MISO);
stm32_configgpio(GPIO_SPI2_MOSI);
#if BOARD_USE_DRDY
stm32_configgpio(GPIO_SPI2_DRDY1_ISM330);
#endif
}
if (mask & 4) {

6
msg/CMakeLists.txt
View File

@@ -45,8 +45,8 @@ set(msg_files
camera_capture.msg
camera_trigger.msg
cellular_status.msg
collision_report.msg
collision_constraints.msg
collision_report.msg
commander_state.msg
cpuload.msg
debug_array.msg
@@ -104,12 +104,16 @@ set(msg_files
safety.msg
satellite_info.msg
sensor_accel.msg
sensor_accel_fifo.msg
sensor_accel_status.msg
sensor_baro.msg
sensor_bias.msg
sensor_combined.msg
sensor_correction.msg
sensor_gyro.msg
sensor_gyro_control.msg
sensor_gyro_fifo.msg
sensor_gyro_status.msg
sensor_mag.msg
sensor_preflight.msg
sensor_selection.msg

6
msg/sensor_accel.msg
View File

@@ -7,14 +7,16 @@ float32 x # acceleration in the NED X board axis in m/s^2
float32 y # acceleration in the NED Y board axis in m/s^2
float32 z # acceleration in the NED Z board axis in m/s^2
uint32 integral_dt # integration time (microseconds)
uint32 integral_dt # integration time (microseconds)
uint8 integral_samples # number of samples integrated
float32 x_integral # delta velocity in the NED X board axis in m/s over the integration time frame (integral_dt)
float32 y_integral # delta velocity in the NED Y board axis in m/s over the integration time frame (integral_dt)
float32 z_integral # delta velocity in the NED Z board axis in m/s over the integration time frame (integral_dt)
uint8 integral_clip_count # total clip count per integration period on any axis
float32 temperature # temperature in degrees celsius
float32 scaling # scaling from raw to m/s^s
float32 scaling # scaling from raw to m/s^2
int16 x_raw
int16 y_raw
int16 z_raw

13
msg/sensor_accel_fifo.msg Normal file
View File

@@ -0,0 +1,13 @@
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp_sample # time since system start (microseconds)
float32 dt # delta time between samples (microseconds)
float32 scale
uint8 samples # number of valid samples
int16[8] x # acceleration in the NED X board axis in m/s/s
int16[8] y # acceleration in the NED Y board axis in m/s/s
int16[8] z # acceleration in the NED Z board axis in m/s/s

18
msg/sensor_accel_status.msg Normal file
View File

@@ -0,0 +1,18 @@
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 error_count
float32 temperature
uint8 rotation
# clipping per axis?
uint64[3] clipping
uint16 measure_rate
uint16 sample_rate
float32 full_scale_range
float32 high_frequency_vibration # high frequency vibration level in the IMU delta angle data (rad)

5
msg/sensor_gyro.msg
View File

@@ -1,6 +1,5 @@
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 error_count
@@ -9,9 +8,11 @@ float32 y # angular velocity in the NED Y board axis in rad/s
float32 z # angular velocity in the NED Z board axis in rad/s
uint32 integral_dt # integration time (microseconds)
uint8 integral_samples # number of samples integrated
float32 x_integral # delta angle in the NED X board axis in rad/s over the integration time frame (integral_dt)
float32 y_integral # delta angle in the NED Y board axis in rad/s over the integration time frame (integral_dt)
float32 z_integral # delta angle in the NED Z board axis in rad/s over the integration time frame (integral_dt)
uint8 integral_clip_count # total clip count per integration period on any axis
float32 temperature # temperature in degrees celsius

13
msg/sensor_gyro_fifo.msg Normal file
View File

@@ -0,0 +1,13 @@
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # time since system start (microseconds)
float32 dt # delta time between samples (microseconds)
float32 scale
uint8 samples # number of valid samples
int16[8] x # angular velocity in the NED X board axis in rad/s
int16[8] y # angular velocity in the NED Y board axis in rad/s
int16[8] z # angular velocity in the NED Z board axis in rad/s

20
msg/sensor_gyro_status.msg Normal file
View File

@@ -0,0 +1,20 @@
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 error_count
float32 temperature
uint8 rotation
# clipping per axis?
uint64[3] clipping
uint16 measure_rate
uint16 sample_rate
float32 full_scale_range
float32 coning_vibration # Level of coning vibration in the IMU delta angles (rad^2)
float32 high_frequency_vibration # high frequency vibration level in the IMU delta angle data (rad)

8
msg/tools/uorb_rtps_message_ids.yaml
View File

@@ -265,6 +265,14 @@ rtps:
id: 116
- msg: cellular_status
id: 117
- msg: sensor_accel_fifo
id: 118
- msg: sensor_accel_status
id: 119
- msg: sensor_gyro_fifo
id: 120
- msg: sensor_gyro_status
id: 121
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 150

8
platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h
View File

@@ -46,6 +46,8 @@
#include <board_config.h>
__BEGIN_DECLS
/************************************************************************************
* Name: board_dma_alloc_init
*
@@ -114,7 +116,7 @@ __EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16
#if defined(BOARD_DMA_ALLOC_POOL_SIZE)
__EXPORT void *board_dma_alloc(size_t size);
#else
#define board_dma_alloc(size) (NULL)
#define board_dma_alloc(size) malloc(size)
#endif
/************************************************************************************
@@ -131,5 +133,7 @@ __EXPORT void *board_dma_alloc(size_t size);
#if defined(BOARD_DMA_ALLOC_POOL_SIZE)
__EXPORT void board_dma_free(FAR void *memory, size_t size);
#else
#define board_dma_free(memory, size) ()
#define board_dma_free(memory, size) free(memory)
#endif
__END_DECLS

1
src/drivers/drv_sensor.h
View File

@@ -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

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

@@ -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
)

391
src/drivers/imu/st/ism330dlc/ISM330DLC.cpp Normal file
View File

@@ -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();
}

104
src/drivers/imu/st/ism330dlc/ISM330DLC.hpp Normal file
View File

@@ -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};
};

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

@@ -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

153
src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp Normal file
View File

@@ -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;
}

238
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View File

@@ -36,20 +36,23 @@
#include <lib/drivers/device/Device.hpp>
using namespace time_literals;
using matrix::Vector3f;
PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
CDev(nullptr),
ModuleParams(nullptr),
_sensor_accel_pub{ORB_ID(sensor_accel), priority},
_sensor_pub{ORB_ID(sensor_accel), priority},
_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
_sensor_status_pub{ORB_ID(sensor_accel_status), priority},
_device_id{device_id},
_rotation{rotation}
{
_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
_sensor_accel_pub.get().device_id = device_id;
_sensor_accel_pub.get().scaling = 1.0f;
// set software low pass filter for controllers
updateParams();
configure_filter(_param_imu_accel_cutoff.get());
ConfigureFilter(_param_imu_accel_cutoff.get());
}
PX4Accelerometer::~PX4Accelerometer()
@@ -68,14 +71,14 @@ PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
accel_calibration_s cal{};
memcpy(&cal, (accel_calibration_s *) arg, sizeof(cal));
_calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
_calibration_scale = matrix::Vector3f{cal.x_scale, cal.y_scale, cal.z_scale};
_calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
_calibration_scale = Vector3f{cal.x_scale, cal.y_scale, cal.z_scale};
}
return PX4_OK;
case DEVIOCGDEVICEID:
return _sensor_accel_pub.get().device_id;
return _device_id;
default:
return -ENOTTY;
@@ -87,45 +90,58 @@ PX4Accelerometer::set_device_type(uint8_t devtype)
{
// current DeviceStructure
union device::Device::DeviceId device_id;
device_id.devid = _sensor_accel_pub.get().device_id;
device_id.devid = _device_id;
// update to new device type
device_id.devid_s.devtype = devtype;
// copy back to report
_sensor_accel_pub.get().device_id = device_id.devid;
_device_id = device_id.devid;
}
void
PX4Accelerometer::set_sample_rate(unsigned rate)
PX4Accelerometer::set_sample_rate(uint16_t rate)
{
_sample_rate = rate;
_filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq());
ConfigureFilter(_filter.get_cutoff_freq());
}
void
PX4Accelerometer::set_update_rate(uint16_t rate)
{
const uint32_t update_interval = 1000000 / rate;
_integrator_reset_samples = 4000 / update_interval;
}
void
PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
{
sensor_accel_s &report = _sensor_accel_pub.get();
report.timestamp = timestamp;
// Apply rotation (before scaling)
rotate_3f(_rotation, x, y, z);
const matrix::Vector3f raw{x, y, z};
const Vector3f raw{x, y, z};
// Apply range scale and the calibrating offset/scale
const matrix::Vector3f val_calibrated{(((raw * report.scaling) - _calibration_offset).emult(_calibration_scale))};
const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
// Filtered values
const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
const Vector3f val_filtered{_filter.apply(val_calibrated)};
// Integrated values
matrix::Vector3f integrated_value;
Vector3f integrated_value;
uint32_t integral_dt = 0;
if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {
sensor_accel_s report{};
report.timestamp = timestamp;
report.device_id = _device_id;
report.temperature = _temperature;
report.scaling = _scale;
report.error_count = _error_count;
// Raw values (ADC units 0 - 65535)
report.x_raw = x;
report.y_raw = y;
@@ -140,11 +156,190 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
report.y_integral = integrated_value(1);
report.z_integral = integrated_value(2);
poll_notify(POLLIN);
_sensor_accel_pub.update();
_sensor_pub.publish(report);
}
}
void
PX4Accelerometer::updateFIFO(const FIFOSample &sample)
{
// filtered data (control)
float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
float y_filtered = _filterArrayY.apply(sample.y, sample.samples);
float z_filtered = _filterArrayZ.apply(sample.z, sample.samples);
// Apply rotation (before scaling)
rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
const Vector3f raw{x_filtered, y_filtered, z_filtered};
// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
// status
{
sensor_accel_status_s &status = _sensor_status_pub.get();
const int16_t clip_limit = (_range / _scale) * 0.95f;
// x clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.x[n]) > clip_limit) {
status.clipping[0]++;
_integrator_clipping++;
}
}
// y clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.y[n]) > clip_limit) {
status.clipping[1]++;
_integrator_clipping++;
}
}
// z clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.z[n]) > clip_limit) {
status.clipping[2]++;
_integrator_clipping++;
}
}
status.device_id = _device_id;
status.error_count = _error_count;
status.full_scale_range = _range;
status.rotation = _rotation;
status.measure_rate = _update_rate;
status.sample_rate = _sample_rate;
status.temperature = _temperature;
status.timestamp = hrt_absolute_time();
_sensor_status_pub.publish(status);
}
// integrated data (INS)
{
// reset integrator if previous sample was too long ago
if ((sample.timestamp_sample > _timestamp_sample_prev)
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) {
ResetIntegrator();
}
if (_integrator_samples == 0) {
_integrator_timestamp_sample = sample.timestamp_sample;
}
// integrate
_integrator_samples += 1;
_integrator_fifo_samples += sample.samples;
for (int n = 0; n < sample.samples; n++) {
_integrator_accum[0] += sample.x[n];
}
for (int n = 0; n < sample.samples; n++) {
_integrator_accum[1] += sample.y[n];
}
for (int n = 0; n < sample.samples; n++) {
_integrator_accum[2] += sample.z[n];
}
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
// average integrated values to apply calibration
float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples;
float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples;
float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples;
// Apply rotation (before scaling)
rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg);
const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
// Apply range scale and the calibrating offset/scale
Vector3f val_int_calibrated{(((raw_int * _scale) - _calibration_offset).emult(_calibration_scale))};
val_int_calibrated *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore
// publish
sensor_accel_s report{};
report.device_id = _device_id;
report.temperature = _temperature;
report.scaling = _scale;
report.error_count = _error_count;
// Raw values (ADC units 0 - 65535)
report.x_raw = sample.x[0];
report.y_raw = sample.y[0];
report.z_raw = sample.z[0];
report.x = val_calibrated(0);
report.y = val_calibrated(1);
report.z = val_calibrated(2);
report.integral_dt = integrator_dt_us;
report.integral_samples = _integrator_fifo_samples;
report.x_integral = val_int_calibrated(0);
report.y_integral = val_int_calibrated(1);
report.z_integral = val_int_calibrated(2);
report.integral_clip_count = _integrator_clipping;
report.timestamp = _integrator_timestamp_sample;
_sensor_pub.publish(report);
// reset integrator
ResetIntegrator();
}
_timestamp_sample_prev = sample.timestamp_sample;
}
sensor_accel_fifo_s fifo{};
fifo.device_id = _device_id;
fifo.timestamp_sample = sample.timestamp_sample;
fifo.dt = sample.dt;
fifo.scale = _scale;
fifo.samples = sample.samples;
memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples);
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples);
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples);
fifo.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(fifo);
}
void
PX4Accelerometer::ResetIntegrator()
{
_integrator_samples = 0;
_integrator_fifo_samples = 0;
_integrator_accum[0] = 0;
_integrator_accum[1] = 0;
_integrator_accum[2] = 0;
_integrator_clipping = 0;
_integrator_timestamp_sample = 0;
_timestamp_sample_prev = 0;
}
void
PX4Accelerometer::ConfigureFilter(float cutoff_freq)
{
_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
_filterArrayX.set_cutoff_frequency(_sample_rate, cutoff_freq);
_filterArrayY.set_cutoff_frequency(_sample_rate, cutoff_freq);
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
}
void
PX4Accelerometer::print_status()
{
@@ -157,5 +352,4 @@ PX4Accelerometer::print_status()
PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
(double)_calibration_offset(2));
print_message(_sensor_accel_pub.get());
}

75
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View File

@@ -33,16 +33,19 @@
#pragma once
#include <drivers/device/integrator.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_hrt.h>
#include <lib/cdev/CDev.hpp>
#include <lib/conversion/rotation.h>
#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/drivers/device/integrator.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/uORB.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_accel_fifo.h>
#include <uORB/topics/sensor_accel_status.h>
class PX4Accelerometer : public cdev::CDev, public ModuleParams
{
@@ -53,34 +56,80 @@ public:
int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _sensor_accel_pub.get().error_count = error_count; }
void set_scale(float scale) { _sensor_accel_pub.get().scaling = scale; }
void set_temperature(float temperature) { _sensor_accel_pub.get().temperature = temperature; }
uint32_t get_device_id() const { return _device_id; }
void set_sample_rate(unsigned rate);
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _error_count += error_count; }
void set_range(float range) { _range = range; }
void set_sample_rate(uint16_t rate);
void set_scale(float scale) { _scale = scale; }
void set_temperature(float temperature) { _temperature = temperature; }
void set_update_rate(uint16_t rate);
void update(hrt_abstime timestamp, float x, float y, float z);
void print_status();
struct FIFOSample {
hrt_abstime timestamp_sample;
uint8_t samples; // number of samples
float dt; // in microseconds
int16_t x[8];
int16_t y[8];
int16_t z[8];
};
static_assert(sizeof(FIFOSample::x) == sizeof(sensor_accel_fifo_s::x), "FIFOSample.x invalid size");
static_assert(sizeof(FIFOSample::y) == sizeof(sensor_accel_fifo_s::y), "FIFOSample.y invalid size");
static_assert(sizeof(FIFOSample::z) == sizeof(sensor_accel_fifo_s::z), "FIFOSample.z invalid size");
void updateFIFO(const FIFOSample &sample);
private:
void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
void ConfigureFilter(float cutoff_freq);
void ResetIntegrator();
uORB::PublicationMultiData<sensor_accel_s> _sensor_accel_pub;
uORB::PublicationMulti<sensor_accel_s> _sensor_pub; // legacy message
uORB::PublicationMulti<sensor_accel_fifo_s> _sensor_fifo_pub;
uORB::PublicationMultiData<sensor_accel_status_s> _sensor_status_pub;
math::LowPassFilter2pVector3f _filter{1000, 100};
Integrator _integrator{4000, false};
const enum Rotation _rotation;
math::LowPassFilter2pArray _filterArrayX{4000, 100};
math::LowPassFilter2pArray _filterArrayY{4000, 100};
math::LowPassFilter2pArray _filterArrayZ{4000, 100};
Integrator _integrator{4000, false};
matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f};
matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};
int _class_device_instance{-1};
unsigned _sample_rate{1000};
uint32_t _device_id{0};
const enum Rotation _rotation;
float _range{16.0f * CONSTANTS_ONE_G};
float _scale{1.0f};
float _temperature{0.0f};
uint64_t _error_count{0};
uint16_t _sample_rate{1000};
uint16_t _update_rate{1000};
// integrator
hrt_abstime _integrator_timestamp_sample{0};
hrt_abstime _timestamp_sample_prev{0};
int32_t _integrator_accum[3] {};
uint8_t _integrator_reset_samples{4};
uint8_t _integrator_samples{0};
uint8_t _integrator_fifo_samples{0};
uint8_t _integrator_clipping{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff

278
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@@ -36,22 +36,24 @@
#include <lib/drivers/device/Device.hpp>
using namespace time_literals;
using matrix::Vector3f;
PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
CDev(nullptr),
ModuleParams(nullptr),
_sensor_gyro_pub{ORB_ID(sensor_gyro), priority},
_sensor_gyro_control_pub{ORB_ID(sensor_gyro_control), priority},
_sensor_pub{ORB_ID(sensor_gyro), priority},
_sensor_control_pub{ORB_ID(sensor_gyro_control), priority},
_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
_sensor_status_pub{ORB_ID(sensor_gyro_status), priority},
_device_id{device_id},
_rotation{rotation}
{
_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
_sensor_gyro_pub.get().device_id = device_id;
_sensor_gyro_pub.get().scaling = 1.0f;
_sensor_gyro_control_pub.get().device_id = device_id;
// set software low pass filter for controllers
updateParams();
configure_filter(_param_imu_gyro_cutoff.get());
ConfigureFilter(_param_imu_gyro_cutoff.get());
}
PX4Gyroscope::~PX4Gyroscope()
@@ -70,13 +72,13 @@ PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
gyro_calibration_s cal{};
memcpy(&cal, (gyro_calibration_s *) arg, sizeof(cal));
_calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
_calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
}
return PX4_OK;
case DEVIOCGDEVICEID:
return _sensor_gyro_pub.get().device_id;
return _device_id;
default:
return -ENOTTY;
@@ -88,67 +90,82 @@ PX4Gyroscope::set_device_type(uint8_t devtype)
{
// current DeviceStructure
union device::Device::DeviceId device_id;
device_id.devid = _sensor_gyro_pub.get().device_id;
device_id.devid = _device_id;
// update to new device type
device_id.devid_s.devtype = devtype;
// copy back to report
_sensor_gyro_pub.get().device_id = device_id.devid;
_sensor_gyro_control_pub.get().device_id = device_id.devid;
_device_id = device_id.devid;
}
void
PX4Gyroscope::set_sample_rate(unsigned rate)
PX4Gyroscope::set_sample_rate(uint16_t rate)
{
_sample_rate = rate;
_filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq());
ConfigureFilter(_filter.get_cutoff_freq());
}
void
PX4Gyroscope::set_update_rate(uint16_t rate)
{
const uint32_t update_interval = 1000000 / rate;
_integrator_reset_samples = 4000 / update_interval;
}
void
PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
{
sensor_gyro_s &report = _sensor_gyro_pub.get();
report.timestamp = timestamp;
// Apply rotation (before scaling)
rotate_3f(_rotation, x, y, z);
const matrix::Vector3f raw{x, y, z};
const Vector3f raw{x, y, z};
// Apply range scale and the calibrating offset/scale
const matrix::Vector3f val_calibrated{((raw * report.scaling) - _calibration_offset)};
const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};
// Filtered values
const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
const Vector3f val_filtered{_filter.apply(val_calibrated)};
// publish control data (filtered gyro) immediately
// publish control data (filtered) immediately
bool publish_control = true;
sensor_gyro_control_s &control = _sensor_gyro_control_pub.get();
sensor_gyro_control_s control{};
if (_param_imu_gyro_rate_max.get() > 0) {
const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
if (hrt_elapsed_time(&control.timestamp_sample) < interval) {
if (hrt_elapsed_time(&_control_last_publish) < interval) {
publish_control = false;
}
}
if (publish_control) {
control.timestamp_sample = timestamp;
control.device_id = _device_id;
val_filtered.copyTo(control.xyz);
control.timestamp = hrt_absolute_time();
_sensor_gyro_control_pub.update(); // publish
_sensor_control_pub.publish(control);
_control_last_publish = control.timestamp_sample;
}
// Integrated values
matrix::Vector3f integrated_value;
Vector3f integrated_value;
uint32_t integral_dt = 0;
if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {
sensor_gyro_s report{};
report.timestamp = timestamp;
report.device_id = _device_id;
report.temperature = _temperature;
report.scaling = _scale;
report.error_count = _error_count;
// Raw values (ADC units 0 - 65535)
report.x_raw = x;
report.y_raw = y;
@@ -163,11 +180,216 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
report.y_integral = integrated_value(1);
report.z_integral = integrated_value(2);
poll_notify(POLLIN);
_sensor_gyro_pub.update(); // publish
_sensor_pub.publish(report);
}
}
void
PX4Gyroscope::updateFIFO(const FIFOSample &sample)
{
// filtered data (control)
float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
float y_filtered = _filterArrayY.apply(sample.y, sample.samples);
float z_filtered = _filterArrayZ.apply(sample.z, sample.samples);
// Apply rotation (before scaling)
rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
const Vector3f raw{x_filtered, y_filtered, z_filtered};
// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{(raw * _scale) - _calibration_offset};
// control
{
// publish control data (filtered) immediately
bool publish_control = true;
sensor_gyro_control_s control{};
if (_param_imu_gyro_rate_max.get() > 0) {
const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
if (hrt_elapsed_time(&_control_last_publish) < interval) {
publish_control = false;
}
}
if (publish_control) {
control.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
control.device_id = _device_id;
val_calibrated.copyTo(control.xyz);
control.timestamp = hrt_absolute_time();
_sensor_control_pub.publish(control);
_control_last_publish = control.timestamp_sample;
}
}
// status
{
sensor_gyro_status_s &status = _sensor_status_pub.get();
const int16_t clip_limit = (_range / _scale) * 0.95f;
// x clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.x[n]) > clip_limit) {
status.clipping[0]++;
_integrator_clipping++;
}
}
// y clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.y[n]) > clip_limit) {
status.clipping[1]++;
_integrator_clipping++;
}
}
// z clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.z[n]) > clip_limit) {
status.clipping[2]++;
_integrator_clipping++;
}
}
status.device_id = _device_id;
status.error_count = _error_count;
status.full_scale_range = _range;
status.rotation = _rotation;
status.measure_rate = _update_rate;
status.sample_rate = _sample_rate;
status.temperature = _temperature;
status.timestamp = hrt_absolute_time();
_sensor_status_pub.publish(status);
}
// integrated data (INS)
{
// reset integrator if previous sample was too long ago
if ((sample.timestamp_sample > _timestamp_sample_prev)
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) {
ResetIntegrator();
}
if (_integrator_samples == 0) {
_integrator_timestamp_sample = sample.timestamp_sample;
}
// integrate
_integrator_samples += 1;
_integrator_fifo_samples += sample.samples;
for (int n = 0; n < sample.samples; n++) {
_integrator_accum[0] += sample.x[n];
}
for (int n = 0; n < sample.samples; n++) {
_integrator_accum[1] += sample.y[n];
}
for (int n = 0; n < sample.samples; n++) {
_integrator_accum[2] += sample.z[n];
}
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
// average integrated values to apply calibration
float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples;
float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples;
float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples;
// Apply rotation (before scaling)
rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg);
const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
// Apply range scale and the calibrating offset/scale
Vector3f val_int_calibrated{(raw_int * _scale) - _calibration_offset};
val_int_calibrated *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore
// publish
sensor_gyro_s report{};
report.device_id = _device_id;
report.temperature = _temperature;
report.scaling = _scale;
report.error_count = _error_count;
// Raw values (ADC units 0 - 65535)
report.x_raw = sample.x[0];
report.y_raw = sample.y[0];
report.z_raw = sample.z[0];
report.x = val_calibrated(0);
report.y = val_calibrated(1);
report.z = val_calibrated(2);
report.integral_dt = integrator_dt_us;
report.integral_samples = _integrator_fifo_samples;
report.x_integral = val_int_calibrated(0);
report.y_integral = val_int_calibrated(1);
report.z_integral = val_int_calibrated(2);
report.integral_clip_count = _integrator_clipping;
report.timestamp = _integrator_timestamp_sample;
_sensor_pub.publish(report);
// reset integrator
ResetIntegrator();
}
_timestamp_sample_prev = sample.timestamp_sample;
}
sensor_gyro_fifo_s fifo{};
fifo.device_id = _device_id;
fifo.timestamp_sample = sample.timestamp_sample;
fifo.dt = sample.dt;
fifo.scale = _scale;
fifo.samples = sample.samples;
memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples);
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples);
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples);
fifo.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(fifo);
}
void
PX4Gyroscope::ResetIntegrator()
{
_integrator_samples = 0;
_integrator_fifo_samples = 0;
_integrator_accum[0] = 0;
_integrator_accum[1] = 0;
_integrator_accum[2] = 0;
_integrator_clipping = 0;
_integrator_timestamp_sample = 0;
_timestamp_sample_prev = 0;
}
void
PX4Gyroscope::ConfigureFilter(float cutoff_freq)
{
_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
_filterArrayX.set_cutoff_frequency(_sample_rate, cutoff_freq);
_filterArrayY.set_cutoff_frequency(_sample_rate, cutoff_freq);
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
}
void
PX4Gyroscope::print_status()
{
@@ -178,6 +400,4 @@ PX4Gyroscope::print_status()
PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
(double)_calibration_offset(2));
print_message(_sensor_gyro_pub.get());
print_message(_sensor_gyro_control_pub.get());
}

78
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View File

@@ -33,17 +33,19 @@
#pragma once
#include <drivers/device/integrator.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_hrt.h>
#include <lib/cdev/CDev.hpp>
#include <lib/conversion/rotation.h>
#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/drivers/device/integrator.h>
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/uORB.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_control.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/topics/sensor_gyro_status.h>
class PX4Gyroscope : public cdev::CDev, public ModuleParams
{
@@ -54,34 +56,82 @@ public:
int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _sensor_gyro_pub.get().error_count = error_count; }
void set_scale(float scale) { _sensor_gyro_pub.get().scaling = scale; }
void set_temperature(float temperature) { _sensor_gyro_pub.get().temperature = temperature; }
uint32_t get_device_id() const { return _device_id; }
void set_sample_rate(unsigned rate);
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _error_count += error_count; }
void set_range(float range) { _range = range; }
void set_sample_rate(uint16_t rate);
void set_scale(float scale) { _scale = scale; }
void set_temperature(float temperature) { _temperature = temperature; }
void set_update_rate(uint16_t rate);
void update(hrt_abstime timestamp, float x, float y, float z);
void print_status();
struct FIFOSample {
hrt_abstime timestamp_sample;
uint8_t samples; // number of samples
float dt; // in microseconds
int16_t x[8];
int16_t y[8];
int16_t z[8];
};
static_assert(sizeof(FIFOSample::x) == sizeof(sensor_gyro_fifo_s::x), "FIFOSample.x invalid size");
static_assert(sizeof(FIFOSample::y) == sizeof(sensor_gyro_fifo_s::y), "FIFOSample.y invalid size");
static_assert(sizeof(FIFOSample::z) == sizeof(sensor_gyro_fifo_s::z), "FIFOSample.z invalid size");
void updateFIFO(const FIFOSample &sample);
private:
void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
void ConfigureFilter(float cutoff_freq);
void ResetIntegrator();
uORB::PublicationMultiData<sensor_gyro_s> _sensor_gyro_pub;
uORB::PublicationMultiData<sensor_gyro_control_s> _sensor_gyro_control_pub;
uORB::PublicationMulti<sensor_gyro_s> _sensor_pub; // legacy message
uORB::PublicationMulti<sensor_gyro_control_s> _sensor_control_pub;
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub;
uORB::PublicationMultiData<sensor_gyro_status_s> _sensor_status_pub;
math::LowPassFilter2pVector3f _filter{1000, 100};
Integrator _integrator{4000, true};
const enum Rotation _rotation;
hrt_abstime _control_last_publish{0};
math::LowPassFilter2pArray _filterArrayX{8000, 100};
math::LowPassFilter2pArray _filterArrayY{8000, 100};
math::LowPassFilter2pArray _filterArrayZ{8000, 100};
Integrator _integrator{4000, true};
matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};
int _class_device_instance{-1};
unsigned _sample_rate{1000};
uint32_t _device_id{0};
const enum Rotation _rotation;
float _range{math::radians(2000.0f)};
float _scale{1.0f};
float _temperature{0.0f};
uint64_t _error_count{0};
uint16_t _sample_rate{1000};
uint16_t _update_rate{1000};
// integrator
hrt_abstime _integrator_timestamp_sample{0};
hrt_abstime _timestamp_sample_prev{0};
int32_t _integrator_accum[3] {};
uint8_t _integrator_reset_samples{4};
uint8_t _integrator_samples{0};
uint8_t _integrator_fifo_samples{0};
uint8_t _integrator_clipping{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,

2
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
View File

@@ -66,7 +66,7 @@ public:
// Reset the filter state to this value
float reset(float sample);
private:
protected:
float _cutoff_freq{0.0f};

85
src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp Normal file
View File

@@ -0,0 +1,85 @@
/****************************************************************************
*
* 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 LowPassFilter2pArray.hpp
/// @brief A class to implement a second order low pass filter
#pragma once
#include "LowPassFilter2p.hpp"
namespace math
{
class LowPassFilter2pArray : public LowPassFilter2p
{
public:
LowPassFilter2pArray(float sample_freq, float cutoff_freq) : LowPassFilter2p(sample_freq, cutoff_freq)
{
}
/**
* Add a new raw value to the filter
*
* @return retrieve the filtered result
*/
inline float apply(const int16_t samples[], uint8_t num_samples)
{
float output = 0.0f;
for (int n = 0; n < num_samples; n++) {
// do the filtering
float delay_element_0 = samples[n] - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (n == num_samples - 1) {
output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
}
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
}
// don't allow bad values to propagate via the filter
if (!PX4_ISFINITE(output)) {
reset(samples[num_samples - 1]);
output = samples[num_samples - 1];
}
// return the value. Should be no need to check limits
return output;
}
};
} // namespace math

2
src/modules/logger/logger.cpp
View File

@@ -553,6 +553,8 @@ void Logger::add_default_topics()
add_topic_multi("actuator_outputs", 100);
add_topic_multi("battery_status", 500);
add_topic_multi("distance_sensor", 100);
add_topic_multi("sensor_accel_status", 1000);
add_topic_multi("sensor_gyro_status", 1000);
add_topic_multi("telemetry_status");
add_topic_multi("vehicle_gps_position");
add_topic_multi("wind_estimate", 200);