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 ism330dlc: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-03-18 17:31:20 +01:00
committed by Daniel Agar
parent 2d6a9ae1fb
commit 3423809cd9
4 changed files with 116 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
boards/px4/fmu-v5x/init/rc.board_sensors
View File

@@ -13,7 +13,7 @@ ina226 -X -b 2 -t 2 -k start
mpu6000 -R 8 -s -T 20602 start
# Internal SPI bus ISM300DLC
ism330dlc start
ism330dlc -s start
# Internal SPI bus BMI088 accel
bmi088 -A -R 12 -s start

86
src/drivers/imu/st/ism330dlc/ISM330DLC.cpp
View File

@@ -39,9 +39,11 @@ static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u)
static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval
ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
ISM330DLC::ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) :
SPI(MODULE_NAME, nullptr, bus, device, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_drdy_gpio(drdy_gpio),
_px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation)
{
@@ -55,8 +57,6 @@ ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
ISM330DLC::~ISM330DLC()
{
Stop();
perf_free(_interval_perf);
perf_free(_transfer_perf);
perf_free(_fifo_empty_perf);
@@ -66,33 +66,47 @@ ISM330DLC::~ISM330DLC()
perf_free(_drdy_interval_perf);
}
void ISM330DLC::exit_and_cleanup()
{
if (_drdy_gpio != 0) {
// Disable data ready callback
px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr);
RegisterWrite(Register::INT1_CTRL, 0);
}
I2CSPIDriverBase::exit_and_cleanup();
}
int ISM330DLC::probe()
{
const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
if (whoami != ISM330DLC_WHO_AM_I) {
PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami);
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
return PX4_ERROR;
}
return PX4_OK;
}
bool ISM330DLC::Init()
int ISM330DLC::init()
{
if (SPI::init() != PX4_OK) {
PX4_ERR("SPI::init failed");
return false;
int ret = SPI::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("SPI::init failed (%i)", ret);
return ret;
}
if (!Reset()) {
PX4_ERR("reset failed");
return false;
return PX4_ERROR;
}
Start();
return true;
return PX4_OK;
}
bool ISM330DLC::Reset()
@@ -193,41 +207,28 @@ void ISM330DLC::DataReady()
void ISM330DLC::Start()
{
Stop();
ResetFIFO();
#if defined(GPIO_SPI2_DRDY1_ISM330) && false // TODO: enable
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_SPI2_DRDY1_ISM330, true, true, false, &ISM330DLC::DataReadyInterruptCallback, this);
if (_drdy_gpio != 0 && false) { // TODO: enable
// Setup data ready on rising edge
px4_arch_gpiosetevent(_drdy_gpio, true, true, false, &ISM330DLC::DataReadyInterruptCallback, this);
// FIFO threshold level setting
// FIFO_CTRL1: FTH_[7:0]
// FIFO_CTRL2: FTH_[10:8]
const uint8_t fifo_threshold = 12;
RegisterWrite(Register::FIFO_CTRL1, fifo_threshold);
// FIFO threshold level setting
// FIFO_CTRL1: FTH_[7:0]
// FIFO_CTRL2: FTH_[10:8]
const uint8_t fifo_threshold = 12;
RegisterWrite(Register::FIFO_CTRL1, fifo_threshold);
// INT1: FIFO full, overrun, or threshold
RegisterWrite(Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FULL_FLAG | INT1_CTRL_BIT::INT1_FIFO_OVR |
INT1_CTRL_BIT::INT1_FTH);
#else
ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
#endif
// INT1: FIFO full, overrun, or threshold
RegisterWrite(Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FULL_FLAG | INT1_CTRL_BIT::INT1_FIFO_OVR |
INT1_CTRL_BIT::INT1_FTH);
} else {
ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
}
}
void ISM330DLC::Stop()
{
#if defined(GPIO_SPI2_DRDY1_ISM330) && false // TODO: enable
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_SPI2_DRDY1_ISM330, false, false, false, nullptr, nullptr);
RegisterWrite(Register::INT1_CTRL, 0);
#else
ScheduleClear();
#endif
}
void ISM330DLC::Run()
void ISM330DLC::RunImpl()
{
perf_count(_interval_perf);
@@ -333,8 +334,9 @@ void ISM330DLC::Run()
_px4_accel.updateFIFO(accel);
}
void ISM330DLC::PrintInfo()
void ISM330DLC::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_interval_perf);
perf_print_counter(_transfer_perf);
perf_print_counter(_fifo_empty_perf);

31
src/drivers/imu/st/ism330dlc/ISM330DLC.hpp
View File

@@ -48,22 +48,33 @@
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
using namespace ST_ISM330DLC;
class ISM330DLC : public device::SPI, public px4::ScheduledWorkItem
class ISM330DLC : public device::SPI, public I2CSPIDriver<ISM330DLC>
{
public:
ISM330DLC(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
~ISM330DLC() override;
bool Init();
void Start();
void Stop();
bool Reset();
void PrintInfo();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void print_status() override;
void RunImpl();
int init() override;
void Start();
bool Reset();
protected:
void custom_method(const BusCLIArguments &cli) override;
void exit_and_cleanup() override;
private:
// Sensor Configuration
@@ -84,8 +95,6 @@ private:
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
void Run() override;
uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);
void RegisterSetBits(Register reg, uint8_t setbits);
@@ -93,6 +102,8 @@ private:
void ResetFIFO();
const spi_drdy_gpio_t _drdy_gpio;
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;

141
src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp
View File

@@ -34,125 +34,84 @@
#include "ISM330DLC.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
namespace ism330dlc
void
ISM330DLC::print_usage()
{
ISM330DLC *g_dev{nullptr};
int start(enum Rotation rotation);
int stop();
int status();
void usage();
int start(enum Rotation rotation)
{
if (g_dev != nullptr) {
PX4_WARN("already started");
return 0;
}
// create the driver
g_dev = new ISM330DLC(PX4_SPI_BUS_SENSORS2, PX4_SPIDEV_ISM330, rotation); // v5x TODO: board manifest
if (g_dev == nullptr) {
PX4_ERR("driver start failed");
return -1;
}
if (!g_dev->Init()) {
PX4_ERR("driver init failed");
delete g_dev;
g_dev = nullptr;
return -1;
}
return 0;
PRINT_MODULE_USAGE_NAME("ism330dlc", "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_COMMAND("reset");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
int stop()
I2CSPIDriverBase *ISM330DLC::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
ISM330DLC *instance = new ISM330DLC(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
g_dev->Stop();
delete g_dev;
return 0;
}
int reset()
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
if (OK != instance->init()) {
delete instance;
return nullptr;
}
return g_dev->Reset();
return instance;
}
int status()
void ISM330DLC::custom_method(const BusCLIArguments &cli)
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
return -1;
}
g_dev->PrintInfo();
return 0;
Reset();
}
void usage()
extern "C" int ism330dlc_main(int argc, char *argv[])
{
PX4_INFO("missing command: try 'start', 'stop', 'reset', 'status'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
}
int ch;
using ThisDriver = ISM330DLC;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = ST_ISM330DLC::SPI_SPEED;
} // namespace ism330dlc
extern "C" __EXPORT int ism330dlc_main(int argc, char *argv[])
{
enum Rotation rotation = ROTATION_NONE;
int myoptind = 1;
int ch = 0;
const char *myoptarg = nullptr;
/* 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:
ism330dlc::usage();
return 0;
}
}
if (myoptind >= argc) {
ism330dlc::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
const char *verb = argv[myoptind];
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ST_ISM330DLC);
if (!strcmp(verb, "start")) {
return ism330dlc::start(rotation);
} else if (!strcmp(verb, "stop")) {
return ism330dlc::stop();
} else if (!strcmp(verb, "status")) {
return ism330dlc::status();
} else if (!strcmp(verb, "reset")) {
return ism330dlc::reset();
return ThisDriver::module_start(cli, iterator);
}
ism330dlc::usage();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
return 0;
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
if (!strcmp(verb, "reset")) {
return ThisDriver::module_custom_method(cli, iterator);
}
ThisDriver::print_usage();
return -1;
}