diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index a525dbe8c6a..09c8706a485 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -6,8 +6,8 @@ adc start # Start Digital power monitors -ina226 -b 0 -t 1 -f start -ina226 -b 1 -t 2 -f start +ina226 -X -b 1 -t 1 -k start +ina226 -X -b 2 -t 2 -k start # Internal SPI bus ICM-20602 mpu6000 -R 8 -s -T 20602 start diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index abba8c65b72..6329650f8e9 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -136,6 +136,7 @@ #define DRV_DIST_DEVTYPE_SRF02 0x74 #define DRV_DIST_DEVTYPE_TERARANGER 0x75 #define DRV_DIST_DEVTYPE_VL53LXX 0x76 +#define DRV_POWER_DEVTYPE_INA226 0x77 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index d499f0233c2..900e80c72b4 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -37,29 +37,14 @@ * * Driver for the I2C attached INA226 */ -#define INA226_RAW // remove this #include "ina226.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -INA226::INA226(int battery_index, int bus, int address) : - I2C("INA226", nullptr, i2c_bus_options[bus], address, 100000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +INA226::INA226(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address, int battery_index) : + I2C("INA226", nullptr, bus, address, bus_frequency), ModuleParams(nullptr), - index(bus), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address), _sample_perf(perf_alloc(PC_ELAPSED, "ina226_read")), _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")), @@ -112,9 +97,6 @@ INA226::INA226(int battery_index, int bus, int address) : INA226::~INA226() { - /* make sure we are truly inactive */ - stop(); - /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); @@ -173,8 +155,6 @@ INA226::init() ret = OK; } - set_device_address(INA226_BASEADDR); /* set I2c Address */ - start(); _sensor_ok = true; @@ -329,13 +309,7 @@ INA226::start() } void -INA226::stop() -{ - ScheduleClear(); -} - -void -INA226::Run() +INA226::RunImpl() { if (_initialized) { if (_collect_phase) { @@ -391,8 +365,10 @@ INA226::Run() } void -INA226::print_info() +INA226::print_status() { + I2CSPIDriverBase::print_status(); + if (_initialized) { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -401,6 +377,6 @@ INA226::print_info() } else { PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.", - INA226_INIT_RETRY_INTERVAL_US); + INA226_INIT_RETRY_INTERVAL_US / 1000); } } diff --git a/src/drivers/power_monitor/ina226/ina226.h b/src/drivers/power_monitor/ina226/ina226.h index ccd81f70412..6f4740752d9 100644 --- a/src/drivers/power_monitor/ina226/ina226.h +++ b/src/drivers/power_monitor/ina226/ina226.h @@ -1,8 +1,6 @@ #pragma once -#include - #include #include #include @@ -13,10 +11,9 @@ #include #include #include -#include +#include /* Configuration Constants */ -#define INA226_BUS_DEFAULT PX4_I2C_BUS_EXPANSION #define INA226_BASEADDR 0x41 /* 7-bit address. 8-bit address is 0x41 */ // If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to // connect to the INA226 every this many microseconds @@ -110,13 +107,19 @@ #define swap16(w) __builtin_bswap16((w)) -class INA226 : public device::I2C, px4::ScheduledWorkItem, ModuleParams +class INA226 : public device::I2C, public ModuleParams, public I2CSPIDriver { public: - INA226(int battery_index, int bus = INA226_BUS_DEFAULT, int address = INA226_BASEADDR); + INA226(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address, int battery_index); virtual ~INA226(); - virtual int init() override; + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; /** * Tries to call the init() function. If it fails, then it will schedule to retry again in @@ -129,12 +132,10 @@ public: /** * Diagnostics - print some basic information about the driver. */ - void print_info(); - - size_t index; + void print_status() override; protected: - virtual int probe() override; + int probe() override; private: bool _sensor_ok{false}; @@ -166,6 +167,7 @@ private: uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)}; uORB::Subscription _parameters_sub{ORB_ID(parameter_update)}; + /** * Test whetpower_monitorhe device supported by the driver is present at a * specific address. @@ -184,17 +186,6 @@ private: */ void start(); - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; - int measure(); int collect(); diff --git a/src/drivers/power_monitor/ina226/ina226_main.cpp b/src/drivers/power_monitor/ina226/ina226_main.cpp index 5cd6e5023b3..81e7b961e7c 100644 --- a/src/drivers/power_monitor/ina226/ina226_main.cpp +++ b/src/drivers/power_monitor/ina226/ina226_main.cpp @@ -1,194 +1,35 @@ -#include #include #include -#include -#include -#include -#include -#include -#include -#include #include "ina226.h" -#define MAX_I2C_BATTERY_COUNT 2 - -extern "C" __EXPORT int ina226_main(int argc, char *argv[]); - -/** - * Local functions in support of the shell command. - */ -namespace ina226 +I2CSPIDriverBase *INA226::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { + INA226 *instance = new INA226(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address, + cli.custom2); -INA226 *g_dev[MAX_I2C_BATTERY_COUNT] = {nullptr}; - -int start(int i2c_bus, int address); -int start_bus(int i2c_bus, int address, int battery_index, bool force); -int stop(int i2c_bus, int address); -int info(int i2c_bus, int address); - -/** - * If a device is already running on the specified bus and address, return the index of that device. - * If not, return the index of the first empty slot (nullptr) in the array. - * @param i2c_bus The bus, as an index in i2c_bus_options - * @param address I2C address of the power monitor - * @return If there is already a device running on the given bus with the given address: Return the index of that device - * If there is not already a device running: Return the index of the first nullptr in the array. - * If there are no empty slots in the array, return -1. - */ -int get_index(int i2c_bus, int address) -{ - int first_nullptr = -1; - - for (size_t i = 0; i < MAX_I2C_BATTERY_COUNT; i++) { - //PX4_INFO("Checking number %lu", i); - if (g_dev[i] == nullptr) { - if (first_nullptr < 0) { - first_nullptr = i; - } - - } else if (g_dev[i]->get_device_bus() == i2c_bus_options[i2c_bus] && g_dev[i]->get_device_address() == address) { - return i; - } + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; } - return first_nullptr; -} - -/** - * - * Attempt to start driver on all available I2C busses. - * - * This function will return as soon as the first sensor - * is detected on one of the available busses or if no - * sensors are detected. - * - */ -int -start() -{ - for (unsigned i2c_bus = 0; i2c_bus < NUM_I2C_BUS_OPTIONS; i2c_bus++) { - int index = get_index(i2c_bus, INA226_BASEADDR); - - if (index < 0) { - PX4_ERR("There are already %d instances of INA226 running. No more can be instantiated.", - MAX_I2C_BATTERY_COUNT); - return PX4_ERROR; - - } else if (g_dev[index] == nullptr && start_bus(i2c_bus, INA226_BASEADDR, 1, false) == PX4_OK) { - return PX4_OK; - } - } - - return PX4_ERROR; -} - -/** - * Start the driver on a specific bus. - * - * This function only returns if the sensor is up and running - * or could not be detected successfully. - */ -int -start_bus(int i2c_bus, int address, int battery_index, bool force) -{ - int idx = get_index(i2c_bus, address); - - if (idx < 0) { - PX4_ERR("There are already %d instances of INA226 running. No more can be instantiated.", - MAX_I2C_BATTERY_COUNT); - return PX4_ERROR; - } - - if (g_dev[idx] != nullptr) { - PX4_ERR("Already started on bus %d, address 0x%02X", i2c_bus, address); - return PX4_ERROR; - } - - /* create the driver */ - // TODO: Possibly change this to use statically-allocated memory and placement-new? - g_dev[idx] = new INA226(battery_index, i2c_bus, address); - - if (g_dev[idx] == nullptr) { - PX4_ERR("Unable to allocate memory for INA226"); - goto fail; - } - - if (force) { - if (g_dev[idx]->force_init() != OK) { - PX4_INFO("Failed to initialize INA226 on bus %d, but will try again periodically.", i2c_bus); + if (cli.custom1 == 1) { + if (instance->force_init() != OK) { + PX4_INFO("Failed to init INA226 on bus %d, but will try again periodically.", iterator.bus()); } - } else if (OK != g_dev[idx]->init()) { - PX4_ERR("Failed to initialize on bus %d, address 0x%02X", i2c_bus, address); - goto fail; - - } else { - PX4_INFO("Started INA226 on bus %d", i2c_bus); + } else if (OK != instance->init()) { + delete instance; + return nullptr; } - return PX4_OK; - -fail: - - if (g_dev[idx] != nullptr) { - delete g_dev[idx]; - g_dev[idx] = nullptr; - - } - - return PX4_ERROR; + return instance; } -/** - * Stop the driver - */ -int -stop(int bus, int address) -{ - int idx = get_index(bus, address); - - if (idx < 0 || g_dev[idx] == nullptr) { - PX4_ERR("Driver not running on bus %d, address 0x%02X", bus, address); - return PX4_ERROR; - - } else { - delete g_dev[idx]; - g_dev[idx] = nullptr; - return PX4_OK; - } -} - -/** - * Print a little info about the driver. - */ -int -info(int bus, int address) -{ - bool any_running = false; - - for (int i = 0; i < MAX_I2C_BATTERY_COUNT; i++) { - if (g_dev[i] != nullptr) { - any_running = true; - PX4_INFO("Bus %d, address 0x%02X:", (int) g_dev[i]->index, g_dev[i]->get_device_address()); - g_dev[i]->print_info(); - } - } - - if (!any_running) { - PX4_INFO("No devices are running."); - } - - return PX4_OK; -} - -} /* namespace */ - - -static void -ina2262_usage() +void +INA226::print_usage() { PRINT_MODULE_DESCRIPTION( R"DESCR_STR( @@ -208,101 +49,56 @@ this flag set, the battery must be plugged in before starting the driver. PRINT_MODULE_USAGE_NAME("ina226", "driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start a new instance of the driver"); - PRINT_MODULE_USAGE_PARAM_FLAG('a', "If set, try to start the driver on each availabe I2C bus until a module is found", true); - PRINT_MODULE_USAGE_PARAM_FLAG('f', "If initialization fails, keep retrying periodically. Ignored if the -a flag is set. See full driver documentation for more info", true); - PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, NUM_I2C_BUS_OPTIONS - 1, "I2C bus (default: use board-specific bus)", true); - // The module documentation system INSISTS on decimal literals here. So we can't use a #define constant, and - // we can't use hexadecimal. - PRINT_MODULE_USAGE_PARAM_INT('d', 65, 0, UINT8_MAX, "I2C Address (Start with '0x' for hexadecimal)", true); - PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "Which battery calibration values should be used (1 or 2)", true); - - PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop one instance of the driver"); - PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, NUM_I2C_BUS_OPTIONS - 1, "I2C bus (default: use board-specific bus)", true); - PRINT_MODULE_USAGE_PARAM_INT('d', 65, 0, UINT8_MAX, "I2C Address (Start with '0x' for hexadecimal)", true); - - PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Status of every instance of the driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Status of every instance of the driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x41); + PRINT_MODULE_USAGE_PARAM_FLAG('k', "if initialization (probing) fails, keep retrying periodically", true); + PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "battery index for calibration values (1 or 2)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int +extern "C" int ina226_main(int argc, char *argv[]) { int ch; - int myoptind = 1; - const char *myoptarg = nullptr; - bool start_all = false; - bool force = false; + using ThisDriver = INA226; + BusCLIArguments cli{true, false}; + cli.i2c_address = INA226_BASEADDR; + cli.default_i2c_frequency = 100000; + cli.custom2 = 1; - int i2c_bus = INA226_BUS_DEFAULT; - int address = INA226_BASEADDR; - int battery = 1; - - while ((ch = px4_getopt(argc, argv, "afb:d:t:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "kt:")) != EOF) { switch (ch) { + case 'k': // keep retrying + cli.custom1 = 1; + break; - case 'b': - i2c_bus = atoi(myoptarg); - break; - - case 'a': - start_all = true; - break; - - case 'f': - force = true; - break; - - case 'd': - address = strtol(myoptarg, nullptr, 0); - break; - - case 't': - battery = atoi(myoptarg); - break; - - default: - PX4_WARN("Unknown option!"); - goto out_error; + case 't': // battery index + cli.custom2 = (int)strtol(cli.optarg(), NULL, 0); + break; } } - if (myoptind >= argc) { - goto out_error; + const char *verb = cli.optarg(); + if (!verb) { + ThisDriver::print_usage(); + return -1; } - if (address > 255 || address < 0) { - PX4_ERR("Address must be between 0 and 255"); - goto out_error; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA226); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - if (start_all) { - return ina226::start(); - - } else { - return ina226::start_bus(i2c_bus, address, battery, force); - } + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - return ina226::stop(i2c_bus, address); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - return ina226::info(i2c_bus, address); - } - - out_error: - ina2262_usage(); - return PX4_ERROR; + ThisDriver::print_usage(); + return -1; }