fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete old imu/mpu6000 driver

Browse Source
This commit is contained in:
Daniel Agar 2021-02-23 14:46:08 -05:00
parent 4364e23633
commit 11ad41f7cb
23 changed files with 4 additions and 1966 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/holybro/pix32v5/default.cmake
View File

@ -34,7 +34,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
irlock
lights/blinkm
lights/rgbled

1
boards/nxp/fmurt1062-v1/default.cmake
View File

@ -31,7 +31,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
#irlock
#lights/blinkm
lights/rgbled

1
boards/px4/fmu-v2/default.cmake
View File

@ -43,7 +43,6 @@ px4_add_board(
#imu/invensense/icm20948
imu/invensense/mpu6000
#imu/invensense/mpu9250
#imu/mpu6000 # legacy mpu6000
#iridiumsbd
#irlock
#lights/blinkm

2
boards/px4/fmu-v2/multicopter.cmake
View File

@ -26,7 +26,7 @@ px4_add_board(
gps
imu/l3gd20
imu/lsm303d
imu/mpu6000
imu/invensense/mpu6000
#imu/invensense/mpu9250
irlock
lights/rgbled

1
boards/px4/fmu-v5/ctrlalloc.cmake
View File

@ -34,7 +34,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
irlock
lights/blinkm
lights/rgbled

1
boards/px4/fmu-v5/debug.cmake
View File

@ -35,7 +35,6 @@ px4_add_board(
#imu/bosch/bmi055
#imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
#irlock
#lights/blinkm
lights/rgbled

1
boards/px4/fmu-v5/default.cmake
View File

@ -34,7 +34,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
irlock
lights/blinkm
lights/rgbled

1
boards/px4/fmu-v5/fixedwing.cmake
View File

@ -30,7 +30,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
lights/rgbled
lights/rgbled_ncp5623c
lights/rgbled_pwm

1
boards/px4/fmu-v5/multicopter.cmake
View File

@ -31,7 +31,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
irlock
lights/blinkm
lights/rgbled

1
boards/px4/fmu-v5/optimized.cmake
View File

@ -33,7 +33,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
irlock
#lights/blinkm
lights/rgbled

1
boards/px4/fmu-v5/rover.cmake
View File

@ -28,7 +28,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
lights/rgbled
lights/rgbled_ncp5623c
lights/rgbled_pwm

1
boards/px4/fmu-v5/rtps.cmake
View File

@ -33,7 +33,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
irlock
lights/blinkm
lights/rgbled

1
boards/px4/fmu-v5/stackcheck.cmake
View File

@ -35,7 +35,6 @@ px4_add_board(
#imu/bosch/bmi055
#imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
#irlock
#lights/blinkm
lights/rgbled

1
boards/px4/fmu-v5/uavcanv0periph.cmake
View File

@ -36,7 +36,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
#irlock
#lights/blinkm
lights/rgbled

1
boards/px4/fmu-v5/uavcanv1.cmake
View File

@ -34,7 +34,6 @@ px4_add_board(
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
irlock
lights/blinkm
lights/rgbled

2
boards/spracing/h7extreme/default.cmake
View File

@ -28,7 +28,7 @@ px4_add_board(
#imu/adis16477
#imu/adis16497
#imu/bmi088
imu/mpu6000
imu/invensense/mpu6000
imu/invensense/icm20602
#imu/mpu9250
#irlock

4
cmake/px4_add_board.cmake
View File

@ -105,8 +105,8 @@
# DRIVERS
# barometer/ms5611
# gps
# imu/bmi055
# imu/mpu6000
# imu/bosch/bmi055
# imu/invensense/mpu6000
# magnetometer/isentek/ist8310
# pwm_out
# px4io

44
src/drivers/imu/mpu6000/CMakeLists.txt
View File

@ -1,44 +0,0 @@
############################################################################
#
# Copyright (c) 2015 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__mpu6000
MAIN mpu6000
SRCS
MPU6000.cpp
MPU6000_I2C.cpp
mpu6000_main.cpp
MPU6000_SPI.cpp
DEPENDS
drivers_accelerometer
drivers_gyroscope
)

903
src/drivers/imu/mpu6000/MPU6000.cpp
View File

@ -1,903 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 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 "MPU6000.hpp"
/*
list of registers that will be checked in check_registers(). Note
that MPUREG_PRODUCT_ID must be first in the list.
*/
constexpr uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
MPU6000::MPU6000(device::Device *interface, enum Rotation rotation, int device_type, I2CSPIBusOption bus_option,
int bus) :
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, 0, device_type),
_interface(interface),
_device_type(device_type),
_px4_accel(_interface->get_device_id(), rotation),
_px4_gyro(_interface->get_device_id(), rotation),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6k_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu6k_bad_trans")),
_bad_registers(perf_alloc(PC_COUNT, "mpu6k_bad_reg")),
_reset_retries(perf_alloc(PC_COUNT, "mpu6k_reset")),
_duplicates(perf_alloc(PC_COUNT, "mpu6k_duplicates"))
{
switch (_device_type) {
default:
case MPU_DEVICE_TYPE_MPU6000:
_interface->set_device_type(DRV_IMU_DEVTYPE_MPU6000);
_px4_accel.set_device_type(DRV_IMU_DEVTYPE_MPU6000);
_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_MPU6000);
break;
case MPU_DEVICE_TYPE_ICM20602:
_interface->set_device_type(DRV_IMU_DEVTYPE_ICM20602);
_px4_accel.set_device_type(DRV_IMU_DEVTYPE_ICM20602);
_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ICM20602);
break;
case MPU_DEVICE_TYPE_ICM20608:
_interface->set_device_type(DRV_IMU_DEVTYPE_ICM20608G);
_px4_accel.set_device_type(DRV_IMU_DEVTYPE_ICM20608G);
_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ICM20608G);
break;
case MPU_DEVICE_TYPE_ICM20689:
_interface->set_device_type(DRV_IMU_DEVTYPE_ICM20689);
_px4_accel.set_device_type(DRV_IMU_DEVTYPE_ICM20689);
_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ICM20689);
break;
}
}
MPU6000::~MPU6000()
{
perf_free(_sample_perf);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_reset_retries);
perf_free(_duplicates);
}
int
MPU6000::init()
{
/* probe again to get our settings that are based on the device type */
int ret = probe();
/* if probe failed, bail now */
if (ret != OK) {
PX4_DEBUG("probe init failed");
return ret;
}
if (reset() != OK) {
return ret;
}
return ret;
}
int MPU6000::reset()
{
// if the mpu6000 is initialized after the l3gd20 and lsm303d
// then if we don't do an irqsave/irqrestore here the mpu6000
// frequently comes up in a bad state where all transfers
// come as zero
uint8_t tries = 5;
irqstate_t state;
while (--tries != 0) {
state = px4_enter_critical_section();
// Hold off sampling for 60 ms
_reset_wait = hrt_absolute_time() + 60000;
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
up_udelay(10000);
// Wake up device and select GyroZ clock. Note that the
// MPU6000 starts up in sleep mode, and it can take some time
// for it to come out of sleep
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Enable I2C bus or Disable I2C bus (recommended on data sheet)
const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
write_checked_reg(MPUREG_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS);
px4_leave_critical_section(state);
if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) {
break;
}
perf_count(_reset_retries);
px4_usleep(2000);
}
// Hold off sampling for 30 ms
state = px4_enter_critical_section();
_reset_wait = hrt_absolute_time() + 30000;
px4_leave_critical_section(state);
if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) {
return -EIO;
}
px4_usleep(1000);
// SAMPLE RATE
_set_sample_rate(1000);
px4_usleep(1000);
_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
if (is_icm_device()) {
_set_icm_acc_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
}
px4_usleep(1000);
// Gyro scale 2000 deg/s ()
write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
px4_usleep(1000);
// correct gyro scale factors
// scale to rad/s in SI units
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
// scaling factor:
// 1/(2^15)*(2000/180)*PI
_px4_gyro.set_scale(0.0174532f / 16.4f);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
set_accel_range(MPU6000_ACCEL_DEFAULT_RANGE_G);
px4_usleep(1000);
// INT CFG => Interrupt on Data Ready
write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
px4_usleep(1000);
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
px4_usleep(1000);
if (is_icm_device()) {
write_checked_reg(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE);
}
// Oscillator set
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
px4_usleep(1000);
return OK;
}
int
MPU6000::probe()
{
uint8_t whoami = read_reg(MPUREG_WHOAMI);
uint8_t expected = 0;
bool unknown_product_id = true;
switch (_device_type) {
default:
case MPU_DEVICE_TYPE_MPU6000:
expected = MPU_WHOAMI_6000;
break;
case MPU_DEVICE_TYPE_ICM20602:
expected = ICM_WHOAMI_20602;
break;
case MPU_DEVICE_TYPE_ICM20608:
expected = ICM_WHOAMI_20608;
break;
case MPU_DEVICE_TYPE_ICM20689:
expected = ICM_WHOAMI_20689;
break;
}
if (whoami != expected) {
PX4_DEBUG("unexpected WHOAMI 0x%02x", whoami);
return -EIO;
}
/* look for a product ID we recognize */
_product = read_reg(MPUREG_PRODUCT_ID);
// verify product revision
switch (_product) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
case ICM20608_REV_FF:
case ICM20689_REV_FE:
case ICM20689_REV_03:
case ICM20689_REV_04:
case ICM20602_REV_01:
case ICM20602_REV_02:
case MPU6050_REV_D8:
unknown_product_id = false;
}
_checked_values[MPU6000_CHECKED_PRODUCT_ID_INDEX] = _product;
PX4_DEBUG("ID 0x%02x", _product);
if (unknown_product_id) {
PX4_WARN("unexpected ID 0x%02x %s", _product, is_icm_device() ? "accepted" : "exiting!");
if (is_mpu_device()) {
return -EIO;
}
}
return OK;
}
/*
set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
*/
void
MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz)
{
if (desired_sample_rate_hz == 0) {
desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE;
}
uint8_t div = 1000 / desired_sample_rate_hz;
if (div > 200) {
div = 200;
}
if (div < 1) {
div = 1;
}
write_checked_reg(MPUREG_SMPLRT_DIV, div - 1);
_sample_rate = 1000 / div;
}
/*
set the DLPF filter frequency. This affects both accel and gyro.
*/
void
MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
{
uint8_t filter = MPU_GYRO_DLPF_CFG_2100HZ_NOLPF;
/*
choose next highest filter frequency available
*/
if (frequency_hz == 0) {
filter = MPU_GYRO_DLPF_CFG_2100HZ_NOLPF;
} else if (frequency_hz <= 5) {
filter = MPU_GYRO_DLPF_CFG_5HZ;
} else if (frequency_hz <= 10) {
filter = MPU_GYRO_DLPF_CFG_10HZ;
} else if (frequency_hz <= 20) {
filter = MPU_GYRO_DLPF_CFG_20HZ;
} else if (frequency_hz <= 42) {
filter = MPU_GYRO_DLPF_CFG_42HZ;
} else if (frequency_hz <= 98) {
filter = MPU_GYRO_DLPF_CFG_98HZ;
} else if (frequency_hz <= 188) {
filter = MPU_GYRO_DLPF_CFG_188HZ;
} else if (frequency_hz <= 256) {
filter = MPU_GYRO_DLPF_CFG_256HZ_NOLPF2;
}
write_checked_reg(MPUREG_CONFIG, filter);
}
void
MPU6000::_set_icm_acc_dlpf_filter(uint16_t frequency_hz)
{
uint8_t filter = ICM_ACC_DLPF_CFG_1046HZ_NOLPF;
/*
choose next highest filter frequency available
*/
if (frequency_hz == 0) {
filter = ICM_ACC_DLPF_CFG_1046HZ_NOLPF;
} else if (frequency_hz <= 5) {
filter = ICM_ACC_DLPF_CFG_5HZ;
} else if (frequency_hz <= 10) {
filter = ICM_ACC_DLPF_CFG_10HZ;
} else if (frequency_hz <= 21) {
filter = ICM_ACC_DLPF_CFG_21HZ;
} else if (frequency_hz <= 44) {
filter = ICM_ACC_DLPF_CFG_44HZ;
} else if (frequency_hz <= 99) {
filter = ICM_ACC_DLPF_CFG_99HZ;
} else if (frequency_hz <= 218) {
filter = ICM_ACC_DLPF_CFG_218HZ;
} else if (frequency_hz <= 420) {
filter = ICM_ACC_DLPF_CFG_420HZ;
}
write_checked_reg(ICMREG_ACCEL_CONFIG2, filter);
}
#ifndef CONSTRAINED_FLASH
/*
perform a self-test comparison to factory trim values. This takes
about 200ms and will return OK if the current values are within 14%
of the expected values (as per datasheet)
*/
void
MPU6000::factory_self_test()
{
_in_factory_test = true;
uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG);
uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG);
const uint16_t repeats = 100;
int ret = OK;
// gyro self test has to be done at 250DPS
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
struct MPUReport mpu_report {};
float accel_baseline[3] {};
float gyro_baseline[3] {};
float accel[3] {};
float gyro[3] {};
float accel_ftrim[3] {};
float gyro_ftrim[3] {};
// get baseline values without self-test enabled
for (uint8_t i = 0; i < repeats; i++) {
up_udelay(1000);
_interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED), (uint8_t *)&mpu_report,
sizeof(mpu_report));
accel_baseline[0] += int16_t_from_bytes(mpu_report.accel_x);
accel_baseline[1] += int16_t_from_bytes(mpu_report.accel_y);
accel_baseline[2] += int16_t_from_bytes(mpu_report.accel_z);
gyro_baseline[0] += int16_t_from_bytes(mpu_report.gyro_x);
gyro_baseline[1] += int16_t_from_bytes(mpu_report.gyro_y);
gyro_baseline[2] += int16_t_from_bytes(mpu_report.gyro_z);
}
#if 1
write_reg(MPUREG_GYRO_CONFIG,
BITS_FS_250DPS |
BITS_GYRO_ST_X |
BITS_GYRO_ST_Y |
BITS_GYRO_ST_Z);
// accel 8g, self-test enabled all axes
write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config | 0xE0);
#endif
up_udelay(20000);
// get values with self-test enabled
for (uint8_t i = 0; i < repeats; i++) {
up_udelay(1000);
_interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED), (uint8_t *)&mpu_report,
sizeof(mpu_report));
accel[0] += int16_t_from_bytes(mpu_report.accel_x);
accel[1] += int16_t_from_bytes(mpu_report.accel_y);
accel[2] += int16_t_from_bytes(mpu_report.accel_z);
gyro[0] += int16_t_from_bytes(mpu_report.gyro_x);
gyro[1] += int16_t_from_bytes(mpu_report.gyro_y);
gyro[2] += int16_t_from_bytes(mpu_report.gyro_z);
}
for (uint8_t i = 0; i < 3; i++) {
accel_baseline[i] /= repeats;
gyro_baseline[i] /= repeats;
accel[i] /= repeats;
gyro[i] /= repeats;
}
// extract factory trim values
uint8_t trims[4];
trims[0] = read_reg(MPUREG_TRIM1);
trims[1] = read_reg(MPUREG_TRIM2);
trims[2] = read_reg(MPUREG_TRIM3);
trims[3] = read_reg(MPUREG_TRIM4);
uint8_t atrim[3];
uint8_t gtrim[3];
atrim[0] = ((trims[0] >> 3) & 0x1C) | ((trims[3] >> 4) & 0x03);
atrim[1] = ((trims[1] >> 3) & 0x1C) | ((trims[3] >> 2) & 0x03);
atrim[2] = ((trims[2] >> 3) & 0x1C) | ((trims[3] >> 0) & 0x03);
gtrim[0] = trims[0] & 0x1F;
gtrim[1] = trims[1] & 0x1F;
gtrim[2] = trims[2] & 0x1F;
// convert factory trims to right units
for (uint8_t i = 0; i < 3; i++) {
accel_ftrim[i] = 4096 * 0.34f * powf(0.92f / 0.34f, (atrim[i] - 1) / 30.0f);
gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i] - 1);
}
// Y gyro trim is negative
gyro_ftrim[1] *= -1;
for (uint8_t i = 0; i < 3; i++) {
float diff = accel[i] - accel_baseline[i];
float err = 100 * (diff - accel_ftrim[i]) / accel_ftrim[i];
::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n",
(unsigned)i,
(int)(1000 * accel_baseline[i]),
(int)(1000 * accel[i]),
(int)(1000 * diff),
(int)(1000 * accel_ftrim[i]),
(int)err);
if (fabsf(err) > 14) {
::printf("FAIL\n");
ret = -EIO;
}
}
for (uint8_t i = 0; i < 3; i++) {
float diff = gyro[i] - gyro_baseline[i];
float err = 100 * (diff - gyro_ftrim[i]) / gyro_ftrim[i];
::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n",
(unsigned)i,
(int)(1000 * gyro_baseline[i]),
(int)(1000 * gyro[i]),
(int)(1000 * (gyro[i] - gyro_baseline[i])),
(int)(1000 * gyro_ftrim[i]),
(int)err);
if (fabsf(err) > 14) {
::printf("FAIL\n");
ret = -EIO;
}
}
write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
_in_factory_test = false;
if (ret == OK) {
::printf("PASSED\n");
}
}
#endif
/*
deliberately trigger an error in the sensor to trigger recovery
*/
void
MPU6000::test_error()
{
_in_factory_test = true;
// deliberately trigger an error. This was noticed during
// development as a handy way to test the reset logic
uint8_t data[sizeof(MPUReport)] {};
_interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_LOW_BUS_SPEED), data, sizeof(data));
PX4_WARN("error triggered");
print_registers();
_in_factory_test = false;
}
uint8_t
MPU6000::read_reg(unsigned reg, uint32_t speed)
{
uint8_t buf{};
_interface->read(MPU6000_SET_SPEED(reg, speed), &buf, 1);
return buf;
}
uint16_t
MPU6000::read_reg16(unsigned reg)
{
uint8_t buf[2] {};
// general register transfer at low clock speed
_interface->read(MPU6000_LOW_SPEED_OP(reg), &buf, arraySize(buf));
return (uint16_t)(buf[0] << 8) | buf[1];
}
int
MPU6000::write_reg(unsigned reg, uint8_t value)
{
// general register transfer at low clock speed
return _interface->write(MPU6000_LOW_SPEED_OP(reg), &value, 1);
}
void
MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_reg(reg, val);
}
void
MPU6000::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i = 0; i < MPU6000_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
}
}
}
int
MPU6000::set_accel_range(unsigned max_g_in)
{
// workaround for bugged versions of MPU6k (rev C)
if (is_mpu_device()) {
switch (_product) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.0f);
return OK;
}
}
uint8_t afs_sel;
float lsb_per_g;
//float max_accel_g;
if (max_g_in > 8) { // 16g - AFS_SEL = 3
afs_sel = 3;
lsb_per_g = 2048;
//max_accel_g = 16;
} else if (max_g_in > 4) { // 8g - AFS_SEL = 2
afs_sel = 2;
lsb_per_g = 4096;
//max_accel_g = 8;
} else if (max_g_in > 2) { // 4g - AFS_SEL = 1
afs_sel = 1;
lsb_per_g = 8192;
//max_accel_g = 4;
} else { // 2g - AFS_SEL = 0
afs_sel = 0;
lsb_per_g = 16384;
//max_accel_g = 2;
}
write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3);
_px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
return OK;
}
void
MPU6000::start()
{
/* make sure we are stopped first */
uint32_t last_call_interval = _call_interval;
stop();
_call_interval = last_call_interval;
ScheduleOnInterval(_call_interval - MPU6000_TIMER_REDUCTION, 1000);
}
void
MPU6000::stop()
{
ScheduleClear();
/* reset internal states */
memset(_last_accel, 0, sizeof(_last_accel));
}
void
MPU6000::check_registers(void)
{
/*
we read the register at full speed, even though it isn't
listed as a high speed register. The low speed requirement
for some registers seems to be a propgation delay
requirement for changing sensor configuration, which should
not apply to reading a single register. It is also a better
test of SPI bus health to read at the same speed as we read
the data registers.
*/
uint8_t v;
// the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented)
if ((_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device())) {
_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
}
if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
_checked_values[_checked_next]) {
/*
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
/*
try to fix the bad register value. We only try to
fix one per loop to prevent a bad sensor hogging the
bus.
*/
if (_register_wait == 0 || _checked_next == MPU6000_CHECKED_PRODUCT_ID_INDEX) {
// if the product_id is wrong then reset the
// sensor completely
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
// after doing a reset we need to wait a long
// time before we do any other register writes
// or we will end up with the mpu6000 in a
// bizarre state where it has all correct
// register values but large offsets on the
// accel axes
_reset_wait = hrt_absolute_time() + 10000;
_checked_next = 0;
} else {
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
// waiting 3ms between register writes seems
// to raise the chance of the sensor
// recovering considerably
_reset_wait = hrt_absolute_time() + 3000;
}
_register_wait = 20;
}
_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
}
void MPU6000::RunImpl()
{
if (_in_factory_test) {
// don't publish any data while in factory test mode
return;
}
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct MPUReport mpu_report;
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the MPU6000 in one pass.
*/
// sensor transfer at high clock speed
const hrt_abstime timestamp_sample = hrt_absolute_time();
if (sizeof(mpu_report) != _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED),
(uint8_t *)&mpu_report, sizeof(mpu_report))) {
perf_end(_sample_perf);
return;
}
check_registers();
/*
see if this is duplicate accelerometer data. Note that we
can't use the data ready interrupt status bit in the status
register as that also goes high on new gyro data, and when
we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
sampled at 8kHz, so we would incorrectly think we have new
data when we are in fact getting duplicate accelerometer data.
*/
if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) {
// it isn't new data - wait for next timer
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
return;
}
perf_end(_sample_perf);
memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6);
_got_duplicate = false;
/*
* Convert from big to little endian
*/
report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the mpu6k does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again, don't return any data yet
_register_wait--;
return;
}
/*
* Swap axes and negate y
*/
int16_t accel_xt = report.accel_y;
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
int16_t gyro_xt = report.gyro_y;
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
/*
* Apply the swap
*/
report.accel_x = accel_xt;
report.accel_y = accel_yt;
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
_px4_accel.set_error_count(error_count);
_px4_gyro.set_error_count(error_count);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
float temperature = 0.0f;
if (is_icm_device()) { // if it is an ICM20608
temperature = (report.temp / 326.8f + 25.0f);
} else { // If it is an MPU6000
temperature = (report.temp / 340.0f + 35.0f);
}
_px4_accel.set_temperature(temperature);
_px4_gyro.set_temperature(temperature);
_px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
_px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
}
void
MPU6000::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("Device type: %i", _device_type);
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_reset_retries);
perf_print_counter(_duplicates);
}
void
MPU6000::print_registers()
{
PX4_INFO("registers");
for (uint8_t reg = MPUREG_PRODUCT_ID; reg <= 108; reg++) {
uint8_t v = read_reg(reg);
printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
if ((reg - (MPUREG_PRODUCT_ID - 1)) % 13 == 0) {
printf("\n");
}
}
printf("\n");
}

455
src/drivers/imu/mpu6000/MPU6000.hpp
View File

@ -1,455 +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 mpu6000.cpp
*
* Driver for the Invensense MPU6000, MPU6050, ICM20608, and ICM20602 connected via
* SPI or I2C.
*
* When the device is on the SPI bus the hrt is used to provide thread of
* execution to the driver.
*
* When the device is on the I2C bus a work queue is used provide thread of
* execution to the driver.
*
* The I2C code is only included in the build if USE_I2C is defined by the
* existance of any of PX4_I2C_MPU6050_ADDR, PX4_I2C_MPU6000_ADDR
* PX4_I2C_ICM_20608_G_ADDR in the board_config.h file.
*
* The command line option -T 6000|20608|20602 (default 6000) selects between
* MPU60x0, ICM20608G, or ICM20602G;
*
* @author Andrew Tridgell
* @author Pat Hickey
* @author David Sidrane
*/
#include <lib/conversion/rotation.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/device/spi.h>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/conversions.h>
#include <systemlib/px4_macros.h>
/*
we set the timer interrupt to run a bit faster than the desired
sample rate and then throw away duplicates by comparing
accelerometer values. This time reduction is enough to cope with
worst case timing jitter due to other timers
I2C bus is running at 100 kHz Transaction time is 2.163ms
I2C bus is running at 400 kHz (304 kHz actual) Transaction time
is 583 us
*/
#pragma once
#if defined(PX4_I2C_MPU6050_ADDR) || \
defined(PX4_I2C_MPU6000_ADDR) || \
defined(PX4_I2C_ICM_20608_G_ADDR)
# define USE_I2C
#endif
enum MPU_DEVICE_TYPE {
MPU_DEVICE_TYPE_MPU6000 = 6000,
MPU_DEVICE_TYPE_ICM20602 = 20602,
MPU_DEVICE_TYPE_ICM20608 = 20608,
MPU_DEVICE_TYPE_ICM20689 = 20689
};
#define DIR_READ 0x80
#define DIR_WRITE 0x00
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_FIFO_EN 0x23
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_INT_STATUS 0x3A
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
#define MPUREG_PRODUCT_ID 0x0C
#define MPUREG_TRIM1 0x0D
#define MPUREG_TRIM2 0x0E
#define MPUREG_TRIM3 0x0F
#define MPUREG_TRIM4 0x10
#define MPU_GYRO_DLPF_CFG_256HZ_NOLPF2 0x00 // delay: 0.98ms
#define MPU_GYRO_DLPF_CFG_188HZ 0x01 // delay: 1.9ms
#define MPU_GYRO_DLPF_CFG_98HZ 0x02 // delay: 2.8ms
#define MPU_GYRO_DLPF_CFG_42HZ 0x03 // delay: 4.8ms
#define MPU_GYRO_DLPF_CFG_20HZ 0x04 // delay: 8.3ms
#define MPU_GYRO_DLPF_CFG_10HZ 0x05 // delay: 13.4ms
#define MPU_GYRO_DLPF_CFG_5HZ 0x06 // delay: 18.6ms
#define MPU_GYRO_DLPF_CFG_2100HZ_NOLPF 0x07
#define MPU_DLPF_CFG_MASK 0x07
// Configuration bits MPU 3000 and MPU 6000 (not revised)?
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_GYRO_ST_X 0x80
#define BITS_GYRO_ST_Y 0x40
#define BITS_GYRO_ST_Z 0x20
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_MASK 0x18
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define BIT_INT_STATUS_DATA 0x01
#define MPU_WHOAMI_6000 0x68
#define ICM_WHOAMI_20602 0x12
#define ICM_WHOAMI_20608 0xaf
#define ICM_WHOAMI_20689 0x98
// ICM2608 specific registers
#define ICMREG_ACCEL_CONFIG2 0x1D
#define ICM_ACC_DLPF_CFG_1046HZ_NOLPF 0x00
#define ICM_ACC_DLPF_CFG_218HZ 0x01
#define ICM_ACC_DLPF_CFG_99HZ 0x02
#define ICM_ACC_DLPF_CFG_44HZ 0x03
#define ICM_ACC_DLPF_CFG_21HZ 0x04
#define ICM_ACC_DLPF_CFG_10HZ 0x05
#define ICM_ACC_DLPF_CFG_5HZ 0x06
#define ICM_ACC_DLPF_CFG_420HZ 0x07
/* this is an undocumented register which
if set incorrectly results in getting a 2.7m/s/s offset
on the Y axis of the accelerometer
*/
#define MPUREG_ICM_UNDOC1 0x11
#define MPUREG_ICM_UNDOC1_VALUE 0xc9
// Product ID Description for ICM20602
// Read From device
#define ICM20602_REV_01 1
#define ICM20602_REV_02 2
// Product ID Description for ICM20608
#define ICM20608_REV_FF 0xff // In the past, was thought to be not returning a value. But seem repeatable.
// Product ID Description for ICM20689
#define ICM20689_REV_FE 0xfe
#define ICM20689_REV_03 0x03
#define ICM20689_REV_04 0x04
// Product ID Description for MPU6000
// high 4 bits low 4 bits
// Product Name Product Revision
#define MPU6000ES_REV_C4 0x14
#define MPU6000ES_REV_C5 0x15
#define MPU6000ES_REV_D6 0x16
#define MPU6000ES_REV_D7 0x17
#define MPU6000ES_REV_D8 0x18
#define MPU6000_REV_C4 0x54
#define MPU6000_REV_C5 0x55
#define MPU6000_REV_D6 0x56
#define MPU6000_REV_D7 0x57
#define MPU6000_REV_D8 0x58
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
#define MPU6050_REV_D8 0x28 // TODO:Need verification
#define MPU6000_ACCEL_DEFAULT_RANGE_G 16
#define MPU6000_GYRO_DEFAULT_RANGE_G 8
#define MPU6000_GYRO_DEFAULT_RATE 1000
#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 98
#pragma pack(push, 1)
/**
* Report conversation within the MPU6000, including command byte and
* interrupt status.
*/
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
};
#pragma pack(pop)
#define MPU_MAX_READ_BUFFER_SIZE (sizeof(MPUReport) + 1)
#define MPU_MAX_WRITE_BUFFER_SIZE (2)
/*
The MPU6000 can only handle high bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
Communication with all registers of the device is performed using either
I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
the sensor and interrupt registers may be read using SPI at 20MHz
*/
#define MPU6000_LOW_BUS_SPEED 0
#define MPU6000_HIGH_BUS_SPEED 0x8000
# define MPU6000_IS_HIGH_SPEED(r) ((r) & MPU6000_HIGH_BUS_SPEED)
# define MPU6000_REG(r) ((r) &~MPU6000_HIGH_BUS_SPEED)
# define MPU6000_SET_SPEED(r, s) ((r)|(s))
# define MPU6000_HIGH_SPEED_OP(r) MPU6000_SET_SPEED((r), MPU6000_HIGH_BUS_SPEED)
# define MPU6000_LOW_SPEED_OP(r) MPU6000_REG((r))
/* interface factories */
extern device::Device *MPU6000_SPI_interface(int bus, uint32_t devid, int device_type, bool external_bus,
int bus_frequency, spi_mode_e spi_mode);
extern device::Device *MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus,
int bus_frequency);
extern int MPU6000_probe(device::Device *dev, int device_type);
#define MPU6000_TIMER_REDUCTION 200
class MPU6000 : public I2CSPIDriver<MPU6000>
{
public:
MPU6000(device::Device *interface, enum Rotation rotation, int device_type, I2CSPIBusOption bus_option, int bus);
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual ~MPU6000();
int init();
void print_registers();
#ifndef CONSTRAINED_FLASH
/**
* Test behaviour against factory offsets
*/
void factory_self_test();
#endif
// deliberately cause a sensor error
void test_error();
/**
* Start automatic measurement.
*/
void start();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
void RunImpl();
protected:
device::Device *_interface;
int probe();
void print_status() override;
void custom_method(const BusCLIArguments &cli) override;
private:
int _device_type;
uint8_t _product{0}; /** product code */
unsigned _call_interval{1000};
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
unsigned _sample_rate{1000};
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _reset_retries;
perf_counter_t _duplicates;
uint8_t _register_wait{0};
uint64_t _reset_wait{0};
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
static constexpr int MPU6000_CHECKED_PRODUCT_ID_INDEX = 0;
static constexpr int MPU6000_NUM_CHECKED_REGISTERS = 10;
static constexpr uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS] {
MPUREG_PRODUCT_ID,
MPUREG_PWR_MGMT_1,
MPUREG_USER_CTRL,
MPUREG_SMPLRT_DIV,
MPUREG_CONFIG,
MPUREG_GYRO_CONFIG,
MPUREG_ACCEL_CONFIG,
MPUREG_INT_ENABLE,
MPUREG_INT_PIN_CFG,
MPUREG_ICM_UNDOC1
};
uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS];
uint8_t _checked_next{0};
// use this to avoid processing measurements when in factory
// self test
volatile bool _in_factory_test{false};
// keep last accel reading for duplicate detection
uint16_t _last_accel[3] {};
bool _got_duplicate{false};
/**
* Stop automatic measurement.
*/
void stop();
/**
* is_icm_device
*/
bool is_icm_device() { return !is_mpu_device(); }
/**
* is_mpu_device
*/
bool is_mpu_device() { return _device_type == MPU_DEVICE_TYPE_MPU6000; }
/**
* Read a register from the MPU6000
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg, uint32_t speed = MPU6000_LOW_BUS_SPEED);
uint16_t read_reg16(unsigned reg);
/**
* Write a register in the MPU6000
*
* @param reg The register to write.
* @param value The new value to write.
*/
int write_reg(unsigned reg, uint8_t value);
/**
* Modify a register in the MPU6000
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the MPU6000, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the MPU6000 measurement range.
*
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int set_accel_range(unsigned max_g);
/*
set low pass filter frequency
*/
void _set_dlpf_filter(uint16_t frequency_hz);
void _set_icm_acc_dlpf_filter(uint16_t frequency_hz);
/*
set sample rate (approximate) - 1kHz to 5Hz
*/
void _set_sample_rate(unsigned desired_sample_rate_hz);
/*
check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
MPU6000(const MPU6000 &);
MPU6000 operator=(const MPU6000 &);
};

123
src/drivers/imu/mpu6000/MPU6000_I2C.cpp
View File

@ -1,123 +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 mpu6000_i2c.cpp
*
* I2C interface for MPU6000 /MPU6050
*/
#include <drivers/device/i2c.h>
#include "MPU6000.hpp"
#ifdef PX4_I2C_MPU6050_ADDR
device::Device *MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency);
class MPU6000_I2C : public device::I2C
{
public:
MPU6000_I2C(int bus, int device_type, int bus_frequency);
~MPU6000_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;
private:
int _device_type;
};
device::Device *
MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency)
{
return new MPU6000_I2C(bus, device_type, bus_frequency);
}
MPU6000_I2C::MPU6000_I2C(int bus, int device_type, int bus_frequency) :
I2C(DRV_IMU_DEVTYPE_MPU6000, MODULE_NAME, bus, PX4_I2C_MPU6050_ADDR, bus_frequency)
_device_type(device_type)
{
}
int
MPU6000_I2C::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
cmd[0] = MPU6000_REG(reg_speed);
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], count + 1, nullptr, 0);
}
int
MPU6000_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 = MPU6000_REG(reg_speed);
int ret = transfer(&cmd, 1, &((uint8_t *)data)[offset], count - offset);
return ret == OK ? count : ret;
}
int
MPU6000_I2C::probe()
{
uint8_t whoami = 0;
uint8_t expected = _device_type == MPU_DEVICE_TYPE_MPU6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
}
#else
device::Device *
MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency)
{
return nullptr;
}
#endif /* USE_I2C */

210
src/drivers/imu/mpu6000/MPU6000_SPI.cpp
View File

@ -1,210 +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 mpu6000_spi.cpp
*
* Driver for the Invensense MPU6000 connected via SPI.
*
* @author Andrew Tridgell
* @author Pat Hickey
* @author David sidrane
*/
#include <drivers/device/spi.h>
#include "MPU6000.hpp"
#define DIR_READ 0x80
#define DIR_WRITE 0x00
/* The MPU6000 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 ICM parts are not rated as high.
*
* The Actual Value will be rounded down by the spi driver.
* 168 Mhz CPU 180 Mhz CPU
* Selected ------------actual---------------
* 20 Mhz 10.5 Mhz 11.250 Mhz
* 10 Mhz 5.250 Mhz 5.625 Mhz
* 8 Mhz 5.250 Mhz 5.625 Mhz
* 1 Mhz 0.703125 Mhz 0.65625 Mhz
*
*/
#define MPU6000_LOW_SPI_BUS_SPEED 1000*1000
#define MPU6000_HIGH_SPI_BUS_SPEED 20*1000*1000
#define ICM20608_HIGH_SPI_BUS_SPEED 8*1000*1000
#define ICM20689_HIGH_SPI_BUS_SPEED 8*1000*1000
#define ICM20602_HIGH_SPI_BUS_SPEED 10*1000*1000
#define UNKNOWN_HIGH_SPI_BUS_SPEED 8*1000*1000 // Use the minimum
device::Device *MPU6000_SPI_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency,
spi_mode_e spi_mode);
class MPU6000_SPI : public device::SPI
{
public:
MPU6000_SPI(int bus, uint32_t device, int device_type, int bus_frequency, spi_mode_e spi_mode);
~MPU6000_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:
int _device_type;
/* Helper to set the desired speed and isolate the register on return */
int _max_frequency;
void set_bus_frequency(unsigned &reg_speed_reg_out);
};
device::Device *
MPU6000_SPI_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency,
spi_mode_e spi_mode)
{
return new MPU6000_SPI(bus, devid, device_type, bus_frequency, spi_mode);
}
MPU6000_SPI::MPU6000_SPI(int bus, uint32_t device, int device_type, int bus_frequency, spi_mode_e spi_mode) :
SPI(DRV_IMU_DEVTYPE_MPU6000, MODULE_NAME, bus, device, spi_mode, bus_frequency),
_device_type(device_type)
{
}
void
MPU6000_SPI::set_bus_frequency(unsigned &reg_speed)
{
/* Set the desired speed */
set_frequency(MPU6000_IS_HIGH_SPEED(reg_speed) ? _max_frequency : MPU6000_LOW_SPI_BUS_SPEED);
/* Isolate the register on return */
reg_speed = MPU6000_REG(reg_speed);
}
int
MPU6000_SPI::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
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
MPU6000_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] = {0, 0, 0};
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 == OK ? count : ret;
}
int
MPU6000_SPI::probe()
{
uint8_t whoami = 0;
uint8_t expected = MPU_WHOAMI_6000;
_max_frequency = UNKNOWN_HIGH_SPI_BUS_SPEED;
switch (_device_type) {
default:
case MPU_DEVICE_TYPE_MPU6000:
_max_frequency = MPU6000_HIGH_SPI_BUS_SPEED;
break;
case MPU_DEVICE_TYPE_ICM20602:
expected = ICM_WHOAMI_20602;
_max_frequency = ICM20602_HIGH_SPI_BUS_SPEED;
break;
case MPU_DEVICE_TYPE_ICM20608:
expected = ICM_WHOAMI_20608;
_max_frequency = ICM20608_HIGH_SPI_BUS_SPEED;
break;
case MPU_DEVICE_TYPE_ICM20689:
expected = ICM_WHOAMI_20689;
_max_frequency = ICM20689_HIGH_SPI_BUS_SPEED;
break;
}
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
}

213
src/drivers/imu/mpu6000/mpu6000_main.cpp
View File

@ -1,213 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 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 "MPU6000.hpp"
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
void
MPU6000::print_usage()
{
PRINT_MODULE_USAGE_NAME("mpu6000", "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_PARAM_STRING('T', "6000", "6000|20608|20602|20689", "Device type", true);
PRINT_MODULE_USAGE_COMMAND("reset");
PRINT_MODULE_USAGE_COMMAND("regdump");
#ifndef CONSTRAINED_FLASH
PRINT_MODULE_USAGE_COMMAND("factorytest");
#endif
PRINT_MODULE_USAGE_COMMAND("testerror");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
void
MPU6000::custom_method(const BusCLIArguments &cli)
{
switch (cli.custom1) {
case 0: reset();
break;
case 1: print_registers();
break;
#ifndef CONSTRAINED_FLASH
case 2: factory_self_test();
break;
#endif
case 3: test_error();
break;
}
}
I2CSPIDriverBase *MPU6000::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
device::Device *interface = nullptr;
int device_type = cli.type;
if (iterator.busType() == BOARD_I2C_BUS) {
interface = MPU6000_I2C_interface(iterator.bus(), iterator.devid(), device_type, iterator.external(),
cli.bus_frequency);
} else if (iterator.busType() == BOARD_SPI_BUS) {
interface = MPU6000_SPI_interface(iterator.bus(), iterator.devid(), device_type, iterator.external(),
cli.bus_frequency, cli.spi_mode);
}
if (interface == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
if (interface->init() != OK) {
delete interface;
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
return nullptr;
}
MPU6000 *dev = new MPU6000(interface, cli.rotation, device_type, iterator.configuredBusOption(), iterator.bus());
if (dev == nullptr) {
delete interface;
return nullptr;
}
if (OK != dev->init()) {
delete dev;
return nullptr;
}
dev->start();
return dev;
}
/** driver 'main' command */
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
int
mpu6000_main(int argc, char *argv[])
{
int ch;
using ThisDriver = MPU6000;
BusCLIArguments cli{true, true};
cli.type = MPU_DEVICE_TYPE_MPU6000;
cli.default_i2c_frequency = 400000;
cli.default_spi_frequency = 1000 * 1000; // low speed bus frequency
while ((ch = cli.getopt(argc, argv, "T:R:")) != EOF) {
switch (ch) {
case 'T':
cli.type = atoi(cli.optarg());
break;
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
}
}
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
uint16_t dev_type_driver = 0;
switch (cli.type) {
case MPU_DEVICE_TYPE_MPU6000:
dev_type_driver = DRV_IMU_DEVTYPE_MPU6000;
break;
case MPU_DEVICE_TYPE_ICM20602:
dev_type_driver = DRV_IMU_DEVTYPE_ICM20602;
break;
case MPU_DEVICE_TYPE_ICM20608:
dev_type_driver = DRV_IMU_DEVTYPE_ICM20608G;
break;
case MPU_DEVICE_TYPE_ICM20689:
dev_type_driver = DRV_IMU_DEVTYPE_ICM20689;
break;
}
BusInstanceIterator iterator(MODULE_NAME, cli, dev_type_driver);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
if (!strcmp(verb, "reset")) {
cli.custom1 = 0;
return ThisDriver::module_custom_method(cli, iterator);
}
if (!strcmp(verb, "regdump")) {
cli.custom1 = 1;
return ThisDriver::module_custom_method(cli, iterator);
}
#ifndef CONSTRAINED_FLASH
if (!strcmp(verb, "factorytest")) {
cli.custom1 = 2;
return ThisDriver::module_custom_method(cli, iterator);
}
#endif
if (!strcmp(verb, "testerror")) {
cli.custom1 = 3;
return ThisDriver::module_custom_method(cli, iterator);
}
ThisDriver::print_usage();
return -1;
}