dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

refactor adis16477: use driver base class

This commit is contained in:
Beat Küng 2020-03-17 18:03:24 +01:00 committed by Daniel Agar
parent d15fa82841
commit 1f152d7d43
7 changed files with 99 additions and 136 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -5,7 +5,7 @@
adc start
adis16477 -R 8 start
adis16477 -s -R 8 start
lps22hb -s start

2
boards/av/x-v1/src/board_config.h
View File

@ -125,7 +125,7 @@
#include <drivers/drv_sensor.h>
/* SPI1 */
#define PX4_SPIDEV_ADIS16477 PX4_MK_SPI_SEL(0,DRV_GYR_DEVTYPE_ADIS16477)
#define PX4_SPIDEV_ADIS16477 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ADIS16477)
#define PX4_SENSOR1_BUS_CS_GPIO {GPIO_SPI1_CS1_ADIS16477}
/* SPI2 */

2
boards/av/x-v1/src/spi.cpp
View File

@ -37,7 +37,7 @@
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_GYR_DEVTYPE_ADIS16477, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortJ, GPIO::Pin0}),
initSPIDevice(DRV_IMU_DEVTYPE_ADIS16477, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortJ, GPIO::Pin0}),
}),
initSPIBusExternal(SPI::Bus::SPI2, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin0}),

3
src/drivers/drv_sensor.h
View File

@ -114,9 +114,8 @@
#define DRV_GYR_DEVTYPE_FXAS2100C 0x54
#define DRV_IMU_DEVTYPE_ADIS16448 0x57
#define DRV_BARO_DEVTYPE_LPS22HB 0x58
#define DRV_ACC_DEVTYPE_ADIS16477 0x59
#define DRV_IMU_DEVTYPE_ADIS16477 0x59
#define DRV_GYR_DEVTYPE_ADIS16477 0x60
#define DRV_ACC_DEVTYPE_LSM303AGR 0x61
#define DRV_MAG_DEVTYPE_LSM303AGR 0x62
#define DRV_ACC_DEVTYPE_ADIS16497 0x63

67
src/drivers/imu/adis16477/ADIS16477.cpp
View File

@ -53,31 +53,30 @@ static constexpr uint32_t ADIS16477_DEFAULT_RATE = 1000;
using namespace time_literals;
ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) :
SPI("ADIS16477", nullptr, bus, device, SPIDEV_MODE3, 1000000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
ADIS16477::ADIS16477(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("ADIS16477", 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, "adis16477: read")),
_bad_transfers(perf_alloc(PC_COUNT, "adis16477: bad transfers"))
_bad_transfers(perf_alloc(PC_COUNT, "adis16477: bad transfers")),
_drdy_gpio(drdy_gpio)
{
#ifdef GPIO_SPI1_RESET_ADIS16477
// Configure hardware reset line
px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16477);
#endif // GPIO_SPI1_RESET_ADIS16477
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16477);
_px4_accel.set_device_type(DRV_IMU_DEVTYPE_ADIS16477);
_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16477);
_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ADIS16477);
_px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB
}
ADIS16477::~ADIS16477()
{
// make sure we are truly inactive
stop();
// delete the perf counters
perf_free(_sample_perf);
perf_free(_bad_transfers);
@ -86,11 +85,12 @@ ADIS16477::~ADIS16477()
int
ADIS16477::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();
@ -170,18 +170,18 @@ ADIS16477::probe()
uint16_t product_id = read_reg16(PROD_ID);
if (product_id == PROD_ID_ADIS16477) {
PX4_DEBUG("PRODUCT: %X", product_id);
DEVICE_DEBUG("PRODUCT: %X", product_id);
if (self_test_memory() && self_test_sensor()) {
return PX4_OK;
} 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();
}
}
@ -273,27 +273,25 @@ ADIS16477::write_reg16(uint8_t reg, uint16_t value)
void
ADIS16477::start()
{
#ifdef GPIO_SPI1_DRDY1_ADIS16477
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16477, true, false, true, &ADIS16477::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, &ADIS16477::data_ready_interrupt, this);
// start polling at the specified rate
ScheduleOnInterval((1_s / ADIS16477_DEFAULT_RATE), 10000);
#endif
} else {
// start polling at the specified rate
ScheduleOnInterval((1_s / ADIS16477_DEFAULT_RATE), 10000);
}
}
void
ADIS16477::stop()
ADIS16477::exit_and_cleanup()
{
#ifdef GPIO_SPI1_DRDY1_ADIS16477
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16477, 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
@ -308,7 +306,7 @@ ADIS16477::data_ready_interrupt(int irq, void *context, void *arg)
}
void
ADIS16477::Run()
ADIS16477::RunImpl()
{
// make another measurement
measure();
@ -384,8 +382,9 @@ ADIS16477::measure()
}
void
ADIS16477::print_info()
ADIS16477::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);

28
src/drivers/imu/adis16477/ADIS16477.hpp
View File

@ -45,20 +45,27 @@
#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>
class ADIS16477 : public device::SPI, public px4::ScheduledWorkItem
class ADIS16477 : public device::SPI, public I2CSPIDriver<ADIS16477>
{
public:
ADIS16477(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
ADIS16477(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 ~ADIS16477();
virtual int init();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void print_info();
int init();
void print_status() override;
void RunImpl();
protected:
virtual int probe();
int probe() override;
void exit_and_cleanup() override;
private:
@ -68,6 +75,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 in the ADIS16477, including command byte and interrupt status.
struct ADISReport {
@ -91,11 +100,6 @@ private:
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
@ -103,8 +107,6 @@ private:
*/
int reset();
void Run() override;
static int data_ready_interrupt(int irq, void *context, void *arg);
/**

131
src/drivers/imu/adis16477/adis16477_main.cpp
View File

@ -34,111 +34,74 @@
#include "ADIS16477.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
namespace adis16477
void
ADIS16477::print_usage()
{
ADIS16477 *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_ADIS16477)
g_dev = new ADIS16477(PX4_SPI_BUS_SENSOR1, PX4_SPIDEV_ADIS16477, rotation);
#elif defined(PX4_SPI_BUS_EXT)
g_dev = new ADIS16477(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, rotation);
#elif defined(PX4_SPIDEV_EXTERNAL1_1)
g_dev = new ADIS16477(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("adis16477", "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 *ADIS16477::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
return -1;
ADIS16477 *instance = new ADIS16477(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 adis16477
extern "C" int adis16477_main(int argc, char *argv[])
{
enum Rotation rotation = ROTATION_NONE;
int myoptind = 1;
int ch = 0;
const char *myoptarg = nullptr;
int ch;
using ThisDriver = ADIS16477;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = 1000000;
// 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 adis16477::usage();
}
}
const char *verb = argv[myoptind];
const char *verb = cli.optarg();
if (!strcmp(verb, "start")) {
return adis16477::start(rotation);
} else if (!strcmp(verb, "stop")) {
return adis16477::stop();
} else if (!strcmp(verb, "status")) {
return adis16477::status();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
return adis16477::usage();
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ADIS16477);
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;
}