forked from Archive/PX4-Autopilot
refactor bmi088: use driver base class
This commit is contained in:
parent
aad6fe6f03
commit
54da4997ad
|
@ -8,10 +8,10 @@ adc start
|
||||||
mpu6000 -R 8 -s -T 20689 start
|
mpu6000 -R 8 -s -T 20689 start
|
||||||
|
|
||||||
# Internal SPI bus BMI088 accel
|
# Internal SPI bus BMI088 accel
|
||||||
bmi088 -A -R 10 start
|
bmi088 -A -R 10 -s start
|
||||||
|
|
||||||
# Internal SPI bus BMI088 gyro
|
# Internal SPI bus BMI088 gyro
|
||||||
bmi088 -G -R 10 start
|
bmi088 -G -R 10 -s start
|
||||||
|
|
||||||
# Possible external compasses
|
# Possible external compasses
|
||||||
ist8310 -X start
|
ist8310 -X start
|
||||||
|
|
|
@ -16,10 +16,10 @@ mpu6000 -R 6 -s -T 20602 start
|
||||||
icm42688p -R 12 start
|
icm42688p -R 12 start
|
||||||
|
|
||||||
# Internal SPI bus BMI088 accel
|
# Internal SPI bus BMI088 accel
|
||||||
bmi088 -A -R 4 start
|
bmi088 -A -R 4 -s start
|
||||||
|
|
||||||
# Internal SPI bus BMI088 gyro
|
# Internal SPI bus BMI088 gyro
|
||||||
bmi088 -G -R 4 start
|
bmi088 -G -R 4 -s start
|
||||||
|
|
||||||
# Possible external compasses
|
# Possible external compasses
|
||||||
ist8310 -X start
|
ist8310 -X start
|
||||||
|
|
|
@ -12,10 +12,10 @@ mpu6000 -R 10 -s -T 20602 start
|
||||||
#icm20689 -R 10 20689 start
|
#icm20689 -R 10 20689 start
|
||||||
|
|
||||||
# Internal SPI bus BMI088 accel
|
# Internal SPI bus BMI088 accel
|
||||||
bmi088 -A -R 10 start
|
bmi088 -A -R 10 -s start
|
||||||
|
|
||||||
# Internal SPI bus BMI088 gyro
|
# Internal SPI bus BMI088 gyro
|
||||||
bmi088 -G -R 10 start
|
bmi088 -G -R 10 -s start
|
||||||
|
|
||||||
# Interal DPS310 (barometer)
|
# Interal DPS310 (barometer)
|
||||||
dps310 -s start
|
dps310 -s start
|
||||||
|
|
|
@ -16,10 +16,10 @@ mpu6000 -R 8 -s -T 20602 start
|
||||||
ism330dlc start
|
ism330dlc start
|
||||||
|
|
||||||
# Internal SPI bus BMI088 accel
|
# Internal SPI bus BMI088 accel
|
||||||
bmi088 -A -R 12 start
|
bmi088 -A -R 12 -s start
|
||||||
|
|
||||||
# Internal SPI bus BMI088 gyro
|
# Internal SPI bus BMI088 gyro
|
||||||
bmi088 -G -R 12 start
|
bmi088 -G -R 12 -s start
|
||||||
|
|
||||||
# Possible external compasses
|
# Possible external compasses
|
||||||
ist8310 -X start
|
ist8310 -X start
|
||||||
|
@ -36,5 +36,5 @@ bmp388 -I -a 0x77 start
|
||||||
# Baro on I2C3
|
# Baro on I2C3
|
||||||
ms5611 -X start
|
ms5611 -X start
|
||||||
|
|
||||||
# External RM3100 (I2C or SPI)
|
# External RM3100
|
||||||
rm3100 start
|
rm3100 -X start
|
||||||
|
|
|
@ -38,7 +38,7 @@
|
||||||
#include <lib/conversion/rotation.h>
|
#include <lib/conversion/rotation.h>
|
||||||
#include <lib/perf/perf_counter.h>
|
#include <lib/perf/perf_counter.h>
|
||||||
#include <px4_platform_common/getopt.h>
|
#include <px4_platform_common/getopt.h>
|
||||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
#include <px4_platform_common/i2c_spi_buses.h>
|
||||||
|
|
||||||
#define DIR_READ 0x80
|
#define DIR_READ 0x80
|
||||||
#define DIR_WRITE 0x00
|
#define DIR_WRITE 0x00
|
||||||
|
@ -50,12 +50,28 @@
|
||||||
|
|
||||||
#define BMI088_TIMER_REDUCTION 200
|
#define BMI088_TIMER_REDUCTION 200
|
||||||
|
|
||||||
class BMI088 : public device::SPI
|
class BMI088 : public device::SPI, public I2CSPIDriver<BMI088>
|
||||||
{
|
{
|
||||||
|
public:
|
||||||
|
BMI088(const char *name, const char *devname, I2CSPIBusOption bus_option, int bus, int type, uint32_t device,
|
||||||
|
enum spi_mode_e mode, uint32_t frequency, enum Rotation rotation);
|
||||||
|
virtual ~BMI088() = default;
|
||||||
|
|
||||||
|
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||||
|
int runtime_instance);
|
||||||
|
static void print_usage();
|
||||||
|
|
||||||
|
virtual void start() = 0;
|
||||||
|
|
||||||
|
virtual void RunImpl() = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
virtual void print_registers() = 0;
|
||||||
|
virtual void test_error() = 0;
|
||||||
|
|
||||||
uint8_t _whoami; /** whoami result */
|
void custom_method(const BusCLIArguments &cli) override;
|
||||||
|
|
||||||
|
uint8_t _whoami; ///< whoami result
|
||||||
|
|
||||||
uint8_t _register_wait;
|
uint8_t _register_wait;
|
||||||
uint64_t _reset_wait;
|
uint64_t _reset_wait;
|
||||||
|
@ -80,17 +96,4 @@ protected:
|
||||||
* @param value The new value to write.
|
* @param value The new value to write.
|
||||||
*/
|
*/
|
||||||
void write_reg(unsigned reg, uint8_t value);
|
void write_reg(unsigned reg, uint8_t value);
|
||||||
|
|
||||||
/* do not allow to copy this class due to pointer data members */
|
|
||||||
BMI088(const BMI088 &);
|
|
||||||
BMI088 operator=(const BMI088 &);
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
BMI088(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency,
|
|
||||||
enum Rotation rotation);
|
|
||||||
|
|
||||||
virtual ~BMI088() = default;
|
|
||||||
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -52,9 +52,10 @@ const uint8_t BMI088_accel::_checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTER
|
||||||
BMI088_ACC_PWR_CTRL,
|
BMI088_ACC_PWR_CTRL,
|
||||||
};
|
};
|
||||||
|
|
||||||
BMI088_accel::BMI088_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) :
|
BMI088_accel::BMI088_accel(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device,
|
||||||
BMI088("BMI088_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation),
|
enum Rotation rotation,
|
||||||
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
|
int bus_frequency, spi_mode_e spi_mode) :
|
||||||
|
BMI088("bmi088_accel", path_accel, bus_option, bus, DRV_ACC_DEVTYPE_BMI088, device, spi_mode, bus_frequency, rotation),
|
||||||
_px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
|
_px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "bmi088_accel_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "bmi088_accel_read")),
|
||||||
_bad_transfers(perf_alloc(PC_COUNT, "bmi088_accel_bad_transfers")),
|
_bad_transfers(perf_alloc(PC_COUNT, "bmi088_accel_bad_transfers")),
|
||||||
|
@ -67,9 +68,6 @@ BMI088_accel::BMI088_accel(int bus, const char *path_accel, uint32_t device, enu
|
||||||
|
|
||||||
BMI088_accel::~BMI088_accel()
|
BMI088_accel::~BMI088_accel()
|
||||||
{
|
{
|
||||||
/* make sure we are truly inactive */
|
|
||||||
stop();
|
|
||||||
|
|
||||||
/* delete the perf counter */
|
/* delete the perf counter */
|
||||||
perf_free(_sample_perf);
|
perf_free(_sample_perf);
|
||||||
perf_free(_bad_transfers);
|
perf_free(_bad_transfers);
|
||||||
|
@ -326,9 +324,6 @@ BMI088_accel::set_accel_range(unsigned max_g)
|
||||||
void
|
void
|
||||||
BMI088_accel::start()
|
BMI088_accel::start()
|
||||||
{
|
{
|
||||||
/* make sure we are stopped first */
|
|
||||||
stop();
|
|
||||||
|
|
||||||
// Reset the accelerometer
|
// Reset the accelerometer
|
||||||
reset();
|
reset();
|
||||||
|
|
||||||
|
@ -337,19 +332,6 @@ BMI088_accel::start()
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
|
||||||
BMI088_accel::stop()
|
|
||||||
{
|
|
||||||
ScheduleClear();
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
BMI088_accel::Run()
|
|
||||||
{
|
|
||||||
/* make another measurement */
|
|
||||||
measure();
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
void
|
||||||
BMI088_accel::check_registers(void)
|
BMI088_accel::check_registers(void)
|
||||||
{
|
{
|
||||||
|
@ -394,7 +376,7 @@ BMI088_accel::check_registers(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
BMI088_accel::measure()
|
BMI088_accel::RunImpl()
|
||||||
{
|
{
|
||||||
if (hrt_absolute_time() < _reset_wait) {
|
if (hrt_absolute_time() < _reset_wait) {
|
||||||
// we're waiting for a reset to complete
|
// we're waiting for a reset to complete
|
||||||
|
@ -530,8 +512,9 @@ BMI088_accel::measure()
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
BMI088_accel::print_info()
|
BMI088_accel::print_status()
|
||||||
{
|
{
|
||||||
|
I2CSPIDriverBase::print_status();
|
||||||
PX4_INFO("Accel");
|
PX4_INFO("Accel");
|
||||||
|
|
||||||
perf_print_counter(_sample_perf);
|
perf_print_counter(_sample_perf);
|
||||||
|
|
|
@ -154,36 +154,35 @@
|
||||||
|
|
||||||
#define BMI088_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50
|
#define BMI088_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50
|
||||||
|
|
||||||
class BMI088_accel : public BMI088, public px4::ScheduledWorkItem
|
class BMI088_accel : public BMI088
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
BMI088_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation);
|
BMI088_accel(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device, enum Rotation rotation,
|
||||||
|
int bus_frequency, spi_mode_e spi_mode);
|
||||||
virtual ~BMI088_accel();
|
virtual ~BMI088_accel();
|
||||||
|
|
||||||
virtual int init();
|
int init() override;
|
||||||
|
|
||||||
// Start automatic measurement.
|
// Start automatic measurement.
|
||||||
void start();
|
void start();
|
||||||
|
|
||||||
// We need to override the read_reg function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation
|
// We need to override the read_reg function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation
|
||||||
virtual uint8_t read_reg(unsigned reg);
|
uint8_t read_reg(unsigned reg) override;
|
||||||
|
|
||||||
// We need to override the read_reg16 function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation
|
// We need to override the read_reg16 function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation
|
||||||
virtual uint16_t read_reg16(unsigned reg);
|
uint16_t read_reg16(unsigned reg) override;
|
||||||
|
|
||||||
/**
|
void print_status() override;
|
||||||
* Diagnostics - print some basic information about the driver.
|
|
||||||
*/
|
|
||||||
void print_info();
|
|
||||||
|
|
||||||
void print_registers();
|
void print_registers();
|
||||||
|
|
||||||
// deliberately cause a sensor error
|
// deliberately cause a sensor error
|
||||||
void test_error();
|
void test_error();
|
||||||
|
|
||||||
|
void RunImpl() override;
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
virtual int probe();
|
int probe() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
@ -204,11 +203,6 @@ private:
|
||||||
|
|
||||||
bool _got_duplicate;
|
bool _got_duplicate;
|
||||||
|
|
||||||
/**
|
|
||||||
* Stop automatic measurement.
|
|
||||||
*/
|
|
||||||
void stop();
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Reset chip.
|
* Reset chip.
|
||||||
*
|
*
|
||||||
|
@ -216,13 +210,6 @@ private:
|
||||||
*/
|
*/
|
||||||
int reset();
|
int reset();
|
||||||
|
|
||||||
void Run() override;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Fetch measurements from the sensor and update the report buffers.
|
|
||||||
*/
|
|
||||||
void measure();
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Modify a register in the BMI088_accel
|
* Modify a register in the BMI088_accel
|
||||||
*
|
*
|
||||||
|
|
|
@ -55,9 +55,10 @@ const uint8_t BMI088_gyro::_checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS]
|
||||||
BMI088_GYR_INT_MAP_1
|
BMI088_GYR_INT_MAP_1
|
||||||
};
|
};
|
||||||
|
|
||||||
BMI088_gyro::BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) :
|
BMI088_gyro::BMI088_gyro(I2CSPIBusOption bus_option, int bus, const char *path_gyro, uint32_t device,
|
||||||
BMI088("BMI088_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation),
|
enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) :
|
||||||
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
|
BMI088("bmi088_gyro", path_gyro, bus_option, bus, DRV_GYR_DEVTYPE_BMI088, device, spi_mode, bus_frequency,
|
||||||
|
rotation),
|
||||||
_px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
|
_px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "bmi088_gyro_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, "bmi088_gyro_read")),
|
||||||
_bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")),
|
_bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")),
|
||||||
|
@ -68,9 +69,6 @@ BMI088_gyro::BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum R
|
||||||
|
|
||||||
BMI088_gyro::~BMI088_gyro()
|
BMI088_gyro::~BMI088_gyro()
|
||||||
{
|
{
|
||||||
/* make sure we are truly inactive */
|
|
||||||
stop();
|
|
||||||
|
|
||||||
/* delete the perf counter */
|
/* delete the perf counter */
|
||||||
perf_free(_sample_perf);
|
perf_free(_sample_perf);
|
||||||
perf_free(_bad_transfers);
|
perf_free(_bad_transfers);
|
||||||
|
@ -263,35 +261,10 @@ BMI088_gyro::set_gyro_range(unsigned max_dps)
|
||||||
void
|
void
|
||||||
BMI088_gyro::start()
|
BMI088_gyro::start()
|
||||||
{
|
{
|
||||||
/* make sure we are stopped first */
|
|
||||||
stop();
|
|
||||||
|
|
||||||
/* start polling at the specified rate */
|
/* start polling at the specified rate */
|
||||||
ScheduleOnInterval(BMI088_GYRO_DEFAULT_RATE - BMI088_TIMER_REDUCTION, 1000);
|
ScheduleOnInterval(BMI088_GYRO_DEFAULT_RATE - BMI088_TIMER_REDUCTION, 1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
|
||||||
BMI088_gyro::stop()
|
|
||||||
{
|
|
||||||
ScheduleClear();
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
BMI088_gyro::Run()
|
|
||||||
{
|
|
||||||
/* make another measurement */
|
|
||||||
measure();
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
BMI088_gyro::measure_trampoline(void *arg)
|
|
||||||
{
|
|
||||||
BMI088_gyro *dev = static_cast<BMI088_gyro *>(arg);
|
|
||||||
|
|
||||||
/* make another measurement */
|
|
||||||
dev->measure();
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
void
|
||||||
BMI088_gyro::check_registers(void)
|
BMI088_gyro::check_registers(void)
|
||||||
{
|
{
|
||||||
|
@ -336,7 +309,7 @@ BMI088_gyro::check_registers(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
BMI088_gyro::measure()
|
BMI088_gyro::RunImpl()
|
||||||
{
|
{
|
||||||
if (hrt_absolute_time() < _reset_wait) {
|
if (hrt_absolute_time() < _reset_wait) {
|
||||||
// we're waiting for a reset to complete
|
// we're waiting for a reset to complete
|
||||||
|
@ -424,8 +397,9 @@ BMI088_gyro::measure()
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
BMI088_gyro::print_info()
|
BMI088_gyro::print_status()
|
||||||
{
|
{
|
||||||
|
I2CSPIDriverBase::print_status();
|
||||||
PX4_INFO("Gyro");
|
PX4_INFO("Gyro");
|
||||||
|
|
||||||
perf_print_counter(_sample_perf);
|
perf_print_counter(_sample_perf);
|
||||||
|
|
|
@ -129,30 +129,29 @@
|
||||||
/* Mask definitions for Gyro bandwidth */
|
/* Mask definitions for Gyro bandwidth */
|
||||||
#define BMI088_GYRO_BW_MASK 0x0F
|
#define BMI088_GYRO_BW_MASK 0x0F
|
||||||
|
|
||||||
class BMI088_gyro : public BMI088, public px4::ScheduledWorkItem
|
class BMI088_gyro : public BMI088
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation);
|
BMI088_gyro(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device, enum Rotation rotation,
|
||||||
|
int bus_frequency, spi_mode_e spi_mode);
|
||||||
virtual ~BMI088_gyro();
|
virtual ~BMI088_gyro();
|
||||||
|
|
||||||
virtual int init();
|
int init() override;
|
||||||
|
|
||||||
// Start automatic measurement.
|
// Start automatic measurement.
|
||||||
void start();
|
void start() override;
|
||||||
|
|
||||||
/**
|
void print_status() override;
|
||||||
* Diagnostics - print some basic information about the driver.
|
|
||||||
*/
|
|
||||||
void print_info();
|
|
||||||
|
|
||||||
void print_registers();
|
void print_registers() override;
|
||||||
|
|
||||||
// deliberately cause a sensor error
|
// deliberately cause a sensor error
|
||||||
void test_error();
|
void test_error() override;
|
||||||
|
|
||||||
|
void RunImpl() override;
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
virtual int probe();
|
int probe() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
@ -173,11 +172,6 @@ private:
|
||||||
// last temperature reading for print_info()
|
// last temperature reading for print_info()
|
||||||
float _last_temperature;
|
float _last_temperature;
|
||||||
|
|
||||||
/**
|
|
||||||
* Stop automatic measurement.
|
|
||||||
*/
|
|
||||||
void stop();
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Reset chip.
|
* Reset chip.
|
||||||
*
|
*
|
||||||
|
@ -185,8 +179,6 @@ private:
|
||||||
*/
|
*/
|
||||||
int reset();
|
int reset();
|
||||||
|
|
||||||
void Run() override;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Static trampoline from the hrt_call context; because we don't have a
|
* Static trampoline from the hrt_call context; because we don't have a
|
||||||
* generic hrt wrapper yet.
|
* generic hrt wrapper yet.
|
||||||
|
@ -198,11 +190,6 @@ private:
|
||||||
*/
|
*/
|
||||||
static void measure_trampoline(void *arg);
|
static void measure_trampoline(void *arg);
|
||||||
|
|
||||||
/**
|
|
||||||
* Fetch measurements from the sensor and update the report buffers.
|
|
||||||
*/
|
|
||||||
void measure();
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Modify a register in the BMI088_gyro
|
* Modify a register in the BMI088_gyro
|
||||||
*
|
*
|
||||||
|
|
|
@ -33,276 +33,74 @@
|
||||||
|
|
||||||
#include "BMI088_accel.hpp"
|
#include "BMI088_accel.hpp"
|
||||||
#include "BMI088_gyro.hpp"
|
#include "BMI088_gyro.hpp"
|
||||||
|
#include <px4_platform_common/i2c_spi_buses.h>
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
|
||||||
/** driver 'main' command */
|
extern "C" __EXPORT int bmi088_main(int argc, char *argv[]);
|
||||||
extern "C" { __EXPORT int bmi088_main(int argc, char *argv[]); }
|
|
||||||
|
|
||||||
enum sensor_type {
|
|
||||||
BMI088_NONE = 0,
|
|
||||||
BMI088_ACCEL = 1,
|
|
||||||
BMI088_GYRO
|
|
||||||
};
|
|
||||||
|
|
||||||
namespace bmi088
|
|
||||||
{
|
|
||||||
|
|
||||||
BMI088_accel *g_acc_dev_int; // on internal bus (accel)
|
|
||||||
BMI088_accel *g_acc_dev_ext; // on external bus (accel)
|
|
||||||
BMI088_gyro *g_gyr_dev_int; // on internal bus (gyro)
|
|
||||||
BMI088_gyro *g_gyr_dev_ext; // on external bus (gyro)
|
|
||||||
|
|
||||||
void start(bool, enum Rotation, enum sensor_type);
|
|
||||||
void stop(bool, enum sensor_type);
|
|
||||||
void info(bool, enum sensor_type);
|
|
||||||
void regdump(bool, enum sensor_type);
|
|
||||||
void testerror(bool, enum sensor_type);
|
|
||||||
void usage();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Start the driver.
|
|
||||||
*
|
|
||||||
* This function only returns if the driver is up and running
|
|
||||||
* or failed to detect the sensor.
|
|
||||||
*/
|
|
||||||
void
|
void
|
||||||
start(bool external_bus, enum Rotation rotation, enum sensor_type sensor)
|
BMI088::print_usage()
|
||||||
{
|
{
|
||||||
|
PRINT_MODULE_USAGE_NAME("bmi088", "driver");
|
||||||
|
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
|
||||||
|
PRINT_MODULE_USAGE_COMMAND("start");
|
||||||
|
PRINT_MODULE_USAGE_PARAM_FLAG('A', "Accel", true);
|
||||||
|
PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true);
|
||||||
|
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
|
||||||
|
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||||
|
|
||||||
BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
|
PRINT_MODULE_USAGE_COMMAND("regdump");
|
||||||
const char *path_accel = external_bus ? BMI088_DEVICE_PATH_ACCEL_EXT : BMI088_DEVICE_PATH_ACCEL;
|
PRINT_MODULE_USAGE_COMMAND("testerror");
|
||||||
|
|
||||||
BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||||
const char *path_gyro = external_bus ? BMI088_DEVICE_PATH_GYRO_EXT : BMI088_DEVICE_PATH_GYRO;
|
}
|
||||||
|
|
||||||
if (sensor == BMI088_ACCEL) {
|
I2CSPIDriverBase *BMI088::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||||
if (*g_dev_acc_ptr != nullptr)
|
int runtime_instance)
|
||||||
/* if already started, the still command succeeded */
|
{
|
||||||
{
|
BMI088 *instance = nullptr;
|
||||||
errx(0, "bmi088 accel sensor already started");
|
|
||||||
|
if (cli.type == DRV_ACC_DEVTYPE_BMI088) {
|
||||||
|
instance = new BMI088_accel(iterator.configuredBusOption(), iterator.bus(), BMI088_DEVICE_PATH_ACCEL, iterator.devid(),
|
||||||
|
cli.rotation, cli.bus_frequency, cli.spi_mode);
|
||||||
|
|
||||||
|
} else if (cli.type == DRV_GYR_DEVTYPE_BMI088) {
|
||||||
|
instance = new BMI088_gyro(iterator.configuredBusOption(), iterator.bus(), BMI088_DEVICE_PATH_GYRO, iterator.devid(),
|
||||||
|
cli.rotation, cli.bus_frequency, cli.spi_mode);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* create the driver */
|
if (instance == nullptr) {
|
||||||
if (external_bus) {
|
return nullptr;
|
||||||
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
|
|
||||||
*g_dev_acc_ptr = new BMI088_accel(PX4_SPI_BUS_EXT, path_accel, PX4_SPIDEV_EXT_BMI, rotation);
|
|
||||||
#else
|
|
||||||
errx(0, "External SPI not available");
|
|
||||||
#endif
|
|
||||||
|
|
||||||
} else {
|
|
||||||
#if defined(PX4_SPI_BUS_SENSORS3) && defined(PX4_SPIDEV_BMI088_ACC)
|
|
||||||
*g_dev_acc_ptr = new BMI088_accel(PX4_SPI_BUS_SENSORS3, path_accel, PX4_SPIDEV_BMI088_ACC, rotation);
|
|
||||||
#elif defined(PX4_SPI_BUS_5) && defined(PX4_SPIDEV_BMI088_ACC)
|
|
||||||
*g_dev_acc_ptr = new BMI088_accel(PX4_SPI_BUS_5, path_accel, PX4_SPIDEV_BMI088_ACC, rotation);
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (*g_dev_acc_ptr == nullptr) {
|
if (OK != instance->init()) {
|
||||||
goto fail_accel;
|
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
|
||||||
|
delete instance;
|
||||||
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (OK != (*g_dev_acc_ptr)->init()) {
|
instance->start();
|
||||||
goto fail_accel;
|
|
||||||
}
|
|
||||||
|
|
||||||
// start automatic data collection
|
return instance;
|
||||||
(*g_dev_acc_ptr)->start();
|
|
||||||
}
|
|
||||||
|
|
||||||
if (sensor == BMI088_GYRO) {
|
|
||||||
|
|
||||||
if (*g_dev_gyr_ptr != nullptr) {
|
|
||||||
errx(0, "bmi088 gyro sensor already started");
|
|
||||||
}
|
|
||||||
|
|
||||||
/* create the driver */
|
|
||||||
if (external_bus) {
|
|
||||||
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
|
|
||||||
*g_dev_ptr = new BMI088_gyro(PX4_SPI_BUS_EXT, path_gyro, PX4_SPIDEV_EXT_BMI, rotation);
|
|
||||||
#else
|
|
||||||
errx(0, "External SPI not available");
|
|
||||||
#endif
|
|
||||||
|
|
||||||
} else {
|
|
||||||
#if defined(PX4_SPI_BUS_SENSORS3) && defined(PX4_SPIDEV_BMI088_GYR)
|
|
||||||
*g_dev_gyr_ptr = new BMI088_gyro(PX4_SPI_BUS_SENSORS3, path_gyro, PX4_SPIDEV_BMI088_GYR, rotation);
|
|
||||||
#elif defined(PX4_SPI_BUS_5) && defined(PX4_SPIDEV_BMI088_GYR)
|
|
||||||
*g_dev_gyr_ptr = new BMI088_gyro(PX4_SPI_BUS_5, path_gyro, PX4_SPIDEV_BMI088_GYR, rotation);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
if (*g_dev_gyr_ptr == nullptr) {
|
|
||||||
goto fail_gyro;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (OK != (*g_dev_gyr_ptr)->init()) {
|
|
||||||
goto fail_gyro;
|
|
||||||
}
|
|
||||||
|
|
||||||
// start automatic data collection
|
|
||||||
(*g_dev_gyr_ptr)->start();
|
|
||||||
}
|
|
||||||
|
|
||||||
exit(PX4_OK);
|
|
||||||
|
|
||||||
fail_accel:
|
|
||||||
|
|
||||||
if (*g_dev_acc_ptr != nullptr) {
|
|
||||||
delete (*g_dev_acc_ptr);
|
|
||||||
*g_dev_acc_ptr = nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
PX4_WARN("No BMI088 accel found");
|
|
||||||
exit(PX4_ERROR);
|
|
||||||
|
|
||||||
fail_gyro:
|
|
||||||
|
|
||||||
if (*g_dev_gyr_ptr != nullptr) {
|
|
||||||
delete (*g_dev_gyr_ptr);
|
|
||||||
*g_dev_gyr_ptr = nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
PX4_WARN("No BMI088 gyro found");
|
|
||||||
exit(PX4_ERROR);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
stop(bool external_bus, enum sensor_type sensor)
|
BMI088::custom_method(const BusCLIArguments &cli)
|
||||||
{
|
{
|
||||||
BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
|
switch (cli.custom1) {
|
||||||
BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
|
case 0: print_registers();
|
||||||
|
break;
|
||||||
|
|
||||||
if (sensor == BMI088_ACCEL) {
|
case 1: test_error();
|
||||||
if (*g_dev_acc_ptr != nullptr) {
|
break;
|
||||||
delete *g_dev_acc_ptr;
|
|
||||||
*g_dev_acc_ptr = nullptr;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
/* warn, but not an error */
|
|
||||||
warnx("bmi088 accel sensor already stopped.");
|
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
if (sensor == BMI088_GYRO) {
|
|
||||||
if (*g_dev_gyr_ptr != nullptr) {
|
|
||||||
delete *g_dev_gyr_ptr;
|
|
||||||
*g_dev_gyr_ptr = nullptr;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
/* warn, but not an error */
|
|
||||||
warnx("bmi088 gyro sensor already stopped.");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
exit(0);
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
BMI088::BMI088(const char *name, const char *devname, I2CSPIBusOption bus_option, int bus, int type, uint32_t device,
|
||||||
* Print a little info about the driver.
|
enum spi_mode_e mode,
|
||||||
*/
|
|
||||||
void
|
|
||||||
info(bool external_bus, enum sensor_type sensor)
|
|
||||||
{
|
|
||||||
BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
|
|
||||||
BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
|
|
||||||
|
|
||||||
if (sensor == BMI088_ACCEL) {
|
|
||||||
if (*g_dev_acc_ptr == nullptr) {
|
|
||||||
errx(1, "bmi088 accel driver not running");
|
|
||||||
}
|
|
||||||
|
|
||||||
printf("state @ %p\n", *g_dev_acc_ptr);
|
|
||||||
(*g_dev_acc_ptr)->print_info();
|
|
||||||
}
|
|
||||||
|
|
||||||
if (sensor == BMI088_GYRO) {
|
|
||||||
if (*g_dev_gyr_ptr == nullptr) {
|
|
||||||
errx(1, "bmi088 gyro driver not running");
|
|
||||||
}
|
|
||||||
|
|
||||||
printf("state @ %p\n", *g_dev_gyr_ptr);
|
|
||||||
(*g_dev_gyr_ptr)->print_info();
|
|
||||||
}
|
|
||||||
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Dump the register information
|
|
||||||
*/
|
|
||||||
void
|
|
||||||
regdump(bool external_bus, enum sensor_type sensor)
|
|
||||||
{
|
|
||||||
BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
|
|
||||||
BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
|
|
||||||
|
|
||||||
if (sensor == BMI088_ACCEL) {
|
|
||||||
if (*g_dev_acc_ptr == nullptr) {
|
|
||||||
errx(1, "bmi088 accel driver not running");
|
|
||||||
}
|
|
||||||
|
|
||||||
printf("regdump @ %p\n", *g_dev_acc_ptr);
|
|
||||||
(*g_dev_acc_ptr)->print_registers();
|
|
||||||
}
|
|
||||||
|
|
||||||
if (sensor == BMI088_GYRO) {
|
|
||||||
if (*g_dev_gyr_ptr == nullptr) {
|
|
||||||
errx(1, "bmi088 gyro driver not running");
|
|
||||||
}
|
|
||||||
|
|
||||||
printf("regdump @ %p\n", *g_dev_gyr_ptr);
|
|
||||||
(*g_dev_gyr_ptr)->print_registers();
|
|
||||||
}
|
|
||||||
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* deliberately produce an error to test recovery
|
|
||||||
*/
|
|
||||||
void
|
|
||||||
testerror(bool external_bus, enum sensor_type sensor)
|
|
||||||
{
|
|
||||||
BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
|
|
||||||
BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
|
|
||||||
|
|
||||||
if (sensor == BMI088_ACCEL) {
|
|
||||||
if (*g_dev_acc_ptr == nullptr) {
|
|
||||||
errx(1, "bmi088 accel driver not running");
|
|
||||||
}
|
|
||||||
|
|
||||||
(*g_dev_acc_ptr)->test_error();
|
|
||||||
}
|
|
||||||
|
|
||||||
if (sensor == BMI088_GYRO) {
|
|
||||||
if (*g_dev_gyr_ptr == nullptr) {
|
|
||||||
errx(1, "bmi088 gyro driver not running");
|
|
||||||
}
|
|
||||||
|
|
||||||
(*g_dev_gyr_ptr)->test_error();
|
|
||||||
}
|
|
||||||
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
usage()
|
|
||||||
{
|
|
||||||
warnx("missing command: try 'start', 'info', 'stop', 'regdump', 'testerror'");
|
|
||||||
warnx("options:");
|
|
||||||
warnx(" -X (external bus)");
|
|
||||||
warnx(" -R rotation");
|
|
||||||
warnx(" -A (Enable Accelerometer)");
|
|
||||||
warnx(" -G (Enable Gyroscope)");
|
|
||||||
}
|
|
||||||
|
|
||||||
}//namespace ends
|
|
||||||
|
|
||||||
|
|
||||||
BMI088::BMI088(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode,
|
|
||||||
uint32_t frequency, enum Rotation rotation):
|
uint32_t frequency, enum Rotation rotation):
|
||||||
SPI(name, devname, bus, device, mode, frequency),
|
SPI(name, devname, bus, device, mode, frequency),
|
||||||
|
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, type),
|
||||||
_whoami(0),
|
_whoami(0),
|
||||||
_register_wait(0),
|
_register_wait(0),
|
||||||
_reset_wait(0),
|
_reset_wait(0),
|
||||||
|
@ -345,77 +143,59 @@ BMI088::write_reg(unsigned reg, uint8_t value)
|
||||||
int
|
int
|
||||||
bmi088_main(int argc, char *argv[])
|
bmi088_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
bool external_bus = false;
|
using ThisDriver = BMI088;
|
||||||
int ch;
|
int ch;
|
||||||
enum Rotation rotation = ROTATION_NONE;
|
BusCLIArguments cli{false, true};
|
||||||
enum sensor_type sensor = BMI088_NONE;
|
cli.type = 0;
|
||||||
int myoptind = 1;
|
cli.default_spi_frequency = BMI088_BUS_SPEED;
|
||||||
const char *myoptarg = NULL;
|
|
||||||
|
|
||||||
/* jump over start/off/etc and look at options first */
|
while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) {
|
||||||
while ((ch = px4_getopt(argc, argv, "XR:AG", &myoptind, &myoptarg)) != EOF) {
|
|
||||||
switch (ch) {
|
switch (ch) {
|
||||||
case 'X':
|
|
||||||
external_bus = true;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 'R':
|
|
||||||
rotation = (enum Rotation)atoi(myoptarg);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 'A':
|
case 'A':
|
||||||
sensor = BMI088_ACCEL;
|
cli.type = DRV_ACC_DEVTYPE_BMI088;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 'G':
|
case 'G':
|
||||||
sensor = BMI088_GYRO;
|
cli.type = DRV_GYR_DEVTYPE_BMI088;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
case 'R':
|
||||||
bmi088::usage();
|
cli.rotation = (enum Rotation)atoi(cli.optarg());
|
||||||
exit(0);
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
const char *verb = argv[myoptind];
|
const char *verb = cli.optarg();
|
||||||
|
|
||||||
if (sensor == BMI088_NONE) {
|
if (!verb) {
|
||||||
bmi088::usage();
|
ThisDriver::print_usage();
|
||||||
exit(0);
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
BusInstanceIterator iterator(MODULE_NAME, cli, cli.type);
|
||||||
* Start/load the driver.
|
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "start")) {
|
if (!strcmp(verb, "start")) {
|
||||||
bmi088::start(external_bus, rotation, sensor);
|
return ThisDriver::module_start(cli, iterator);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
* Stop the driver.
|
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "stop")) {
|
if (!strcmp(verb, "stop")) {
|
||||||
bmi088::stop(external_bus, sensor);
|
return ThisDriver::module_stop(iterator);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
if (!strcmp(verb, "status")) {
|
||||||
* Print driver information.
|
return ThisDriver::module_status(iterator);
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "info")) {
|
|
||||||
bmi088::info(external_bus, sensor);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
* Print register information.
|
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "regdump")) {
|
if (!strcmp(verb, "regdump")) {
|
||||||
bmi088::regdump(external_bus, sensor);
|
cli.custom1 = 0;
|
||||||
|
return ThisDriver::module_custom_method(cli, iterator);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!strcmp(verb, "testerror")) {
|
if (!strcmp(verb, "testerror")) {
|
||||||
bmi088::testerror(external_bus, sensor);
|
cli.custom1 = 1;
|
||||||
|
return ThisDriver::module_custom_method(cli, iterator);
|
||||||
}
|
}
|
||||||
|
|
||||||
bmi088::usage();
|
ThisDriver::print_usage();
|
||||||
exit(1);
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue