Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

invensense icm20602 improvements

Browse Source 
- checked register mechanism and simple watchdog
    - driver checks for errors gradually and can reconfigure itself
 - respect IMU_GYRO_RATEMAX at the driver level
 - fixed sensor INT16_MIN and INT16_MAX handling (y & z axis are flipped before publishing)
 - increased sensor_gyro_fifo max size (enables running the driver much slower, but still transferring all raw data)
 - PX4Accelerometer/PX4Gyroscope remove unnecessary memsets
This commit is contained in:
Daniel Agar
2020-01-22 09:54:55 -05:00
parent 40921241e7
commit 22499effb9
12 changed files with 423 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/px4/fmu-v4/cannode.cmake
+1 -1
View File
@@ -59,7 +59,7 @@ px4_add_board(
MODULES
#ekf2
#load_mon
#sensors
sensors
#temperature_compensation
SYSTEMCMDS
#bl_update
boards/px4/fmu-v4/default.cmake
+1
View File
@@ -30,6 +30,7 @@ px4_add_board(
imu/adis16497
imu/invensense/icm20602
imu/invensense/icm20608-g
#imu/invensense/mpu9250
imu/mpu6000
imu/mpu9250
irlock
msg/sensor_accel_status.msg
+1 -1
View File
@@ -10,7 +10,7 @@ uint8 rotation
uint32[3] clipping # clipping per axis
uint16 measure_rate
uint16 measure_rate_hz
float32 full_scale_range
msg/sensor_gyro_fifo.msg
+3 -3
View File
@@ -8,6 +8,6 @@ float32 scale
uint8 samples # number of valid samples
int16[16] x # angular velocity in the NED X board axis in rad/s
int16[16] y # angular velocity in the NED Y board axis in rad/s
int16[16] z # angular velocity in the NED Z board axis in rad/s
int16[32] x # angular velocity in the NED X board axis in rad/s
int16[32] y # angular velocity in the NED Y board axis in rad/s
int16[32] z # angular velocity in the NED Z board axis in rad/s
msg/sensor_gyro_status.msg
+1 -1
View File
@@ -10,7 +10,7 @@ uint8 rotation
uint32[3] clipping # clipping per axis
uint16 measure_rate
uint16 measure_rate_hz
float32 full_scale_range
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
+7 -7
View File
@@ -50,13 +50,13 @@ namespace wq_configurations
{
static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority
static constexpr wq_config_t SPI0{"wq:SPI0", 1900, -1};
static constexpr wq_config_t SPI1{"wq:SPI1", 1900, -2};
static constexpr wq_config_t SPI2{"wq:SPI2", 1900, -3};
static constexpr wq_config_t SPI3{"wq:SPI3", 1900, -4};
static constexpr wq_config_t SPI4{"wq:SPI4", 1900, -5};
static constexpr wq_config_t SPI5{"wq:SPI5", 1900, -6};
static constexpr wq_config_t SPI6{"wq:SPI6", 1900, -7};
static constexpr wq_config_t SPI0{"wq:SPI0", 2000, -1};
static constexpr wq_config_t SPI1{"wq:SPI1", 2000, -2};
static constexpr wq_config_t SPI2{"wq:SPI2", 2000, -3};
static constexpr wq_config_t SPI3{"wq:SPI3", 2000, -4};
static constexpr wq_config_t SPI4{"wq:SPI4", 2000, -5};
static constexpr wq_config_t SPI5{"wq:SPI5", 2000, -6};
static constexpr wq_config_t SPI6{"wq:SPI6", 2000, -7};
static constexpr wq_config_t I2C0{"wq:I2C0", 1400, -8};
static constexpr wq_config_t I2C1{"wq:I2C1", 1400, -9};
src/drivers/imu/invensense/icm20602/ICM20602.cpp
+304 -125
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/invensense/icm20602/ICM20602.hpp
+48 -2
View File
@@ -50,7 +50,7 @@
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
using InvenSense_ICM20602::Register;
using namespace InvenSense_ICM20602;
class ICM20602 : public device::SPI, public px4::ScheduledWorkItem
{
@@ -65,6 +65,13 @@ public:
void PrintInfo();
private:
struct register_config_t {
Register reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
int probe() override;
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
@@ -72,8 +79,16 @@ private:
void Run() override;
void ConfigureSampleRate(int sample_rate);
bool CheckRegister(const register_config_t &reg_cfg, bool notify = true);
bool Configure(bool notify = true);
void ConfigureAccel();
void ConfigureGyro();
uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
void RegisterSetBits(Register reg, uint8_t setbits);
void RegisterClearBits(Register reg, uint8_t clearbits);
@@ -85,10 +100,41 @@ private:
PX4Gyroscope _px4_gyro;
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad 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_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")};
hrt_abstime _time_data_ready{0};
hrt_abstime _last_config_check{0};
uint8_t _checked_register{0};
bool _using_data_ready_interrupt_enabled{false};
// Sensor Configuration
static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro
static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
uint16_t _fifo_empty_interval_us{1000}; // 1000 us / 1000 Hz transfer interval
uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
static constexpr uint8_t size_register_cfg{11};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP },
{ Register::I2C_IF, I2C_IF_BIT::I2C_IF_DIS, 0 },
{ Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 },
{ Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF, 0 },
{ Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF },
{ Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, Bit7 | CONFIG_BIT::FIFO_MODE },
{ Register::FIFO_WM_TH1, 0, 0 }, // FIFO_WM_TH[9:8]
{ Register::FIFO_WM_TH2, 0, 0 }, // FIFO_WM_TH[7:0]
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 },
{ Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, 0 },
{ Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN, INT_ENABLE_BIT::DATA_RDY_INT_EN }
};
};
src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp
+18 -12
View File
@@ -40,6 +40,8 @@
#pragma once
#include <cstdint>
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
@@ -58,6 +60,9 @@ static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t WHOAMI = 0x12;
static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C
static constexpr float ROOM_TEMPERATURE_OFFSET = 25.f; // C
enum class Register : uint8_t {
CONFIG = 0x1A,
GYRO_CONFIG = 0x1B,
@@ -66,10 +71,7 @@ enum class Register : uint8_t {
FIFO_EN = 0x23,
INT_STATUS = 0x3A,
INT_ENABLE = 0x38,
FIFO_WM_INT_STATUS = 0x39,
TEMP_OUT_H = 0x41,
TEMP_OUT_L = 0x42,
@@ -80,6 +82,8 @@ enum class Register : uint8_t {
USER_CTRL = 0x6A,
PWR_MGMT_1 = 0x6B,
I2C_IF = 0x70,
FIFO_COUNTH = 0x72,
FIFO_COUNTL = 0x73,
FIFO_R_W = 0x74,
@@ -102,7 +106,7 @@ enum GYRO_CONFIG_BIT : uint8_t {
FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000
// FCHOICE_B [1:0]
FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b10 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz
FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz
};
// ACCEL_CONFIG
@@ -131,26 +135,28 @@ enum INT_ENABLE_BIT : uint8_t {
DATA_RDY_INT_EN = Bit0
};
// INT_STATUS
enum INT_STATUS_BIT : uint8_t {
FIFO_OFLOW_INT = Bit4,
DATA_RDY_INT = Bit0,
};
// USER_CTRL
enum USER_CTRL_BIT : uint8_t {
FIFO_EN = Bit6,
FIFO_RST = Bit2,
FIFO_EN = Bit6,
FIFO_RST = Bit2,
SIG_COND_RST = Bit0,
};
// PWR_MGMT_1
enum PWR_MGMT_1_BIT : uint8_t {
DEVICE_RESET = Bit7,
SLEEP = Bit6,
CLKSEL_2 = Bit2,
CLKSEL_1 = Bit1,
CLKSEL_0 = Bit0,
};
// I2C_IF
enum I2C_IF_BIT : uint8_t {
I2C_IF_DIS = Bit6, // 1 – Disable I2C Slave module and put the serial interface in SPI mode only.
};
namespace FIFO
{
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+13 -10
View File
@@ -50,12 +50,12 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len)
return sum;
}
static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
{
unsigned clip_count = 0;
for (int n = 0; n < len; n++) {
if (abs(samples[n]) > clip_limit) {
if (abs(samples[n]) >= clip_limit) {
clip_count++;
}
}
@@ -120,7 +120,10 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
void PX4Accelerometer::set_update_rate(uint16_t rate)
{
_update_rate = rate;
const uint32_t update_interval = 1000000 / rate;
// TODO: set this intelligently
_integrator_reset_samples = 4000 / update_interval;
}
@@ -144,7 +147,7 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
// publish raw data immediately
{
sensor_accel_s report{};
sensor_accel_s report;
report.timestamp_sample = timestamp_sample;
report.device_id = _device_id;
@@ -166,7 +169,7 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
// fill sensor_accel_integrated and publish
sensor_accel_integrated_s report{};
sensor_accel_integrated_s report;
report.timestamp_sample = timestamp_sample;
report.error_count = _error_count;
@@ -208,7 +211,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)};
sensor_accel_s report{};
sensor_accel_s report;
report.timestamp_sample = sample.timestamp_sample;
report.device_id = _device_id;
@@ -269,7 +272,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
delta_velocity *= 1e-6f * dt;
// fill sensor_accel_integrated and publish
sensor_accel_integrated_s report{};
sensor_accel_integrated_s report;
report.timestamp_sample = sample.timestamp_sample;
report.error_count = _error_count;
@@ -316,13 +319,13 @@ void PX4Accelerometer::PublishStatus()
{
// publish sensor status
if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) {
sensor_accel_status_s status{};
sensor_accel_status_s status;
status.device_id = _device_id;
status.error_count = _error_count;
status.full_scale_range = _range;
status.rotation = _rotation;
status.measure_rate = _update_rate;
status.measure_rate_hz = _update_rate;
status.temperature = _temperature;
status.vibration_metric = _vibration_metric;
status.clipping[0] = _clipping[0];
@@ -347,8 +350,8 @@ void PX4Accelerometer::ResetIntegrator()
void PX4Accelerometer::UpdateClipLimit()
{
// 95% of potential max
_clip_limit = (_range / _scale) * 0.95f;
// 99.9% of potential max
_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
}
void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+16 -11
View File
@@ -50,12 +50,12 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len)
return sum;
}
static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
{
unsigned clip_count = 0;
for (int n = 0; n < len; n++) {
if (abs(samples[n]) > clip_limit) {
if (abs(samples[n]) >= clip_limit) {
clip_count++;
}
}
@@ -65,6 +65,7 @@ static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, u
PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
CDev(nullptr),
ModuleParams(nullptr),
_sensor_pub{ORB_ID(sensor_gyro), priority},
_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
_sensor_integrated_pub{ORB_ID(sensor_gyro_integrated), priority},
@@ -74,6 +75,8 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
_rotation_dcm{get_rot_matrix(rotation)}
{
_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
updateParams();
}
PX4Gyroscope::~PX4Gyroscope()
@@ -119,7 +122,10 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
void PX4Gyroscope::set_update_rate(uint16_t rate)
{
_update_rate = rate;
const uint32_t update_interval = 1000000 / rate;
// TODO: set this intelligently
_integrator_reset_samples = 4000 / update_interval;
}
@@ -143,7 +149,7 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
// publish raw data immediately
{
sensor_gyro_s report{};
sensor_gyro_s report;
report.timestamp_sample = timestamp_sample;
report.device_id = _device_id;
@@ -165,7 +171,7 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
if (_integrator.put(timestamp_sample, val_calibrated, delta_angle, integral_dt)) {
// fill sensor_gyro_integrated and publish
sensor_gyro_integrated_s report{};
sensor_gyro_integrated_s report;
report.timestamp_sample = timestamp_sample;
report.error_count = _error_count;
@@ -207,7 +213,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
// Apply range scale and the calibration offset
const Vector3f val_calibrated{(Vector3f{x, y, z} * _scale) - _calibration_offset};
sensor_gyro_s report{};
sensor_gyro_s report;
report.timestamp_sample = sample.timestamp_sample;
report.device_id = _device_id;
@@ -268,7 +274,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
delta_angle *= 1e-6f * dt;
// fill sensor_gyro_integrated and publish
sensor_gyro_integrated_s report{};
sensor_gyro_integrated_s report;
report.timestamp_sample = sample.timestamp_sample;
report.error_count = _error_count;
@@ -315,13 +321,13 @@ void PX4Gyroscope::PublishStatus()
{
// publish sensor status
if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) {
sensor_gyro_status_s status{};
sensor_gyro_status_s status;
status.device_id = _device_id;
status.error_count = _error_count;
status.full_scale_range = _range;
status.rotation = _rotation;
status.measure_rate = _update_rate;
status.measure_rate_hz = _update_rate;
status.temperature = _temperature;
status.vibration_metric = _vibration_metric;
status.coning_vibration = _coning_vibration;
@@ -347,11 +353,10 @@ void PX4Gyroscope::ResetIntegrator()
void PX4Gyroscope::UpdateClipLimit()
{
// 95% of potential max
_clip_limit = (_range / _scale) * 0.95f;
// 99.9% of potential max
_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
}
void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
{
// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+10 -4
View File
@@ -38,6 +38,7 @@
#include <lib/cdev/CDev.hpp>
#include <lib/conversion/rotation.h>
#include <lib/drivers/device/integrator.h>
#include <px4_platform_common/module_params.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/PublicationQueuedMulti.hpp>
#include <uORB/topics/sensor_gyro.h>
@@ -45,7 +46,7 @@
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_gyro_status.h>
class PX4Gyroscope : public cdev::CDev
class PX4Gyroscope : public cdev::CDev, public ModuleParams
{
public:
PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
@@ -55,6 +56,8 @@ public:
uint32_t get_device_id() const { return _device_id; }
float get_max_rate_hz() const { return _param_imu_gyro_rate_max.get(); }
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; }
@@ -72,9 +75,9 @@ public:
uint8_t samples; // number of samples
float dt; // in microseconds
int16_t x[16];
int16_t y[16];
int16_t z[16];
int16_t x[32];
int16_t y[32];
int16_t z[32];
};
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");
@@ -131,4 +134,7 @@ private:
uint8_t _integrator_fifo_samples{0};
uint8_t _integrator_clipping{0};
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
)
};