diff --git a/src/drivers/power_monitor/ina226/CMakeLists.txt b/src/drivers/power_monitor/ina226/CMakeLists.txt index 000d4f432f8..3c71b8ef84a 100644 --- a/src/drivers/power_monitor/ina226/CMakeLists.txt +++ b/src/drivers/power_monitor/ina226/CMakeLists.txt @@ -37,4 +37,6 @@ px4_add_module( -Wno-cast-align # TODO: fix and enable SRCS ina226.cpp + DEPENDS + px4_work_queue ) diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index 45f03a795b2..147ffef07f7 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -38,41 +38,23 @@ * Driver for the I2C attached INA226 */ #define INA226_RAW // remove this + +#include + #include #include -#include - #include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - +#include #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 */ -#define INA226_DEVICE_PATH "/dev/ina226" /* INA226 Registers addresses */ - #define INA226_REG_CONFIGURATION (0x00) #define INA226_REG_SHUNTVOLTAGE (0x01) #define INA226_REG_BUSVOLTAGE (0x02) @@ -88,7 +70,6 @@ #define INA226_MFG_DIE (0x2260) // INA2260 /* INA226 Configuration Register */ - #define INA226_MODE_SHIFTS (0) #define INA226_MODE_MASK (7 << INA226_MODE_SHIFTS) #define INA226_MODE_SHUTDOWN (0 << INA226_MODE_SHIFTS) @@ -159,15 +140,9 @@ #define INA226_SHUNT 0.0005f /* Shunt is 500 uOhm */ #define INA226_VSCALE 0.00125f /* LSB of voltage is 1.25 mV */ - -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - #define swap16(w) __builtin_bswap16((w)) - -class INA226 : public device::I2C +class INA226 : public device::I2C, px4::ScheduledWorkItem { public: INA226(int bus = INA226_BUS_DEFAULT, int address = INA226_BASEADDR); @@ -175,8 +150,6 @@ public: virtual int init(); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - /** * Diagnostics - print some basic information about the driver. */ @@ -186,30 +159,27 @@ protected: virtual int probe(); private: - work_s _work{}; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; - int _class_instance; - int _orb_class_instance; + bool _sensor_ok{false}; + int _measure_interval{0}; + bool _collect_phase{false}; - orb_advert_t _power_monitor_topic; + orb_advert_t _power_monitor_topic{nullptr}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; - int16_t _bus_volatage; - int16_t _power; - int16_t _current; - int16_t _shunt; - int16_t _cal; - bool _mode_trigged; + int16_t _bus_volatage{0}; + int16_t _power{-1}; + int16_t _current{-1}; + int16_t _shunt{0}; + int16_t _cal{0}; + bool _mode_trigged{false}; - float _max_current = MAX_CURRENT; - float _rshunt = INA226_SHUNT; - uint16_t _config = INA226_CONFIG; - float _current_lsb = _max_current / DN_MAX; - float _power_lsb = 25 * _current_lsb; + float _max_current{MAX_CURRENT}; + float _rshunt{INA226_SHUNT}; + uint16_t _config{INA226_CONFIG}; + float _current_lsb{_max_current / DN_MAX}; + float _power_lsb{25.0f * _current_lsb}; /** * Test whetpower_monitorhe device supported by the driver is present at a @@ -234,30 +204,14 @@ private: */ void stop(); - /** - * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults INA226_MIN_DISTANCE - * and INA226_MAX_DISTANCE - */ - void set_minimum_distance(float min); - void set_maximum_distance(float max); - float get_minimum_distance(); - float get_maximum_distance(); - /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); + void Run() override; + int measure(); int collect(); - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); }; @@ -267,23 +221,11 @@ private: extern "C" __EXPORT int ina226_main(int argc, char *argv[]); INA226::INA226(int bus, int address) : - I2C("INA226", INA226_DEVICE_PATH, bus, address, 100000), - _sensor_ok(false), - _measure_ticks(0), - _collect_phase(false), - _class_instance(-1), - _orb_class_instance(-1), - _power_monitor_topic(nullptr), + I2C("INA226", nullptr, bus, address, 100000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _sample_perf(perf_alloc(PC_ELAPSED, "ina226_read")), - _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")), - _bus_volatage(0), - _power(-1), - _current(-1), - _shunt(0), - _cal(0), - _mode_trigged(false) + _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")) { - float fvalue = MAX_CURRENT; _max_current = fvalue; param_t ph = param_find("INA226_CURRENT"); @@ -319,15 +261,9 @@ INA226::INA226(int bus, int address) : INA226::~INA226() { /* make sure we are truly inactive */ - stop(); - if (_class_instance != -1) { - unregister_class_devname(INA226_DEVICE_PATH, _class_instance); - } - /* free perf counters */ - perf_free(_sample_perf); perf_free(_comms_errors); } @@ -352,7 +288,7 @@ int INA226::read(uint8_t address) int INA226::write(uint8_t address, uint16_t value) { - uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; + uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; return transfer(data, sizeof(data), nullptr, 0); } @@ -380,19 +316,9 @@ INA226::init() ret = write(INA226_REG_CONFIGURATION, _config); } - set_device_address(INA226_BASEADDR); /* set I2c Address */ + set_device_address(INA226_BASEADDR); /* set I2c Address */ - _class_instance = register_class_devname(INA226_DEVICE_PATH); - - struct power_monitor_s report = {}; - _power_monitor_topic = orb_advertise_multi(ORB_ID(power_monitor), &report, - &_orb_class_instance, ORB_PRIO_LOW); - - if (_power_monitor_topic == nullptr) { - PX4_ERR("failed to create power_monitor object"); - } - - ret = measure(); + start(); _sensor_ok = true; return ret; @@ -425,66 +351,6 @@ INA226::probe() return OK; } -int -INA226::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(INA226_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - int ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(INA226_CONVERSION_INTERVAL)) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} int INA226::measure() @@ -506,7 +372,7 @@ INA226::measure() int INA226::collect() { - int ret = -EIO; + int ret = -EIO; /* read from the sensor */ perf_begin(_sample_perf); @@ -540,10 +406,9 @@ INA226::collect() report.al = read(INA226_REG_ALERTLIMIT); #endif - /* publish it, if we are the primary */ - if (_power_monitor_topic != nullptr) { - orb_publish(ORB_ID(power_monitor), _power_monitor_topic, &report); - } + /* publish it */ + int instance; + orb_publish_auto(ORB_ID(power_monitor), &_power_monitor_topic, &report, &instance, ORB_PRIO_DEFAULT); ret = OK; perf_end(_sample_perf); @@ -562,32 +427,25 @@ INA226::collect() void INA226::start() { + ScheduleClear(); /* reset the report ring and state machine */ _collect_phase = false; + _measure_interval = INA226_CONVERSION_INTERVAL; + /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&INA226::cycle_trampoline, this, 5); + ScheduleDelayed(5); } void INA226::stop() { - work_cancel(HPWORK, &_work); + ScheduleClear(); } void -INA226::cycle_trampoline(void *arg) -{ - - INA226 *dev = (INA226 *)arg; - - dev->cycle(); - -} - -void -INA226::cycle() +INA226::Run() { if (_collect_phase) { @@ -602,15 +460,10 @@ INA226::cycle() /* next phase is measurement */ _collect_phase = !_mode_trigged; - - if (_measure_ticks > USEC2TICK(INA226_CONVERSION_INTERVAL)) { + if (_measure_interval > INA226_CONVERSION_INTERVAL) { /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&INA226::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(INA226_CONVERSION_INTERVAL)); + ScheduleDelayed(_measure_interval - INA226_CONVERSION_INTERVAL); return; } } @@ -626,11 +479,7 @@ INA226::cycle() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&INA226::cycle_trampoline, - this, - USEC2TICK(INA226_CONVERSION_INTERVAL)); + ScheduleDelayed(INA226_CONVERSION_INTERVAL); } void @@ -638,7 +487,8 @@ INA226::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - printf("poll interval: %u ticks\n", _measure_ticks); + + printf("poll interval: %u \n", _measure_interval); } /** @@ -652,7 +502,6 @@ INA226 *g_dev; int start(); int start_bus(int i2c_bus); int stop(); -int test(); int info(); /** @@ -690,8 +539,6 @@ start() int start_bus(int i2c_bus) { - int fd = -1; - if (g_dev != nullptr) { PX4_ERR("already started"); return PX4_ERROR; @@ -708,26 +555,10 @@ start_bus(int i2c_bus) goto fail; } - /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(INA226_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - px4_close(fd); return PX4_OK; fail: - if (fd >= 0) { - px4_close(fd); - } - if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; @@ -754,58 +585,6 @@ stop() return PX4_OK; } -int -test() -{ - int h = orb_subscribe(ORB_ID(power_monitor)); - struct power_monitor_s mon; - bool done = false; - - while (!done) { - - bool updated = false; - orb_check(h, &updated); - - if (updated) { - orb_copy(ORB_ID(power_monitor), h, &mon); - print_message(mon); - } - - for (int k = 0; k < 5; k++) { - char c; - struct pollfd fds; - int ret; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - ret = poll(&fds, 1, 0); - - if (ret > 0) { - ret = read(0, &c, 1); - - if (ret) { - done = true; - } - - return 1; - } - - switch (c) { - case 0x03: // ctrl-c - case 0x1b: // esc - case 'c': - case 'q': - done = true; - break; - } - } - - px4_usleep(250000); - } - - orb_unsubscribe(h); - return 0; - -} /** * Print a little info about the driver. */ @@ -880,13 +659,6 @@ ina226_main(int argc, char *argv[]) } } - /* - * Test the driver/device. - */ - if (!strcmp(argv[myoptind], "test")) { - return ina226::test(); - } - /* * Stop the driver */