yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

refactor adis16497: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-03-17 18:04:57 +01:00
committed by Daniel Agar
parent 1f152d7d43
commit 17761acd49
5 changed files with 97 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
boards/av/x-v1/init/rc.board_sensors
View File

@@ -16,8 +16,7 @@ ms4525_airspeed -T 4515 -I -b 3 start
if ! param greater SENS_EN_PMW3901 0
then
# try to start adis16497 only if pmw3901 isn't enabled, or parameter doesn't exists
# TODO: this one is external SPI
adis16497 start
adis16497 -S start
fi
# Possible external compasses

3
src/drivers/drv_sensor.h
View File

@@ -118,8 +118,7 @@
#define DRV_ACC_DEVTYPE_LSM303AGR 0x61
#define DRV_MAG_DEVTYPE_LSM303AGR 0x62
#define DRV_ACC_DEVTYPE_ADIS16497 0x63
#define DRV_GYR_DEVTYPE_ADIS16497 0x64
#define DRV_IMU_DEVTYPE_ADIS16497 0x63
#define DRV_BARO_DEVTYPE_BAROSIM 0x65
#define DRV_GYR_DEVTYPE_BMI088 0x66
#define DRV_BARO_DEVTYPE_BMP388 0x67

68
src/drivers/imu/adis16497/ADIS16497.cpp
View File

@@ -70,29 +70,27 @@ static constexpr uint32_t ADIS16497_DEFAULT_RATE = 1000;
using namespace time_literals;
ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) :
SPI("ADIS16497", nullptr, bus, device, SPIDEV_MODE3, 5000000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
ADIS16497::ADIS16497(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) :
SPI("ADIS16497", nullptr, bus, device, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_px4_accel(get_device_id(), ORB_PRIO_MAX, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation),
_sample_perf(perf_alloc(PC_ELAPSED, "adis16497: read")),
_bad_transfers(perf_alloc(PC_COUNT, "adis16497: bad transfers"))
_bad_transfers(perf_alloc(PC_COUNT, "adis16497: bad transfers")),
_drdy_gpio(drdy_gpio)
{
#ifdef GPIO_SPI1_RESET_ADIS16497
// Configure hardware reset line
px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16497);
#endif // GPIO_SPI1_RESET_ADIS16497
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16497);
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16497);
_px4_accel.set_device_type(DRV_IMU_DEVTYPE_ADIS16497);
_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ADIS16497);
}
ADIS16497::~ADIS16497()
{
// make sure we are truly inactive
stop();
// delete the perf counters
perf_free(_sample_perf);
perf_free(_bad_transfers);
@@ -101,11 +99,12 @@ ADIS16497::~ADIS16497()
int
ADIS16497::init()
{
// do SPI init (and probe) first
if (SPI::init() != OK) {
int ret = SPI::init();
if (ret != OK) {
// if probe/setup failed, bail now
PX4_DEBUG("SPI setup failed");
return PX4_ERROR;
DEVICE_DEBUG("SPI init failed (%i)", ret);
return ret;
}
start();
@@ -257,17 +256,17 @@ ADIS16497::probe()
return PX4_OK;
} else {
PX4_ERR("probe attempt %d: reading model id failed, resetting", i);
DEVICE_DEBUG("probe attempt %d: reading model id failed, resetting", i);
reset();
}
} else {
PX4_ERR("probe attempt %d: self test failed, resetting", i);
DEVICE_DEBUG("probe attempt %d: self test failed, resetting", i);
reset();
}
} else {
PX4_ERR("probe attempt %d: read product id failed, resetting", i);
DEVICE_DEBUG("probe attempt %d: read product id failed, resetting", i);
reset();
}
}
@@ -380,27 +379,25 @@ ADIS16497::write_reg16(uint8_t reg, uint16_t value)
void
ADIS16497::start()
{
#ifdef GPIO_SPI1_DRDY1_ADIS16497
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16497, true, false, true, &ADIS16497::data_ready_interrupt, this);
#else
// Make sure we are stopped first
stop();
if (_drdy_gpio != 0) {
// Setup data ready on rising edge
px4_arch_gpiosetevent(_drdy_gpio, true, false, true, &ADIS16497::data_ready_interrupt, this);
// start polling at the specified rate
ScheduleOnInterval((1_s / ADIS16497_DEFAULT_RATE), 10000);
#endif
} else {
// start polling at the specified rate
ScheduleOnInterval((1_s / ADIS16497_DEFAULT_RATE), 10000);
}
}
void
ADIS16497::stop()
ADIS16497::exit_and_cleanup()
{
#ifdef GPIO_SPI1_DRDY1_ADIS16497
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16497, false, false, false, nullptr, nullptr);
#else
ScheduleClear();
#endif
if (_drdy_gpio != 0) {
// Disable data ready callback
px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr);
}
I2CSPIDriverBase::exit_and_cleanup();
}
int
@@ -415,7 +412,7 @@ ADIS16497::data_ready_interrupt(int irq, void *context, void *arg)
}
void
ADIS16497::Run()
ADIS16497::RunImpl()
{
// make another measurement
measure();
@@ -505,8 +502,9 @@ ADIS16497::measure()
}
void
ADIS16497::print_info()
ADIS16497::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);

28
src/drivers/imu/adis16497/ADIS16497.hpp
View File

@@ -45,7 +45,7 @@
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <perf/perf_counter.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
// TODO : This is a copy of the NuttX CRC32 table
static constexpr uint32_t crc32_tab[] = {
@@ -83,18 +83,25 @@ static constexpr uint32_t crc32_tab[] = {
0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};
class ADIS16497 : public device::SPI, public px4::ScheduledWorkItem
class ADIS16497 : public device::SPI, public I2CSPIDriver<ADIS16497>
{
public:
ADIS16497(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
ADIS16497(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
virtual ~ADIS16497();
virtual int init();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void print_info();
int init() override;
void print_status() override;
void RunImpl();
protected:
virtual int probe();
int probe() override;
void exit_and_cleanup() override;
private:
@@ -104,6 +111,8 @@ private:
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
const spi_drdy_gpio_t _drdy_gpio;
#pragma pack(push, 1)
// Report conversation with the ADIS16497, including command byte.
struct ADISReport {
@@ -135,11 +144,6 @@ private:
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
@@ -147,8 +151,6 @@ private:
*/
int reset();
void Run() override;
static int data_ready_interrupt(int irq, void *context, void *arg);
/**

127
src/drivers/imu/adis16497/adis16497_main.cpp
View File

@@ -34,107 +34,74 @@
#include "ADIS16497.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
namespace adis16497
void
ADIS16497::print_usage()
{
ADIS16497 *g_dev{nullptr};
static int start(enum Rotation rotation)
{
if (g_dev != nullptr) {
PX4_WARN("already started");
return 0;
}
// create the driver
#if defined(PX4_SPIDEV_EXTERNAL1_1)
g_dev = new ADIS16497(PX4_SPI_BUS_EXTERNAL1, PX4_SPIDEV_EXTERNAL1_1, rotation);
#else
PX4_ERR("External SPI not available");
return -1;
#endif
if (g_dev == nullptr) {
PX4_ERR("driver start failed");
return -1;
}
if (g_dev->init() != PX4_OK) {
PX4_ERR("driver init failed");
delete g_dev;
g_dev = nullptr;
return -1;
}
return 0;
PRINT_MODULE_USAGE_NAME("adis16497", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
static int stop()
I2CSPIDriverBase *ADIS16497::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
return -1;
ADIS16497 *instance = new ADIS16497(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
static int status()
{
if (g_dev == nullptr) {
PX4_INFO("driver not running");
return 0;
if (OK != instance->init()) {
delete instance;
return nullptr;
}
g_dev->print_info();
return 0;
return instance;
}
static int usage()
{
PX4_INFO("missing command: try 'start', 'stop', 'status'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
return 0;
}
} // namespace adis16497
extern "C" int adis16497_main(int argc, char *argv[])
{
enum Rotation rotation = ROTATION_NONE;
int myoptind = 1;
int ch = 0;
const char *myoptarg = nullptr;
int ch;
using ThisDriver = ADIS16497;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = 5000000;
// start options
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
default:
return adis16497::usage();
}
}
const char *verb = argv[myoptind];
const char *verb = cli.optarg();
if (!strcmp(verb, "start")) {
return adis16497::start(rotation);
} else if (!strcmp(verb, "stop")) {
return adis16497::stop();
} else if (!strcmp(verb, "status")) {
return adis16497::status();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
return adis16497::usage();
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ADIS16497);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}