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

refactor lsm9ds1+lsm9ds1_mag: use driver base class

Browse Source
This commit is contained in:
Beat Küng
2020-02-26 14:35:55 +01:00
committed by Daniel Agar
parent 58f386a81c
commit 3e71914fae
9 changed files with 126 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
posix-configs/rpi/px4.config
View File

@@ -20,8 +20,8 @@ param set BAT_CNT_V_CURR 0.001
dataman start
mpu9250 -R 8 start
lsm9ds1 -R 4 start
lsm9ds1_mag -R 4 start
lsm9ds1 -s -R 4 start
lsm9ds1_mag -s -R 4 start
ms5611 start
navio_rgbled start

4
posix-configs/rpi/px4_fw.config
View File

@@ -20,8 +20,8 @@ param set BAT_CNT_V_CURR 0.001
dataman start
mpu9250 -R 8 start
lsm9ds1 -R 4 start
lsm9ds1_mag -R 4 start
lsm9ds1 -s -R 4 start
lsm9ds1_mag -s -R 4 start
ms5611 start
navio_rgbled start

4
posix-configs/rpi/px4_test.config
View File

@@ -20,8 +20,8 @@ param set BAT_CNT_V_CURR 0.001
dataman start
mpu9250 -R 8 start
lsm9ds1 -R 4 start
lsm9ds1_mag -R 4 start
lsm9ds1 -s -R 4 start
lsm9ds1_mag -s -R 4 start
ms5611 start
navio_rgbled start

15
src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
View File

@@ -38,9 +38,10 @@ using namespace ST_LSM9DS1;
static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
LSM9DS1::LSM9DS1(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())),
LSM9DS1::LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode) :
SPI(MODULE_NAME, 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_DEFAULT, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation)
{
@@ -54,8 +55,6 @@ LSM9DS1::LSM9DS1(int bus, uint32_t device, enum Rotation rotation) :
LSM9DS1::~LSM9DS1()
{
Stop();
perf_free(_interval_perf);
perf_free(_transfer_perf);
perf_free(_fifo_empty_perf);
@@ -77,7 +76,6 @@ int LSM9DS1::probe()
bool LSM9DS1::Init()
{
if (SPI::init() != PX4_OK) {
PX4_ERR("SPI::init failed");
return false;
}
@@ -186,7 +184,7 @@ void LSM9DS1::Stop()
ScheduleClear();
}
void LSM9DS1::Run()
void LSM9DS1::RunImpl()
{
perf_count(_interval_perf);
@@ -312,8 +310,9 @@ void LSM9DS1::Run()
}
}
void LSM9DS1::PrintInfo()
void LSM9DS1::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_interval_perf);
perf_print_counter(_transfer_perf);
perf_print_counter(_fifo_empty_perf);

14
src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp
View File

@@ -50,24 +50,30 @@
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
using ST_LSM9DS1::Register;
class LSM9DS1 : public device::SPI, public px4::ScheduledWorkItem
class LSM9DS1 : public device::SPI, public I2CSPIDriver<LSM9DS1>
{
public:
LSM9DS1(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode);
~LSM9DS1() override;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void print_status();
bool Init();
void Start();
void Stop();
bool Reset();
void PrintInfo();
void RunImpl();
private:
int probe() override;
void Run() override;
uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);

119
src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp
View File

@@ -34,110 +34,75 @@
#include "LSM9DS1.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
namespace lsm9ds1
I2CSPIDriverBase *LSM9DS1::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
LSM9DS1 *instance = new LSM9DS1(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
cli.bus_frequency, cli.spi_mode);
LSM9DS1 *g_dev{nullptr};
static int start(enum Rotation rotation)
{
if (g_dev != nullptr) {
PX4_WARN("already started");
return 0;
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
// create the driver
#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_LSM9DS1_AG)
g_dev = new LSM9DS1(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_LSM9DS1_AG, rotation);
#endif // PX4_SPI_BUS_SENSORS && PX4_SPIDEV_LSM9DS1_AG
if (g_dev == nullptr) {
PX4_ERR("driver start failed");
return -1;
if (!instance->Init()) {
delete instance;
return nullptr;
}
if (!g_dev->Init()) {
PX4_ERR("driver init failed");
delete g_dev;
g_dev = nullptr;
return -1;
}
return 0;
return instance;
}
static int stop()
void
LSM9DS1::print_usage()
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
}
g_dev->Stop();
delete g_dev;
return 0;
PRINT_MODULE_USAGE_NAME("lsm9ds1", "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 status()
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
return -1;
}
g_dev->PrintInfo();
return 0;
}
static void usage()
{
PX4_INFO("missing command: try 'start', 'stop', 'status'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
}
} // namespace lsm9ds1
extern "C" __EXPORT int lsm9ds1_main(int argc, char *argv[])
{
enum Rotation rotation = ROTATION_NONE;
int myoptind = 1;
int ch = 0;
const char *myoptarg = nullptr;
int ch;
using ThisDriver = LSM9DS1;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = ST_LSM9DS1::SPI_SPEED;
/* 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:
lsm9ds1::usage();
return 0;
}
}
if (myoptind >= argc) {
lsm9ds1::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_LSM9DS1_AG);
if (!strcmp(verb, "start")) {
return lsm9ds1::start(rotation);
} else if (!strcmp(verb, "stop")) {
return lsm9ds1::stop();
} else if (!strcmp(verb, "status")) {
return lsm9ds1::status();
return ThisDriver::module_start(cli, iterator);
}
lsm9ds1::usage();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
return 0;
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}

15
src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp
View File

@@ -40,9 +40,10 @@ using namespace time_literals;
static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
LSM9DS1_MAG::LSM9DS1_MAG(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())),
LSM9DS1_MAG::LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation,
int bus_frequency, spi_mode_e spi_mode) :
SPI(MODULE_NAME, nullptr, bus, device, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_px4_mag(get_device_id(), ORB_PRIO_DEFAULT, rotation)
{
set_device_type(DRV_MAG_DEVTYPE_ST_LSM9DS1_M);
@@ -53,8 +54,6 @@ LSM9DS1_MAG::LSM9DS1_MAG(int bus, uint32_t device, enum Rotation rotation) :
LSM9DS1_MAG::~LSM9DS1_MAG()
{
Stop();
perf_free(_interval_perf);
perf_free(_transfer_perf);
perf_free(_data_overrun_perf);
@@ -72,7 +71,6 @@ int LSM9DS1_MAG::probe()
bool LSM9DS1_MAG::Init()
{
if (SPI::init() != PX4_OK) {
PX4_ERR("SPI::init failed");
return false;
}
@@ -159,7 +157,7 @@ void LSM9DS1_MAG::Stop()
ScheduleClear();
}
void LSM9DS1_MAG::Run()
void LSM9DS1_MAG::RunImpl()
{
perf_count(_interval_perf);
@@ -204,8 +202,9 @@ void LSM9DS1_MAG::Run()
}
}
void LSM9DS1_MAG::PrintInfo()
void LSM9DS1_MAG::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_interval_perf);
perf_print_counter(_transfer_perf);
perf_print_counter(_data_overrun_perf);

16
src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp
View File

@@ -48,22 +48,30 @@
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
class LSM9DS1_MAG : public device::SPI, public px4::ScheduledWorkItem
class LSM9DS1_MAG : public device::SPI, public I2CSPIDriver<LSM9DS1_MAG>
{
public:
LSM9DS1_MAG(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode);
~LSM9DS1_MAG() override;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void print_status();
bool Init();
void Start();
void Stop();
bool Reset();
void PrintInfo();
void RunImpl();
private:
int probe() override;
void Run() override;
uint8_t RegisterRead(ST_LSM9DS1_MAG::Register reg);
void RegisterWrite(ST_LSM9DS1_MAG::Register reg, uint8_t value);

119
src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp
View File

@@ -34,110 +34,75 @@
#include "LSM9DS1_MAG.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
namespace lsm9ds1_mag
void
LSM9DS1_MAG::print_usage()
{
LSM9DS1_MAG *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_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_LSM9DS1_M)
g_dev = new LSM9DS1_MAG(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_LSM9DS1_M, rotation);
#endif // PX4_SPI_BUS_SENSORS && PX4_SPIDEV_LSM9DS1_M
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("lsm9ds1_mag", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
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 *LSM9DS1_MAG::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
LSM9DS1_MAG *instance = new LSM9DS1_MAG(iterator.configuredBusOption(), iterator.bus(), iterator.devid(),
cli.rotation, cli.bus_frequency, cli.spi_mode);
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
g_dev->Stop();
delete g_dev;
return 0;
}
static int status()
{
if (g_dev == nullptr) {
PX4_WARN("driver not running");
return -1;
if (!instance->Init()) {
delete instance;
return nullptr;
}
g_dev->PrintInfo();
return 0;
return instance;
}
static void usage()
{
PX4_INFO("missing command: try 'start', 'stop', 'status'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
}
} // namespace lsm9ds1_mag
extern "C" __EXPORT int lsm9ds1_mag_main(int argc, char *argv[])
{
enum Rotation rotation = ROTATION_NONE;
int myoptind = 1;
int ch = 0;
const char *myoptarg = nullptr;
using ThisDriver = LSM9DS1_MAG;
int ch;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = ST_LSM9DS1_MAG::SPI_SPEED;
/* 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:
lsm9ds1_mag::usage();
return 0;
}
}
if (myoptind >= argc) {
lsm9ds1_mag::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
const char *verb = argv[myoptind];
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_ST_LSM9DS1_M);
if (!strcmp(verb, "start")) {
return lsm9ds1_mag::start(rotation);
} else if (!strcmp(verb, "stop")) {
return lsm9ds1_mag::stop();
} else if (!strcmp(verb, "status")) {
return lsm9ds1_mag::status();
return ThisDriver::module_start(cli, iterator);
}
lsm9ds1_mag::usage();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
return 0;
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}