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

refactor mpu6000: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-02-26 14:55:57 +01:00
committed by Daniel Agar
parent be0a205438
commit 969a77f889
14 changed files with 180 additions and 491 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/holybro/durandal-v1/init/rc.board_sensors
+1 -1
View File
@@ -5,7 +5,7 @@
adc start
# Internal SPI bus ICM-20689
mpu6000 -R 8 -z -T 20689 start
mpu6000 -R 8 -s -T 20689 start
# Internal SPI bus BMI088 accel
bmi088 -A -R 10 start
boards/holybro/kakutef7/init/rc.board_sensors
+1 -1
View File
@@ -8,7 +8,7 @@ adc start
# The default IMU is an ICM20689, but there might also be an MPU6000
if ! mpu6000 -R 12 -s start
then
mpu6000 -R 12 -z -T 20689 start
mpu6000 -R 12 -s -T 20689 start
fi
# Internal Baro
boards/mro/x21-777/init/rc.board_sensors
+2 -2
View File
@@ -10,10 +10,10 @@ hmc5883 -C -T -X start
lis3mdl -X start
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
mpu6000 -R 2 -T 20608 start
mpu6000 -s -R 2 -T 20608 start
# Internal SPI bus ICM-20602-G is rotated 90 deg yaw
mpu6000 -R 2 -T 20602 start
mpu6000 -s -R 2 -T 20602 start
# Internal SPI bus mpu9250 is rotated 90 deg yaw
mpu9250 -R 2 start
boards/mro/x21/init/rc.board_sensors
+2 -2
View File
@@ -12,10 +12,10 @@ ist8310 -X start
qmc5883 -X start
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
mpu6000 -R 2 -T 20608 start
mpu6000 -s -R 2 -T 20608 start
# Internal SPI bus ICM-20602-G is rotated 90 deg yaw
mpu6000 -R 2 -T 20602 start
mpu6000 -s -R 2 -T 20602 start
# Internal SPI bus mpu9250 is rotated 90 deg yaw
mpu9250 -R 2 start
boards/nxp/fmurt1062-v1/init/rc.board_sensors
+1 -1
View File
@@ -21,7 +21,7 @@ adc start
mpu6000 -R 8 -s -T 20602 start
# Internal SPI bus ICM-20689
mpu6000 -R 8 -z -T 20689 start
mpu6000 -R 8 -s -T 20689 start
# Internal SPI bus BMI055 accel
bmi055 -A -R 10 start
boards/px4/fmu-v2/init/rc.board_sensors
+7 -7
View File
@@ -14,7 +14,7 @@ ist8310 -X start
hmc5883 -C -T -I -R 4 start
# Internal SPI bus ICM-20608-G
mpu6000 -T 20608 start
mpu6000 -s -T 20608 start
# External SPI
ms5611 -S start
@@ -28,8 +28,8 @@ set BOARD_FMUV3 0
if ver hwtypecmp V30
then
# Check for Pixhawk 2.0 cube
# external MPU6K is rotated 180 degrees yaw
if mpu6000 -S -R 4 start
# MPU6K is rotated 180 degrees yaw
if mpu6000 -s -R 4 start
then
set BOARD_FMUV3 20
else
@@ -48,8 +48,8 @@ then
# Pixhawk Mini doesn't have these sensors,
# so if they are found we know its a Pixhack
# external MPU6K is rotated 180 degrees yaw
if mpu6000 -S -R 4 start
# MPU6K is rotated 180 degrees yaw
if mpu6000 -s -R 4 start
then
set BOARD_FMUV3 20
else
@@ -79,7 +79,7 @@ then
if [ $BOARD_FMUV3 = 20 ]
then
# v2.0 internal MPU6000 is rotated 180 deg roll, 270 deg yaw
mpu6000 -R 14 start
mpu6000 -s -R 14 start
# v2.0 Has internal hmc5883 on SPI1
hmc5883 -C -T -S -R 8 start
@@ -94,7 +94,7 @@ then
else
# $BOARD_FMUV3 = 0 -> FMUv2
mpu6000 start
mpu6000 -s start
# As we will use the external mag and the ICM-20608-G
# V2 build hwtypecmp is always false
boards/px4/fmu-v3/init/rc.board_sensors
+7 -7
View File
@@ -15,7 +15,7 @@ qmc5883 -X start
hmc5883 -C -T -I -R 4 start
# Internal SPI bus ICM-20608-G
mpu6000 -T 20608 start
mpu6000 -s -T 20608 start
# External SPI
ms5611 -S start
@@ -29,8 +29,8 @@ set BOARD_FMUV3 0
if ver hwtypecmp V30
then
# Check for Pixhawk 2.0 cube
# external MPU6K is rotated 180 degrees yaw
if mpu6000 -S -R 4 start
# MPU6K is rotated 180 degrees yaw
if mpu6000 -s -R 4 start
then
set BOARD_FMUV3 20
else
@@ -49,8 +49,8 @@ then
# Pixhawk Mini doesn't have these sensors,
# so if they are found we know its a Pixhack
# external MPU6K is rotated 180 degrees yaw
if mpu6000 -S -R 4 start
# MPU6K is rotated 180 degrees yaw
if mpu6000 -s -R 4 start
then
set BOARD_FMUV3 20
else
@@ -87,7 +87,7 @@ then
if [ $BOARD_FMUV3 = 20 ]
then
# v2.0 internal MPU6000 is rotated 180 deg roll, 270 deg yaw
mpu6000 -R 14 start
mpu6000 -s -R 14 start
# v2.0 Has internal hmc5883 on SPI1
hmc5883 -C -T -S -R 8 start
@@ -102,7 +102,7 @@ then
else
# $BOARD_FMUV3 = 0 -> FMUv2
mpu6000 start
mpu6000 -s start
# As we will use the external mag and the ICM-20608-G
# V2 build hwtypecmp is always false
boards/px4/fmu-v5/init/rc.board_sensors
+1 -1
View File
@@ -10,7 +10,7 @@ mpu6000 -R 8 -s -T 20602 start
#icm20602 -R 6 start
# Internal SPI bus ICM-20689
mpu6000 -R 8 -z -T 20689 start
mpu6000 -R 8 -s -T 20689 start
#icm20689 -R 6 start
# new sensor drivers (in testing)
boards/uvify/core/init/rc.board_sensors
+1 -1
View File
@@ -14,7 +14,7 @@ then
# GPS LED
rgbled_ncp5623c start -a 0x38
#mpu6000 -R 4 -T 20608 start
#mpu6000 -s -R 4 -T 20608 start
mpu9250 -R 4 start
# Default GNSS with LIS3MDL magnetometer with external i2c.
src/drivers/imu/mpu6000/MPU6000.cpp
+8 -10
View File
@@ -39,8 +39,9 @@
*/
constexpr uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
MPU6000::MPU6000(device::Device *interface, enum Rotation rotation, int device_type) :
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
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(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation),
@@ -77,10 +78,6 @@ MPU6000::MPU6000(device::Device *interface, enum Rotation rotation, int device_t
MPU6000::~MPU6000()
{
/* make sure we are truly inactive */
stop();
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_bad_transfers);
perf_free(_bad_registers);
@@ -379,7 +376,7 @@ MPU6000::_set_icm_acc_dlpf_filter(uint16_t frequency_hz)
about 200ms and will return OK if the current values are within 14%
of the expected values (as per datasheet)
*/
int
void
MPU6000::factory_self_test()
{
_in_factory_test = true;
@@ -515,7 +512,6 @@ MPU6000::factory_self_test()
::printf("PASSED\n");
}
return ret;
}
#endif
@@ -710,7 +706,7 @@ MPU6000::check_registers(void)
_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
}
void MPU6000::Run()
void MPU6000::RunImpl()
{
if (_in_factory_test) {
// don't publish any data while in factory test mode
@@ -873,8 +869,10 @@ void MPU6000::Run()
}
void
MPU6000::print_info()
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);
src/drivers/imu/mpu6000/MPU6000.hpp
+20 -27
View File
@@ -63,6 +63,7 @@
#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>
@@ -263,48 +264,36 @@ struct MPUReport {
# define MPU6000_LOW_SPEED_OP(r) MPU6000_REG((r))
/* interface factories */
extern device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus);
extern device::Device *MPU6000_I2C_interface(int bus, int device_type, bool external_bus);
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);
typedef device::Device *(*MPU6000_constructor)(int, int, bool);
#define MPU6000_TIMER_REDUCTION 200
enum MPU6000_BUS {
MPU6000_BUS_ALL = 0,
MPU6000_BUS_I2C_INTERNAL,
MPU6000_BUS_I2C_EXTERNAL,
MPU6000_BUS_SPI_INTERNAL1,
MPU6000_BUS_SPI_INTERNAL2,
MPU6000_BUS_SPI_EXTERNAL1,
MPU6000_BUS_SPI_EXTERNAL2
};
class MPU6000 : public px4::ScheduledWorkItem
class MPU6000 : public I2CSPIDriver<MPU6000>
{
public:
MPU6000(device::Device *interface, enum Rotation rotation, int device_type);
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();
virtual int init();
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
int init();
void print_registers();
#ifndef CONSTRAINED_FLASH
/**
* Test behaviour against factory offsets
*
* @return 0 on success, 1 on failure
*/
int factory_self_test();
void factory_self_test();
#endif
// deliberately cause a sensor error
@@ -322,15 +311,19 @@ public:
*/
int reset();
void RunImpl();
protected:
device::Device *_interface;
virtual int probe();
int probe();
void print_status() override;
void custom_method(const BusCLIArguments &cli) override;
private:
void Run() override;
int _device_type;
uint8_t _product{0}; /** product code */
src/drivers/imu/mpu6000/MPU6000_I2C.cpp
+15 -7
View File
@@ -41,14 +41,14 @@
#include "MPU6000.hpp"
#ifdef USE_I2C
#ifdef PX4_I2C_MPU6050_ADDR
device::Device *MPU6000_I2C_interface(int bus, int device_type, bool external_bus);
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);
MPU6000_I2C(int bus, int device_type, int bus_frequency);
~MPU6000_I2C() override = default;
int read(unsigned address, void *data, unsigned count) override;
@@ -64,13 +64,13 @@ private:
device::Device *
MPU6000_I2C_interface(int bus, int device_type, bool external_bus)
MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency)
{
return new MPU6000_I2C(bus, device_type);
return new MPU6000_I2C(bus, device_type, bus_frequency);
}
MPU6000_I2C::MPU6000_I2C(int bus, int device_type) :
I2C("MPU6000_I2C", nullptr, bus, PX4_I2C_MPU6050_ADDR, 400000),
MPU6000_I2C::MPU6000_I2C(int bus, int device_type, int bus_frequency) :
I2C("MPU6000_I2C", nullptr, bus, PX4_I2C_MPU6050_ADDR, bus_frequency),
_device_type(device_type)
{
}
@@ -112,4 +112,12 @@ MPU6000_I2C::probe()
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 */
src/drivers/imu/mpu6000/MPU6000_SPI.cpp
+8 -85
View File
@@ -48,7 +48,6 @@
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#if defined(PX4_SPIDEV_MPU) || defined(PX4_SPIDEV_ICM_20602) || defined(PX4_SPIDEV_ICM_20689)
/* 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
@@ -73,13 +72,14 @@
#define UNKNOWN_HIGH_SPI_BUS_SPEED 8*1000*1000 // Use the minimum
device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus);
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);
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;
@@ -98,90 +98,14 @@ private:
};
device::Device *
MPU6000_SPI_interface(int bus, int device_type, bool external_bus)
MPU6000_SPI_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency,
spi_mode_e spi_mode)
{
int cs = SPIDEV_NONE(0);
device::Device *interface = nullptr;
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) || defined(PX4_SPI_BUS_EXTERNAL)
switch (device_type) {
case MPU_DEVICE_TYPE_MPU6000:
# if defined(PX4_SPIDEV_EXT_MPU)
cs = PX4_SPIDEV_EXT_MPU;
# endif
break;
case MPU_DEVICE_TYPE_ICM20602:
# if defined(PX4_SPIDEV_ICM_20602_EXT)
cs = PX4_SPIDEV_ICM_20602_EXT;
# endif
break;
case MPU_DEVICE_TYPE_ICM20608:
# if defined(PX4_SPIDEV_EXT_ICM)
cs = PX4_SPIDEV_EXT_ICM;
# elif defined(PX4_SPIDEV_ICM_20608_EXT)
cs = PX4_SPIDEV_ICM_20608_EXT;
# endif
break;
case MPU_DEVICE_TYPE_ICM20689:
# if defined(PX4_SPIDEV_ICM_20689_EXT)
cs = PX4_SPIDEV_ICM_20689_EXT;
# endif
break;
default:
break;
}
#endif
} else {
switch (device_type) {
case MPU_DEVICE_TYPE_MPU6000:
#if defined(PX4_SPIDEV_MPU)
cs = PX4_SPIDEV_MPU;
#endif
break;
case MPU_DEVICE_TYPE_ICM20602:
#if defined(PX4_SPIDEV_ICM_20602)
cs = PX4_SPIDEV_ICM_20602;
#endif
break;
case MPU_DEVICE_TYPE_ICM20608:
#if defined(PX4_SPIDEV_ICM_20608)
cs = PX4_SPIDEV_ICM_20608;
#endif
break;
case MPU_DEVICE_TYPE_ICM20689:
# if defined(PX4_SPIDEV_ICM_20689)
cs = PX4_SPIDEV_ICM_20689;
# endif
default:
break;
}
}
if (cs != SPIDEV_NONE(0)) {
interface = new MPU6000_SPI(bus, cs, device_type);
}
return interface;
return new MPU6000_SPI(bus, devid, device_type, bus_frequency, spi_mode);
}
MPU6000_SPI::MPU6000_SPI(int bus, uint32_t device, int device_type) :
SPI("MPU6000", nullptr, bus, device, SPIDEV_MODE3, MPU6000_LOW_SPI_BUS_SPEED),
MPU6000_SPI::MPU6000_SPI(int bus, uint32_t device, int device_type, int bus_frequency, spi_mode_e spi_mode) :
SPI("MPU6000", nullptr, bus, device, spi_mode, bus_frequency),
_device_type(device_type)
{
}
@@ -285,4 +209,3 @@ MPU6000_SPI::probe()
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
}
#endif // PX4_SPIDEV_MPU
src/drivers/imu/mpu6000/mpu6000_main.cpp
+106 -339
View File
File diff suppressed because it is too large Load Diff