Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 16:56:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

imu/invensense/icm20948: sync with other recent invensense improvements

Browse Source 
- clenaup ak09916 with simplifed setup and health check
This commit is contained in:
Daniel Agar
2020-06-18 11:11:34 -04:00
parent e50f92805b
commit d9102ce54c
19 changed files with 246 additions and 2854 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/px4/fmu-v2/default.cmake
+1 -1
View File
@@ -39,9 +39,9 @@ px4_add_board(
imu/l3gd20
imu/lsm303d
#imu/invensense/icm20608g
#imu/invensense/icm20948
imu/invensense/mpu6000
#imu/invensense/mpu9250
#imu/icm20948
#imu/mpu6000 # legacy mpu6000
#iridiumsbd
#irlock
boards/px4/fmu-v3/default.cmake
+1 -1
View File
@@ -35,9 +35,9 @@ px4_add_board(
imu/l3gd20
imu/lsm303d
imu/invensense/icm20608g
imu/invensense/icm20948
imu/invensense/mpu6000
imu/invensense/mpu9250
imu/icm20948
irlock
lights/blinkm
lights/rgbled
boards/px4/fmu-v3/rtps.cmake
+1 -1
View File
@@ -35,9 +35,9 @@ px4_add_board(
imu/l3gd20
imu/lsm303d
imu/invensense/icm20608g
imu/invensense/icm20948
imu/invensense/mpu6000
imu/invensense/mpu9250
imu/icm20948
irlock
lights/blinkm
lights/rgbled
src/drivers/imu/icm20948/CMakeLists.txt
-45
View File
@@ -1,45 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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__icm20948
MAIN icm20948
COMPILE_FLAGS
SRCS
icm20948.cpp
icm20948_i2c.cpp
icm20948_spi.cpp
icm20948_main.cpp
ICM20948_mag.cpp
mag_i2c.cpp
DEPENDS
)
src/drivers/imu/icm20948/ICM20948_mag.cpp
-333
View File
@@ -1,333 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 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 mag.cpp
*
* Driver for the ak09916 magnetometer within the Invensense icm20948
*
* @author Robert Dickenson
*
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/time.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include "ICM20948_mag.h"
#include "icm20948.h"
// If interface is non-null, then it will used for interacting with the device.
// Otherwise, it will passthrough the parent ICM20948
ICM20948_mag::ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rotation rotation) :
_interface(interface),
_px4_mag(parent->_interface->get_device_id(), (parent->_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH),
rotation),
_parent(parent),
_mag_overruns(perf_alloc(PC_COUNT, MODULE_NAME": mag overruns")),
_mag_overflows(perf_alloc(PC_COUNT, MODULE_NAME": mag overflows")),
_mag_errors(perf_alloc(PC_COUNT, MODULE_NAME": mag errors"))
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916);
_px4_mag.set_external(_parent->is_external());
_px4_mag.set_scale(ICM20948_MAG_RANGE_GA);
}
ICM20948_mag::~ICM20948_mag()
{
perf_free(_mag_overruns);
perf_free(_mag_overflows);
perf_free(_mag_errors);
}
void
ICM20948_mag::measure()
{
const hrt_abstime timestamp_sample = hrt_absolute_time();
uint8_t st1 = 0;
int ret = _interface->read(AK09916REG_ST1, &st1, sizeof(st1));
if (ret != OK) {
_px4_mag.set_error_count(perf_event_count(_mag_errors));
return;
}
/* Check if data ready is set.
* This is not described to be set in continuous mode according to the
* MPU9250 datasheet. However, the datasheet of the 8963 recommends to
* check data ready before doing the read and before triggering the
* next measurement by reading ST2. */
if (!(st1 & AK09916_ST1_DRDY)) {
return;
}
/* Monitor if data overrun flag is ever set. */
if (st1 & 0x02) {
perf_count(_mag_overruns);
}
ak09916_regs data{};
ret = _interface->read(AK09916REG_ST1, &data, sizeof(data));
if (ret != OK) {
_px4_mag.set_error_count(perf_event_count(_mag_errors));
return;
}
/* Monitor magnetic sensor overflow flag. */
if (data.st2 & 0x08) {
perf_count(_mag_overflows);
}
_measure(timestamp_sample, data);
}
void
ICM20948_mag::_measure(hrt_abstime timestamp_sample, ak09916_regs data)
{
/* Check if data ready is set.
* This is not described to be set in continuous mode according to the
* MPU9250 datasheet. However, the datasheet of the 8963 recommends to
* check data ready before doing the read and before triggering the
* next measurement by reading ST2.
*
* If _measure is used in passthrough mode, all the data is already
* fetched, however, we should still not use the data if the data ready
* is not set. This has lead to intermittent spikes when the data was
* being updated while getting read.
*/
if (!(data.st1 & AK09916_ST1_DRDY)) {
return;
}
_px4_mag.set_external(_parent->is_external());
_px4_mag.set_temperature(_parent->_last_temperature);
/*
* Align axes - Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them.
*/
_px4_mag.update(timestamp_sample, data.y, data.x, -data.z);
}
void
ICM20948_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out)
{
uint8_t addr;
_parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers
if (out) {
_parent->write_reg(ICMREG_20948_I2C_SLV0_DO, *out);
addr = AK09916_I2C_ADDR;
} else {
addr = AK09916_I2C_ADDR | BIT_I2C_READ_FLAG;
}
_parent->write_reg(ICMREG_20948_I2C_SLV0_ADDR, addr);
_parent->write_reg(ICMREG_20948_I2C_SLV0_REG, reg);
_parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, size | BIT_I2C_SLV0_EN);
}
void
ICM20948_mag::read_block(uint8_t reg, uint8_t *val, uint8_t count)
{
_parent->_interface->read(reg, val, count);
}
void
ICM20948_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
{
set_passthrough(reg, size);
px4_usleep(25 + 25 * size); // wait for the value to be read from slave
read_block(ICMREG_20948_EXT_SLV_SENS_DATA_00, buf, size);
_parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, 0); // disable new reads
}
uint8_t
ICM20948_mag::read_reg(unsigned int reg)
{
uint8_t buf{};
if (_interface == nullptr) {
passthrough_read(reg, &buf, 0x01);
} else {
_interface->read(reg, &buf, 1);
}
return buf;
}
bool
ICM20948_mag::ak09916_check_id(uint8_t &deviceid)
{
deviceid = read_reg(AK09916REG_WIA);
return (AK09916_DEVICE_ID == deviceid);
}
/*
* 400kHz I2C bus speed = 2.5us per bit = 25us per byte
*/
void
ICM20948_mag::passthrough_write(uint8_t reg, uint8_t val)
{
set_passthrough(reg, 1, &val);
px4_usleep(50); // wait for the value to be written to slave
_parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, 0); // disable new writes
}
void
ICM20948_mag::write_reg(unsigned reg, uint8_t value)
{
// general register transfer at low clock speed
if (_interface == nullptr) {
passthrough_write(reg, value);
} else {
_interface->write(ICM20948_LOW_SPEED_OP(reg), &value, 1);
}
}
int
ICM20948_mag::ak09916_reset()
{
// First initialize it to use the bus
int rv = ak09916_setup();
if (rv == OK) {
// Now reset the mag
write_reg(AK09916REG_CNTL3, AK09916_RESET);
// Then re-initialize the bus/mag
rv = ak09916_setup();
}
return rv;
}
bool
ICM20948_mag::ak09916_read_adjustments()
{
uint8_t response[3];
float ak09916_ASA[3];
write_reg(AK09916REG_CNTL1, AK09916_FUZE_MODE | AK09916_16BIT_ADC);
px4_usleep(50);
if (_interface != nullptr) {
_interface->read(AK09916REG_ASAX, response, 3);
} else {
passthrough_read(AK09916REG_ASAX, response, 3);
}
write_reg(AK09916REG_CNTL1, AK09916_POWERDOWN_MODE);
for (int i = 0; i < 3; i++) {
if (0 != response[i] && 0xff != response[i]) {
ak09916_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
} else {
return false;
}
}
_px4_mag.set_sensitivity(ak09916_ASA[0], ak09916_ASA[1], ak09916_ASA[2]);
return true;
}
int
ICM20948_mag::ak09916_setup_master_i2c()
{
/* When _interface is null we are using SPI and must
* use the parent interface to configure the device to act
* in master mode (SPI to I2C bridge)
*/
if (_interface == nullptr) {
// ICM20948 -> AK09916
_parent->modify_checked_reg(ICMREG_20948_USER_CTRL, 0, BIT_I2C_MST_EN);
// WAIT_FOR_ES does not exist for ICM20948. Not sure how to replace this (or if that is needed)
_parent->write_reg(ICMREG_20948_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | ICM_BITS_I2C_MST_CLOCK_400HZ);
} else {
_parent->modify_checked_reg(ICMREG_20948_USER_CTRL, BIT_I2C_MST_EN, 0);
}
return OK;
}
int
ICM20948_mag::ak09916_setup(void)
{
int retries = 10;
do {
ak09916_setup_master_i2c();
write_reg(AK09916REG_CNTL3, AK09916_RESET);
uint8_t id = 0;
if (ak09916_check_id(id)) {
break;
}
retries--;
PX4_WARN("AK09916: bad id %d retries %d", id, retries);
_parent->modify_reg(ICMREG_20948_USER_CTRL, 0, BIT_I2C_MST_RST);
px4_usleep(100) ;
} while (retries > 0);
if (retries == 0) {
PX4_ERR("AK09916: failed to initialize, disabled!");
_parent->modify_checked_reg(ICMREG_20948_USER_CTRL, BIT_I2C_MST_EN, 0);
_parent->write_reg(ICMREG_20948_I2C_MST_CTRL, 0);
return -EIO;
}
write_reg(AK09916REG_CNTL2, AK09916_CNTL2_CONTINOUS_MODE_100HZ);
if (_interface == nullptr) {
// Configure mpu' I2c Master interface to read ak09916 data into to fifo
set_passthrough(AK09916REG_ST1, sizeof(ak09916_regs));
}
return OK;
}
src/drivers/imu/icm20948/ICM20948_mag.h
-152
View File
@@ -1,152 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 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.
*
****************************************************************************/
#pragma once
#include <perf/perf_counter.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <drivers/device/Device.hpp>
/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
static constexpr float ICM20948_MAG_RANGE_GA{1.5e-3f};
#define ICM20948_AK09916_SAMPLE_RATE 100
#define AK09916_I2C_ADDR 0x0C
#define AK09916_DEVICE_ID 0x48
#define AK09916REG_WIA 0x00
#define AK09916REG_CNTL1 0x0A
#define AK09916REG_ASAX 0x10
#define AK09916_SINGLE_MEAS_MODE 0x01
#define AK09916_CONTINUOUS_MODE1 0x02
#define AK09916_CONTINUOUS_MODE2 0x06
#define AK09916_POWERDOWN_MODE 0x00
#define AK09916_SELFTEST_MODE 0x08
#define AK09916_FUZE_MODE 0x0F
#define AK09916_16BIT_ADC 0x10
#define AK09916_14BIT_ADC 0x00
#define AK09916_RESET 0x01
#define AK09916_HOFL 0x08
/* ak09916 deviating register addresses and bit definitions */
#define AK09916_DEVICE_ID_A 0x48 // same as AK09916
#define AK09916_DEVICE_ID_B 0x09 // additional ID byte ("INFO" on AK9063 without content specification.)
#define AK09916REG_HXL 0x11
#define AK09916REG_HXH 0x12
#define AK09916REG_HYL 0x13
#define AK09916REG_HYH 0x14
#define AK09916REG_HZL 0x15
#define AK09916REG_HZH 0x16
#define AK09916REG_ST1 0x10
#define AK09916REG_ST2 0x18
#define AK09916REG_CNTL2 0x31
#define AK09916REG_CNTL3 0x32
#define AK09916_CNTL2_POWERDOWN_MODE 0x00
#define AK09916_CNTL2_SINGLE_MODE 0x01 /* default */
#define AK09916_CNTL2_CONTINOUS_MODE_10HZ 0x02
#define AK09916_CNTL2_CONTINOUS_MODE_20HZ 0x04
#define AK09916_CNTL2_CONTINOUS_MODE_50HZ 0x06
#define AK09916_CNTL2_CONTINOUS_MODE_100HZ 0x08
#define AK09916_CNTL2_SELFTEST_MODE 0x10
#define AK09916_CNTL3_SRST 0x01
#define AK09916_ST1_DRDY 0x01
#define AK09916_ST1_DOR 0x02
class ICM20948;
#pragma pack(push, 1)
struct ak09916_regs {
uint8_t st1;
int16_t x;
int16_t y;
int16_t z;
uint8_t tmps;
uint8_t st2;
};
#pragma pack(pop)
extern device::Device *AK09916_I2C_interface(int bus, int bus_frequency);
/**
* Helper class implementing the magnetometer driver node.
*/
class ICM20948_mag
{
public:
ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rotation rotation);
~ICM20948_mag();
void set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = NULL);
void passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size);
void passthrough_write(uint8_t reg, uint8_t val);
void read_block(uint8_t reg, uint8_t *val, uint8_t count);
int ak09916_reset();
int ak09916_setup();
int ak09916_setup_master_i2c();
bool ak09916_check_id(uint8_t &id);
bool ak09916_read_adjustments();
void print_status() { _px4_mag.print_status(); }
protected:
device::Device *_interface;
friend class ICM20948;
void measure();
void _measure(hrt_abstime timestamp_sample, ak09916_regs data);
uint8_t read_reg(unsigned reg);
void write_reg(unsigned reg, uint8_t value);
bool is_passthrough() { return _interface == nullptr; }
private:
PX4Magnetometer _px4_mag;
ICM20948 *_parent;
bool _mag_reading_data{false};
perf_counter_t _mag_overruns;
perf_counter_t _mag_overflows;
perf_counter_t _mag_errors;
};
src/drivers/imu/icm20948/icm20948.cpp
-832
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/icm20948/icm20948.h
-532
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/icm20948/icm20948_i2c.cpp
-125
View File
@@ -1,125 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 icm20948_i2c.cpp
*
* I2C interface for ICM20948
*/
#include <px4_platform_common/px4_config.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_device.h>
#include "icm20948.h"
device::Device *ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency);
class ICM20948_I2C : public device::I2C
{
public:
ICM20948_I2C(int bus, uint32_t address, int bus_frequency);
~ICM20948_I2C() override = default;
int read(unsigned address, void *data, unsigned count) override;
int write(unsigned address, void *data, unsigned count) override;
protected:
virtual int probe() override;
private:
};
device::Device *
ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency)
{
return new ICM20948_I2C(bus, address, bus_frequency);
}
ICM20948_I2C::ICM20948_I2C(int bus, uint32_t address, int bus_frequency) :
I2C(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, address, bus_frequency)
{
}
int
ICM20948_I2C::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[2] {};
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
cmd[0] = ICM20948_REG(reg_speed);
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], count + 1, nullptr, 0);
}
int
ICM20948_I2C::read(unsigned reg_speed, void *data, unsigned count)
{
/* We want to avoid copying the data of MPUReport: So if the caller
* supplies a buffer not MPUReport in size, it is assume to be a reg or
* reg 16 read
* Since MPUReport has a cmd at front, we must return the data
* after that. Foe anthing else we must return it
*/
uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status);
uint8_t cmd = ICM20948_REG(reg_speed);
return transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
}
int
ICM20948_I2C::probe()
{
uint8_t whoami = 0;
uint8_t register_select = REG_BANK(BANK0); // register bank containing WHOAMI for ICM20948
// Try first for icm20948/6500
read(MPUREG_WHOAMI, &whoami, 1);
/*
* If it's not an MPU it must be an ICM
* Make sure register bank 0 is selected - whoami is only present on bank 0, and that is
* not sure e.g. if the device has rebooted without repowering the sensor
*/
write(ICMREG_20948_BANK_SEL, &register_select, 1);
read(ICMREG_20948_WHOAMI, &whoami, 1);
if (whoami == ICM_WHOAMI_20948) {
return PX4_OK;
}
return -ENODEV;
}
src/drivers/imu/icm20948/icm20948_main.cpp
-140
View File
@@ -1,140 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file main.cpp
*
* Driver for the Invensense icm20948 connected via I2C or SPI.
*/
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include "icm20948.h"
void
ICM20948::print_usage()
{
PRINT_MODULE_USAGE_NAME("icm20948", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
I2CSPIDriverBase *ICM20948::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
device::Device *interface = nullptr;
device::Device *mag_interface = nullptr;
if (iterator.busType() == BOARD_I2C_BUS) {
interface = ICM20948_I2C_interface(iterator.bus(), PX4_I2C_EXT_ICM20948_1, cli.bus_frequency);
// For i2c interfaces, connect to the magnetometer directly
mag_interface = AK09916_I2C_interface(iterator.bus(), cli.bus_frequency);
} else if (iterator.busType() == BOARD_SPI_BUS) {
interface = ICM20948_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode);
}
if (interface == nullptr) {
PX4_ERR("alloc failed");
delete mag_interface;
return nullptr;
}
if (interface->init() != OK) {
delete interface;
delete mag_interface;
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
return nullptr;
}
ICM20948 *dev = new ICM20948(interface, mag_interface, cli.rotation, iterator.configuredBusOption(), iterator.bus());
if (dev == nullptr) {
delete interface;
delete mag_interface;
return nullptr;
}
if (OK != dev->init()) {
delete dev;
return nullptr;
}
return dev;
}
extern "C" int
icm20948_main(int argc, char *argv[])
{
int ch;
using ThisDriver = ICM20948;
BusCLIArguments cli{true, true};
cli.default_spi_frequency = 20 * 1000 * 1000;
cli.default_i2c_frequency = 400000;
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
}
}
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM20948);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}
src/drivers/imu/icm20948/icm20948_spi.cpp
-182
View File
@@ -1,182 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file icm20948_spi.cpp
*
* Driver for the Invensense ICM20948 connected via SPI.
*
* @author Andrew Tridgell
* @author Pat Hickey
* @author David sidrane
*/
#include <drivers/device/spi.h>
#include "icm20948.h"
#define DIR_READ 0x80
#define DIR_WRITE 0x00
/*
* The ICM20948 can only handle high SPI bus speeds of 20Mhz on the sensor and
* interrupt status registers. All other registers have a maximum 1MHz
* SPI speed
*
* The Actual Value will be rounded down by the spi driver.
* for a 168Mhz CPU this will be 10.5 Mhz and for a 180 Mhz CPU
* it will be 11.250 Mhz
*/
#define ICM20948_LOW_SPI_BUS_SPEED 1000*1000
#define ICM20948_HIGH_SPI_BUS_SPEED 20*1000*1000
device::Device *ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
class ICM20948_SPI : public device::SPI
{
public:
ICM20948_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
~ICM20948_SPI() override = default;
int read(unsigned address, void *data, unsigned count) override;
int write(unsigned address, void *data, unsigned count) override;
protected:
int probe() override;
private:
/* Helper to set the desired speed and isolate the register on return */
void set_bus_frequency(unsigned &reg_speed_reg_out);
const int _high_bus_speed;
};
device::Device *
ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
{
return new ICM20948_SPI(bus, devid, bus_frequency, spi_mode);
}
ICM20948_SPI::ICM20948_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
SPI(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, device, spi_mode, ICM20948_LOW_SPI_BUS_SPEED),
_high_bus_speed(bus_frequency)
{
}
void
ICM20948_SPI::set_bus_frequency(unsigned &reg_speed)
{
/* Set the desired speed */
set_frequency(ICM20948_IS_HIGH_SPEED(reg_speed) ? _high_bus_speed : ICM20948_LOW_SPI_BUS_SPEED);
/* Isoolate the register on return */
reg_speed = ICM20948_REG(reg_speed);
}
int
ICM20948_SPI::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[2] {};
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
/* Set the desired speed and isolate the register */
set_bus_frequency(reg_speed);
cmd[0] = reg_speed | DIR_WRITE;
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], &cmd[0], count + 1);
}
int
ICM20948_SPI::read(unsigned reg_speed, void *data, unsigned count)
{
/* We want to avoid copying the data of MPUReport: So if the caller
* supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read
* and we need to provied the buffer large enough for the callers data
* and our command.
*/
uint8_t cmd[3] {};
uint8_t *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ;
if (count < sizeof(MPUReport)) {
/* add command */
count++;
}
set_bus_frequency(reg_speed);
/* Set command */
pbuff[0] = reg_speed | DIR_READ ;
/* Transfer the command and get the data */
int ret = transfer(pbuff, pbuff, count);
if (ret == OK && pbuff == &cmd[0]) {
/* Adjust the count back */
count--;
/* Return the data */
memcpy(data, &cmd[1], count);
}
return ret;
}
int
ICM20948_SPI::probe()
{
uint8_t whoami = 0;
int ret = read(MPUREG_WHOAMI, &whoami, 1);
if (ret != OK) {
return -EIO;
}
switch (whoami) {
case ICM_WHOAMI_20948:
ret = 0;
break;
default:
PX4_WARN("probe failed! %u", whoami);
ret = -EIO;
}
return ret;
}
src/drivers/imu/icm20948/mag_i2c.cpp
-108
View File
@@ -1,108 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 mag_i2c.cpp
*
* I2C interface for AK09916
*/
#include "icm20948.h"
#include "ICM20948_mag.h"
#include <drivers/device/i2c.h>
device::Device *AK09916_I2C_interface(int bus, int bus_frequency);
class AK09916_I2C : public device::I2C
{
public:
AK09916_I2C(int bus, int bus_frequency);
~AK09916_I2C() override = default;
int read(unsigned address, void *data, unsigned count) override;
int write(unsigned address, void *data, unsigned count) override;
protected:
int probe() override;
};
device::Device *
AK09916_I2C_interface(int bus, int bus_frequency)
{
return new AK09916_I2C(bus, bus_frequency);
}
AK09916_I2C::AK09916_I2C(int bus, int bus_frequency) :
I2C(DRV_IMU_DEVTYPE_ICM20948, "AK09916_I2C", bus, AK09916_I2C_ADDR, bus_frequency)
{
}
int
AK09916_I2C::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[2] {};
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
cmd[0] = ICM20948_REG(reg_speed);
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], count + 1, nullptr, 0);
}
int
AK09916_I2C::read(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd = ICM20948_REG(reg_speed);
return transfer(&cmd, 1, (uint8_t *)data, count);
}
int
AK09916_I2C::probe()
{
uint8_t whoami = 0;
uint8_t expected = AK09916_DEVICE_ID;
if (PX4_OK != read(AK09916REG_WIA, &whoami, 1)) {
return -EIO;
}
if (whoami != expected) {
return -EIO;
}
return OK;
}
src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp
+16 -14
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2020 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
@@ -58,12 +58,14 @@ static constexpr uint8_t Bit7 = (1 << 7);
static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface
static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b0001100;
static constexpr uint8_t WHOAMI = 0x09;
static constexpr uint8_t Company_ID = 0x48;
static constexpr uint8_t Device_ID = 0x09;
enum class Register : uint8_t {
WIA = 0x01, // Device ID
WIA1 = 0x00, // Company ID of AKM
WIA2 = 0x01, // Device ID of AK09916
ST1 = 0x10, // Status 1
ST1 = 0x10, // Status 1
HXL = 0x11,
HXH = 0x12,
HYL = 0x13,
@@ -71,30 +73,30 @@ enum class Register : uint8_t {
HZL = 0x15,
HZH = 0x16,
ST2 = 0x18, // Status 2
ST2 = 0x18, // Status 2
CNTL2 = 0x31, // Control 2
CNTL3 = 0x32, // Control 3
CNTL2 = 0x31, // Control 2
CNTL3 = 0x32, // Control 3
};
// ST1
enum ST1_BIT : uint8_t {
DOR = Bit1, // Data overrun
DRDY = Bit0, // Data is ready
DOR = Bit1, // Data overrun
DRDY = Bit0, // Data is ready
};
// ST2
enum ST2_BIT : uint8_t {
BITM = Bit4, // Output bit setting (mirror)
HOFL = Bit3, // Magnetic sensor overflow
};
// CNTL2
enum CNTL2_BIT : uint8_t {
MODE1 = Bit1, // Continuous measurement mode 1 (10Hz)
MODE2 = Bit2, // Continuous measurement mode 2 (20Hz)
MODE3 = Bit2 | Bit1, // Continuous measurement mode 3 (50Hz)
MODE4 = Bit3, // Continuous measurement mode 4 (100Hz)
// MODE[4:0] bits
MODE1 = Bit1, // “00010”: Continuous measurement mode 1 (10Hz)
MODE2 = Bit2, // “00100”: Continuous measurement mode 2 (20Hz)
MODE3 = Bit2 | Bit1, // “00110”: Continuous measurement mode 3 (50Hz)
MODE4 = Bit3, // “01000”: Continuous measurement mode 4 (100Hz)
};
// CNTL3
src/drivers/imu/invensense/icm20948/CMakeLists.txt
+1
View File
@@ -33,6 +33,7 @@
px4_add_module(
MODULE drivers__imu__icm20948
MAIN icm20948
COMPILE_FLAGS
SRCS
AKM_AK09916_registers.hpp
ICM20948.cpp
src/drivers/imu/invensense/icm20948/ICM20948.cpp
+168 -174
View File
File diff suppressed because it is too large Load Diff
src/drivers/imu/invensense/icm20948/ICM20948.hpp
+13 -19
View File
@@ -76,9 +76,9 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4500 Hz accel
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
@@ -130,7 +130,7 @@ private:
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
template <typename T> bool RegisterCheck(const T &reg_cfg, bool notify = false);
template <typename T> bool RegisterCheck(const T &reg_cfg);
template <typename T> uint8_t RegisterRead(T reg);
template <typename T> void RegisterWrite(T reg, uint8_t value);
template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
@@ -138,25 +138,23 @@ private:
template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
void FIFOReset();
bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void UpdateTemperature();
const spi_drdy_gpio_t _drdy_gpio;
// I2C AUX interface (slave 1 - 4)
AKM_AK09916::ICM20948_AK09916 *_slave_ak09916_magnetometer{nullptr};
friend class AKM_AK09916::ICM20948_AK09916;
void I2CSlaveRegisterStartRead(uint8_t slave_i2c_addr, uint8_t reg);
void I2CSlaveRegisterWrite(uint8_t slave_i2c_addr, uint8_t reg, uint8_t val);
void I2CSlaveExternalSensorDataEnable(uint8_t slave_i2c_addr, uint8_t reg, uint8_t size);
bool I2CSlaveExternalSensorDataRead(uint8_t *buffer, uint8_t length);
AKM_AK09916::ICM20948_AK09916 *_slave_ak09916_magnetometer{nullptr};
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
@@ -166,19 +164,18 @@ private:
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")};
perf_counter_t _drdy_interval_perf{nullptr};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _fifo_watermark_interrupt_timestamp{0};
hrt_abstime _temperature_update_timestamp{0};
unsigned _consecutive_failures{0};
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
px4::atomic<uint8_t> _data_ready_count{0};
px4::atomic<uint8_t> _fifo_read_samples{0};
px4::atomic<uint8_t> _drdy_fifo_read_samples{0};
px4::atomic<uint8_t> _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
bool _force_fifo_count_check{true};
enum class STATE : uint8_t {
RESET,
@@ -191,7 +188,6 @@ private:
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 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))};
uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{6};
@@ -203,7 +199,6 @@ private:
{ Register::BANK_0::INT_ENABLE_1, INT_ENABLE_1_BIT::RAW_DATA_0_RDY_EN, 0 },
{ Register::BANK_0::FIFO_EN_2, FIFO_EN_2_BIT::ACCEL_FIFO_EN | FIFO_EN_2_BIT::GYRO_Z_FIFO_EN | FIFO_EN_2_BIT::GYRO_Y_FIFO_EN | FIFO_EN_2_BIT::GYRO_X_FIFO_EN, FIFO_EN_2_BIT::TEMP_FIFO_EN },
{ Register::BANK_0::FIFO_MODE, FIFO_MODE_BIT::Snapshot, 0 },
// { Register::BANK_0::FIFO_CFG, FIFO_CFG_BIT::FIFO_CFG, 0 }, // TODO: enable data ready interrupt
};
uint8_t _checked_register_bank2{0};
@@ -215,10 +210,9 @@ private:
};
uint8_t _checked_register_bank3{0};
static constexpr uint8_t size_register_bank3_cfg{4};
static constexpr uint8_t size_register_bank3_cfg{3};
register_bank3_config_t _register_bank3_cfg[size_register_bank3_cfg] {
// Register | Set bits, Clear bits
{ Register::BANK_3::I2C_MST_ODR_CONFIG, 0, 0 },
{ Register::BANK_3::I2C_MST_CTRL, 0, 0 },
{ Register::BANK_3::I2C_MST_DELAY_CTRL, 0, 0 },
{ Register::BANK_3::I2C_SLV4_CTRL, 0, 0 },
src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp
+42 -165
View File
@@ -52,22 +52,16 @@ ICM20948_AK09916::ICM20948_AK09916(ICM20948 &icm20948, enum Rotation rotation) :
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916);
_px4_mag.set_external(icm20948.external());
// mag resolution is 1.5 milli Gauss per bit (0.15 μT/LSB)
_px4_mag.set_scale(1.5e-3f);
}
ICM20948_AK09916::~ICM20948_AK09916()
{
ScheduleClear();
perf_free(_transfer_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_duplicate_data_perf);
perf_free(_data_not_ready);
}
bool ICM20948_AK09916::Init()
{
return Reset();
perf_free(_magnetic_sensor_overflow_perf);
}
bool ICM20948_AK09916::Reset()
@@ -81,10 +75,8 @@ bool ICM20948_AK09916::Reset()
void ICM20948_AK09916::PrintInfo()
{
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_duplicate_data_perf);
perf_print_counter(_data_not_ready);
perf_print_counter(_magnetic_sensor_overflow_perf);
_px4_mag.print_status();
}
@@ -94,38 +86,40 @@ void ICM20948_AK09916::Run()
switch (_state) {
case STATE::RESET:
// CNTL3 SRST: Soft reset
RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST);
_icm20948.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL3, CNTL3_BIT::SRST);
_reset_timestamp = hrt_absolute_time();
_consecutive_failures = 0;
_state = STATE::READ_WHO_AM_I;
ScheduleDelayed(100_ms);
break;
case STATE::READ_WHO_AM_I:
_icm20948.I2CSlaveRegisterStartRead(I2C_ADDRESS_DEFAULT, (uint8_t)Register::WIA);
_icm20948.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::WIA1, 1);
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(10_ms);
ScheduleDelayed(100_ms);
break;
case STATE::WAIT_FOR_RESET: {
uint8_t WIA = 0;
_icm20948.I2CSlaveExternalSensorDataRead(&WIA, 1);
uint8_t WIA1 = 0;
_icm20948.I2CSlaveExternalSensorDataRead(&WIA1, 1);
if (WIA == WHOAMI) {
// if reset succeeded then configure
PX4_DEBUG("AK09916 reset successful, configuring");
_state = STATE::CONFIGURE;
ScheduleDelayed(10_ms);
if (WIA1 == Company_ID) {
// set continuous mode 3 (50 Hz)
_icm20948.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL2, CNTL2_BIT::MODE3);
_state = STATE::READ;
ScheduleOnInterval(20_ms, 100_ms); // 50 Hz
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
PX4_DEBUG("Reset failed, retrying");
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("AK09916 reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
ScheduleDelayed(1000_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 100 ms");
PX4_DEBUG("AK09916 reset not complete, check again in 100 ms");
ScheduleDelayed(100_ms);
}
}
@@ -133,166 +127,49 @@ void ICM20948_AK09916::Run()
break;
// TODO: read FUSE ROM (to get ASA corrections)
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start reading
PX4_DEBUG("AK09916 configure successful, reading");
_icm20948.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::ST1, sizeof(TransferBuffer));
_state = STATE::READ;
ScheduleOnInterval(20_ms, 20_ms); // 50 Hz
} else {
PX4_DEBUG("Configure failed, retrying");
// try again in 100 ms
ScheduleDelayed(100_ms);
}
break;
case STATE::READ: {
perf_begin(_transfer_perf);
TransferBuffer buffer{};
const hrt_abstime timestamp_sample = hrt_absolute_time();
bool success = _icm20948.I2CSlaveExternalSensorDataRead((uint8_t *)&buffer, sizeof(TransferBuffer));
bool ret = _icm20948.I2CSlaveExternalSensorDataRead((uint8_t *)&buffer, sizeof(TransferBuffer));
perf_end(_transfer_perf);
if (success && !(buffer.ST2 & ST2_BIT::HOFL) && (buffer.ST1 & ST1_BIT::DRDY)) {
// sensor's frame is +y forward (x), -x right, +z down
int16_t x = combine(buffer.HYH, buffer.HYL); // +Y
int16_t y = combine(buffer.HXH, buffer.HXL); // +X
y = (y == INT16_MIN) ? INT16_MAX : -y; // flip y
int16_t z = combine(buffer.HZH, buffer.HZL);
bool success = false;
const bool all_zero = (x == 0 && y == 0 && z == 0);
const bool new_data = (_last_measurement[0] != x || _last_measurement[1] != y || _last_measurement[2] != z);
if (ret) {
if (buffer.ST2 & ST2_BIT::HOFL) {
perf_count(_magnetic_sensor_overflow_perf);
if (!new_data) {
perf_count(_duplicate_data_perf);
}
} else if (buffer.ST1 & ST1_BIT::DRDY) {
const int16_t x = combine(buffer.HXH, buffer.HXL);
const int16_t y = combine(buffer.HYH, buffer.HYL);
const int16_t z = combine(buffer.HZH, buffer.HZL);
if (!all_zero && new_data) {
// sensor's frame is +X forward (X), +Y right (Y), +Z down (Z)
_px4_mag.update(timestamp_sample, x, y, z);
_last_measurement[0] = x;
_last_measurement[1] = y;
_last_measurement[2] = z;
success = true;
} else {
success = false;
_consecutive_failures = 0;
}
} else {
perf_count(_data_not_ready);
}
if (!success) {
perf_count(_bad_transfer_perf);
_consecutive_failures++;
if (_consecutive_failures > 10) {
Reset();
return;
}
}
// ensure icm20948 slave sensor reading is configured
_icm20948.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::ST1, sizeof(TransferBuffer));
}
break;
}
}
bool ICM20948_AK09916::Configure()
{
bool success = true;
for (const auto &reg : _register_cfg) {
if (!RegisterCheck(reg)) {
success = false;
}
}
// TODO: read ASA and set sensitivity
//const uint8_t ASAX = RegisterRead(Register::ASAX);
//const uint8_t ASAY = RegisterRead(Register::ASAY);
//const uint8_t ASAZ = RegisterRead(Register::ASAZ);
// float ak8963_ASA[3] {};
// for (int i = 0; i < 3; i++) {
// if (0 != response[i] && 0xff != response[i]) {
// ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
// } else {
// return false;
// }
// }
// _px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]);
// in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
_px4_mag.set_scale(1.5e-3f);
return success;
}
bool ICM20948_AK09916::RegisterCheck(const register_config_t &reg_cfg, bool notify)
{
bool success = true;
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false;
}
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
success = false;
}
if (!success) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
if (notify) {
perf_count(_bad_register_perf);
_px4_mag.increase_error_count();
}
}
return success;
}
uint8_t ICM20948_AK09916::RegisterRead(Register reg)
{
// TODO: use slave 4 and check register
_icm20948.I2CSlaveRegisterStartRead(I2C_ADDRESS_DEFAULT, static_cast<uint8_t>(reg));
usleep(1000);
uint8_t buffer{};
_icm20948.I2CSlaveExternalSensorDataRead(&buffer, 1);
return buffer;
}
void ICM20948_AK09916::RegisterWrite(Register reg, uint8_t value)
{
return _icm20948.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, static_cast<uint8_t>(reg), value);
}
void ICM20948_AK09916::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = orig_val;
if (setbits) {
val |= setbits;
}
if (clearbits) {
val &= ~clearbits;
}
RegisterWrite(reg, val);
}
} // namespace AKM_AK09916
src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp
+3 -29
View File
@@ -46,7 +46,6 @@
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
class ICM20948;
@@ -60,12 +59,9 @@ public:
ICM20948_AK09916(ICM20948 &icm20948, enum Rotation rotation = ROTATION_NONE);
~ICM20948_AK09916() override;
bool Init();
bool Reset();
void PrintInfo();
void set_temperature(float temperature) { _px4_mag.set_temperature(temperature); }
private:
struct TransferBuffer {
@@ -86,48 +82,26 @@ private:
uint8_t clear_bits{0};
};
int probe();
void Run() override;
bool Configure();
bool RegisterCheck(const register_config_t &reg_cfg, bool notify = false);
uint8_t RegisterRead(AKM_AK09916::Register reg);
void RegisterWrite(AKM_AK09916::Register reg, uint8_t value);
void RegisterSetAndClearBits(AKM_AK09916::Register reg, uint8_t setbits, uint8_t clearbits);
ICM20948 &_icm20948;
PX4Magnetometer _px4_mag;
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME"_ak09916: transfer")};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: bad transfer")};
perf_counter_t _duplicate_data_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: duplicate data")};
perf_counter_t _data_not_ready{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: data not ready")};
perf_counter_t _magnetic_sensor_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: magnetic sensor overflow")};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
int16_t _last_measurement[3] {};
uint8_t _checked_register{0};
unsigned _consecutive_failures{0};
enum class STATE : uint8_t {
RESET,
READ_WHO_AM_I,
WAIT_FOR_RESET,
CONFIGURE,
READ,
} _state{STATE::RESET};;
static constexpr uint8_t size_register_cfg{1};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ AKM_AK09916::Register::CNTL2, AKM_AK09916::CNTL2_BIT::MODE3, (uint8_t)~AKM_AK09916::CNTL2_BIT::MODE3 },
};
} _state{STATE::RESET};
};
} // namespace AKM_AK09916
src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp
-1
View File
@@ -105,7 +105,6 @@ enum class BANK_2 : uint8_t {
};
enum class BANK_3 : uint8_t {
I2C_MST_ODR_CONFIG = 0x00,
I2C_MST_CTRL = 0x01,
I2C_MST_DELAY_CTRL = 0x02,