优化bmi088驱动

nextpilot · Jan 5, 2025 · dc6d3b0 · dc6d3b0
1 parent 184b4cf
commit dc6d3b0
Show file tree

Hide file tree

Showing 6 changed files with 79 additions and 65 deletions.
diff --git a/apps/peripheral/sensor/imu/bmi088/BMI088.cpp b/apps/peripheral/sensor/imu/bmi088/BMI088.cpp
@@ -13,13 +13,13 @@
 #include "BMI088_Accelerometer.hpp"
 #include "BMI088_Gyroscope.hpp"
 
-I2CSPIDriverBase *BMI088::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) {
+BMI088 *BMI088::instantiate(int argc, char *argv[]) {
     BMI088 *instance = nullptr;
 
-    if (config.devid_driver_index == DRV_ACC_DEVTYPE_BMI088) {
+    if (devtype == DRV_ACC_DEVTYPE_BMI088) {
         instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(config);
 
-    } else if (config.devid_driver_index == DRV_GYR_DEVTYPE_BMI088) {
+    } else if (devtype == DRV_GYR_DEVTYPE_BMI088) {
         instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(config);
     }
 

diff --git a/apps/peripheral/sensor/imu/bmi088/BMI088.hpp b/apps/peripheral/sensor/imu/bmi088/BMI088.hpp
@@ -11,22 +11,23 @@
 #pragma once
 
 #include <hrtimer.h>
-#include <drivers/device/spi.h>
+#include <device/spi.hpp>
 #include <perf/perf_counter.h>
-#include <px4_platform_common/i2c_spi_buses.h>
+#include <module/module_command.hpp>
+#include <atomic/atomic.hpp>
 
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb) {
     return (msb << 8u) | lsb;
 }
 
 class BMI088 : public device::SPI, public ModuleCommand<BMI088> {
 public:
-    BMI088(const I2CSPIDriverConfig &config);
+    // BMI088(const I2CSPIDriverConfig &config);
 
     virtual ~BMI088() = default;
 
-    static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
-    static void              print_usage();
+    // static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+    static void print_usage();
 
     virtual void RunImpl() = 0;
 
@@ -43,8 +44,8 @@ class BMI088 : public device::SPI, public ModuleCommand<BMI088> {
     hrt_abstime _temperature_update_timestamp{0};
     int         _failure_count{0};
 
-    px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
-    bool                     _data_ready_interrupt_enabled{false};
+    atomic<hrt_abstime> _drdy_timestamp_sample{0};
+    bool                _data_ready_interrupt_enabled{false};
 
     enum class STATE : uint8_t {
         RESET,

diff --git a/apps/peripheral/sensor/imu/bmi088/BMI088_Accelerometer.hpp b/apps/peripheral/sensor/imu/bmi088/BMI088_Accelerometer.hpp
@@ -12,7 +12,7 @@
 
 #include "BMI088.hpp"
 
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <accelerometer/PX4Accelerometer.hpp>
 
 #include "Bosch_BMI088_Accelerometer_Registers.hpp"
 

diff --git a/apps/peripheral/sensor/imu/bmi088/bmi088_main.cpp b/apps/peripheral/sensor/imu/bmi088/bmi088_main.cpp
@@ -8,12 +8,12 @@
  * Copyright All Reserved © 2015-2024 NextPilot Development Team
  ******************************************************************/
 
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/module.h>
+#include <getopt/getopt.h>
+#include <module/module_usage.h>
 
 #include "BMI088.hpp"
 
-void BMI088::print_usage() {
+int BMI088::print_usage(const char *reason) {
     PRINT_MODULE_USAGE_NAME("bmi088", "driver");
     PRINT_MODULE_USAGE_SUBCATEGORY("imu");
     PRINT_MODULE_USAGE_COMMAND("start");
@@ -22,55 +22,35 @@ void BMI088::print_usage() {
     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();
-}
-
-extern "C" int bmi088_main(int argc, char *argv[]) {
-    int ch;
-    using ThisDriver = BMI088;
-    BusCLIArguments cli{false, true};
-    uint16_t        type      = 0;
-    cli.default_spi_frequency = 10000000;
-    const char *name          = MODULE_NAME;
-
-    while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) {
-        switch (ch) {
-        case 'A':
-            type = DRV_ACC_DEVTYPE_BMI088;
-            name = MODULE_NAME "_accel";
-            break;
 
-        case 'G':
-            type = DRV_GYR_DEVTYPE_BMI088;
-            name = MODULE_NAME "_gyro";
-            break;
+    return 0;
+}
 
-        case 'R':
-            cli.rotation = (enum Rotation)atoi(cli.optArg());
-            break;
-        }
-    }
+static int bmi088_main(int argc, char *argv[]) {
+    return BMI088::main(argc, argv);
+}
 
-    const char *verb = cli.optArg();
+int bmi088_start() {
+    uint32_t sys_hitl = param_get_int32((param_t)params_id::SYS_HITL);
 
-    if (!verb || type == 0) {
-        ThisDriver::print_usage();
-        return -1;
+    if (sys_hitl != 0) {
+        return 0;
     }
 
-    BusInstanceIterator iterator(name, cli, type);
-
-    if (!strcmp(verb, "start")) {
-        return ThisDriver::module_start(cli, iterator);
-    }
+    int ret = 0;
 
-    if (!strcmp(verb, "stop")) {
-        return ThisDriver::module_stop(iterator);
+    // start accel
+    {
+        const char *argv[]  = {"bmi088", "start", "-A", "-R", STRINGIFY(2), "-d", "bmi088_accel"};
+        int         argc    = sizeof(argv) / sizeof(argv[0]);
+        ret                += BMI088::main(argc, (char **)argv);
     }
-
-    if (!strcmp(verb, "status")) {
-        return ThisDriver::module_status(iterator);
+    // start gyro
+    {
+        const char *argv[]  = {"bmi088", "start", "-G", "-R", STRINGIFY(BMI088_ROTATION), "-d", "bmi088_gyro"};
+        int         argc    = sizeof(argv) / sizeof(argv[0]);
+        ret                += BMI088::main(argc, (char **)argv);
     }
 
-    ThisDriver::print_usage();
-    return -1;
+    return ret;
 }
diff --git a/pkgs/device/device.hpp b/pkgs/device/device.hpp
@@ -48,9 +48,23 @@ class Device {
         _bus_name(busname) {
         // 设置device_id
         set_device_type(devtype);
-        rt_device_t dev = rt_device_find(busname);
-        if (dev) {
-            switch (dev->type) {
+        set_device_address(address);
+    }
+
+    virtual ~Device() = default;
+
+    bool valid() {
+        if (_bus_device) {
+            return true;
+        }
+
+        if (!_bus_name) {
+            return false;
+        }
+
+        _bus_device = rt_device_find(_bus_name);
+        if (_bus_device) {
+            switch (_bus_device->type) {
             case RT_Device_Class_I2CBUS:
                 set_device_bus_type(DeviceBusType_I2C);
                 set_device_bus_index(0);
@@ -62,10 +76,9 @@ class Device {
                 break;
             }
         }
-        set_device_address(address);
-    }
 
-    virtual ~Device() = default;
+        return _bus_device != nullptr ? true : false;
+    }
 
     /**
 	 * Initialise the driver and make it ready for use.
@@ -211,17 +224,18 @@ class Device {
     }
 
 protected:
-    // 注册设备的名称比如/dev/ist8310_1
+    // 注册设备名称，比如/dev/ist8310_1
     const char *_dev_name{nullptr};
 
-    // 设备id
+    // 注册设备id
     union DeviceId _dev_id {};
 
-    // 驱动的名称，比如i2c1
+    // 通信接口名称，比如i2c1，spi10
     const char *_bus_name{nullptr};
+    // 通信接口地址
     rt_device_t _bus_device{nullptr};
 
-    // 是否外部设备
+    // 是否外部设备（安装在飞控外部）
     bool _external{false};
 };
 

diff --git a/pkgs/device/spi.hpp b/pkgs/device/spi.hpp
@@ -11,5 +11,24 @@
 #ifndef __SPI_H__
 #define __SPI_H__
 
+#include "device.hpp"
+
+typedef uint32_t spi_drdy_gpio_t;
+
+typedef struct {
+    uint32_t        cs_gpio;   ///< chip-select GPIO (0 if this device is not used)
+    spi_drdy_gpio_t drdy_gpio; ///< data ready GPIO (0 if not set)
+    uint32_t        devid;     ///< SPIDEV_ID(type,index). For PX4 devices on NuttX: index is the device type, and for external buses the CS index
+    uint16_t        devtype;   ///< driver device type, e.g. DRV_IMU_DEVTYPE_ICM20689 (on NuttX: PX4_SPI_DEV_ID(devid) == devtype_driver)
+
+} spi_bus_device_t;
+
+namespace nextpilot::device {
+
+class SPI : public Device {
+    SPI(int16_t chipselect = -1, int bus = -1);
+};
+} // namespace nextpilot::device
+
 
 #endif // __SPI_H__