From 4ac7853479762de3e99a1d72b63a4da40b5fa8c3 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Wed, 21 Jan 2026 19:05:09 +0100 Subject: [PATCH] =?UTF-8?q?auterion=20autostarter:=20Added=20autostart=20f?= =?UTF-8?q?or=20PWM=5FEXPANDER,=20generalized=20c=E2=80=A6=20(#25925)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * auterion autostarter: Added autostart for PWM_EXPANDER, generalized code, renamed module * auterion autostarter: Enable on v6s, robustify error handling * review comments --- boards/accton-godwit/ga1/default.px4board | 1 - boards/auterion/fmu-v6s/default.px4board | 1 + boards/auterion/fmu-v6s/init/rc.board_sensors | 3 + boards/auterion/fmu-v6x/default.px4board | 2 +- boards/auterion/fmu-v6x/init/rc.board_sensors | 4 +- boards/cuav/fmu-v6x/default.px4board | 1 - boards/cuav/x25-evo/default.px4board | 1 - boards/micoair/h743-aio/default.px4board | 1 - boards/micoair/h743-lite/default.px4board | 1 - boards/micoair/h743-v2/default.px4board | 1 - boards/micoair/h743/default.px4board | 1 - boards/px4/fmu-v5x/default.px4board | 2 +- boards/px4/fmu-v5x/init/rc.board_sensors | 4 +- boards/svehicle/e2/default.px4board | 1 - boards/zeroone/x6/default.px4board | 1 - .../AuterionAutostarter.cpp} | 213 ++++++++++++------ .../AuterionAutostarter.h} | 61 +++-- .../CMakeLists.txt | 6 +- src/drivers/auterion_autostarter/Kconfig | 5 + .../pm_selector_auterion/Kconfig | 5 - 20 files changed, 205 insertions(+), 110 deletions(-) rename src/drivers/{power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp => auterion_autostarter/AuterionAutostarter.cpp} (61%) rename src/drivers/{power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h => auterion_autostarter/AuterionAutostarter.h} (78%) rename src/drivers/{power_monitor/pm_selector_auterion => auterion_autostarter}/CMakeLists.txt (93%) create mode 100644 src/drivers/auterion_autostarter/Kconfig delete mode 100644 src/drivers/power_monitor/pm_selector_auterion/Kconfig diff --git a/boards/accton-godwit/ga1/default.px4board b/boards/accton-godwit/ga1/default.px4board index 94af680a4d..9e291d01d7 100644 --- a/boards/accton-godwit/ga1/default.px4board +++ b/boards/accton-godwit/ga1/default.px4board @@ -34,7 +34,6 @@ CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_DRIVERS_RC_INPUT=y diff --git a/boards/auterion/fmu-v6s/default.px4board b/boards/auterion/fmu-v6s/default.px4board index cf1f0f2915..8c0e52cbed 100644 --- a/boards/auterion/fmu-v6s/default.px4board +++ b/boards/auterion/fmu-v6s/default.px4board @@ -22,6 +22,7 @@ CONFIG_COMMON_MAGNETOMETER=y CONFIG_DATAMAN_PERSISTENT_STORAGE=n CONFIG_DRIVERS_PWM_OUT=y CONFIG_COMMON_RC=y +CONFIG_DRIVERS_AUTERION_AUTOSTARTER=y CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y diff --git a/boards/auterion/fmu-v6s/init/rc.board_sensors b/boards/auterion/fmu-v6s/init/rc.board_sensors index ccf6536cf0..b533c9931a 100644 --- a/boards/auterion/fmu-v6s/init/rc.board_sensors +++ b/boards/auterion/fmu-v6s/init/rc.board_sensors @@ -60,6 +60,9 @@ then set INA_CONFIGURED yes fi +# Auterion auto starter +auterion_autostarter start + if param compare BAT1_V_CHANNEL -2 then if [ "$INA_CONFIGURED" != "yes" ] diff --git a/boards/auterion/fmu-v6x/default.px4board b/boards/auterion/fmu-v6x/default.px4board index 84b4cbc57e..1cef7804ab 100644 --- a/boards/auterion/fmu-v6x/default.px4board +++ b/boards/auterion/fmu-v6x/default.px4board @@ -26,10 +26,10 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y +CONFIG_DRIVERS_AUTERION_AUTOSTARTER=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_INPUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y diff --git a/boards/auterion/fmu-v6x/init/rc.board_sensors b/boards/auterion/fmu-v6x/init/rc.board_sensors index 2eeaa65209..7f65a4a7b9 100644 --- a/boards/auterion/fmu-v6x/init/rc.board_sensors +++ b/boards/auterion/fmu-v6x/init/rc.board_sensors @@ -55,8 +55,8 @@ then set INA_CONFIGURED yes fi -#Start Auterion Power Module selector for Skynode boards -pm_selector_auterion start +# Auterion auto starter +auterion_autostarter start # Auterion's INA238 uses a shunt value of 0.0003 instead of 0.0005. param set-default INA238_SHUNT 0.0003 diff --git a/boards/cuav/fmu-v6x/default.px4board b/boards/cuav/fmu-v6x/default.px4board index d54cb466a9..924d74fb03 100644 --- a/boards/cuav/fmu-v6x/default.px4board +++ b/boards/cuav/fmu-v6x/default.px4board @@ -30,7 +30,6 @@ CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_DRIVERS_RC_INPUT=y diff --git a/boards/cuav/x25-evo/default.px4board b/boards/cuav/x25-evo/default.px4board index a5cd2829ca..8c38f593a4 100644 --- a/boards/cuav/x25-evo/default.px4board +++ b/boards/cuav/x25-evo/default.px4board @@ -28,7 +28,6 @@ CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SAFETY_BUTTON=y diff --git a/boards/micoair/h743-aio/default.px4board b/boards/micoair/h743-aio/default.px4board index eb4008cdf2..1a5e79726a 100644 --- a/boards/micoair/h743-aio/default.px4board +++ b/boards/micoair/h743-aio/default.px4board @@ -31,7 +31,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA220=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/micoair/h743-lite/default.px4board b/boards/micoair/h743-lite/default.px4board index 87e3cc7807..372825ced9 100644 --- a/boards/micoair/h743-lite/default.px4board +++ b/boards/micoair/h743-lite/default.px4board @@ -28,7 +28,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA220=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/micoair/h743-v2/default.px4board b/boards/micoair/h743-v2/default.px4board index 27daa469b2..320f1d29e9 100644 --- a/boards/micoair/h743-v2/default.px4board +++ b/boards/micoair/h743-v2/default.px4board @@ -30,7 +30,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA220=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/micoair/h743/default.px4board b/boards/micoair/h743/default.px4board index 8b560b8c3f..90c9b6e6f9 100644 --- a/boards/micoair/h743/default.px4board +++ b/boards/micoair/h743/default.px4board @@ -32,7 +32,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA220=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index b3e93e498d..b964ccbc90 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -34,11 +34,11 @@ CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y +CONFIG_DRIVERS_AUTERION_AUTOSTARTER=y CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_INPUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index a19bcdb8cc..50e3530df9 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -72,8 +72,8 @@ then # Internal magnetometer on I2c bmm150 -I -R 6 start - # Auto start power monitors - pm_selector_auterion start + # Auterion auto starter + auterion_autostarter start # Auterion's INA238 uses a shunt value of 0.0003 instead of 0.0005. param set-default INA238_SHUNT 0.0003 diff --git a/boards/svehicle/e2/default.px4board b/boards/svehicle/e2/default.px4board index a3e6466eea..751fc835dc 100644 --- a/boards/svehicle/e2/default.px4board +++ b/boards/svehicle/e2/default.px4board @@ -31,7 +31,6 @@ CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_COMMON_RC=y diff --git a/boards/zeroone/x6/default.px4board b/boards/zeroone/x6/default.px4board index 341d6c5962..af7e91fb9a 100644 --- a/boards/zeroone/x6/default.px4board +++ b/boards/zeroone/x6/default.px4board @@ -29,7 +29,6 @@ CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_DRIVERS_RC_INPUT=y diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp b/src/drivers/auterion_autostarter/AuterionAutostarter.cpp similarity index 61% rename from src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp rename to src/drivers/auterion_autostarter/AuterionAutostarter.cpp index 294d3ba7b6..28bd1b9a84 100644 --- a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp +++ b/src/drivers/auterion_autostarter/AuterionAutostarter.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2026 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,26 +35,26 @@ * Automatic handling power monitors */ -#include "PowerMonitorSelectorAuterion.h" -#include "../ina226/ina226.h" +#include "AuterionAutostarter.h" +#include "../power_monitor/ina226/ina226.h" #include #include -PowerMonitorSelectorAuterion::PowerMonitorSelectorAuterion() : +AuterionAutostarter::AuterionAutostarter() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { } -PowerMonitorSelectorAuterion::~PowerMonitorSelectorAuterion() = default; +AuterionAutostarter::~AuterionAutostarter() = default; -bool PowerMonitorSelectorAuterion::init() +bool AuterionAutostarter::init() { ScheduleNow(); return true; } -void PowerMonitorSelectorAuterion::Run() +void AuterionAutostarter::Run() { if (should_exit()) { exit_and_cleanup(); @@ -83,33 +83,48 @@ void PowerMonitorSelectorAuterion::Run() ScheduleDelayed(RUN_INTERVAL); } -void PowerMonitorSelectorAuterion::try_eeprom_start() +void AuterionAutostarter::try_eeprom_start() { static_assert(sizeof(_buses) / sizeof(Buses) == BUSES_NUMBERS, "Unexpected number of buses"); - static_assert(sizeof(_eeprom_blocks_pm) / sizeof(EepromBlockPm) == EEPROM_MAX_BLOCKS, + static_assert(sizeof(_eeprom_decoded_blocks) / sizeof(DecodedBlock) == EEPROM_MAX_BLOCKS, "Unexpected number of EEPROM PM blocks"); for (uint32_t i = 0U; i < BUSES_NUMBERS; i++) { if (!_buses[i].started) { if (eeprom_read(i) == PX4_OK) { - for (int ii = 0U; ii < _eeprom_valid_blocks_pm && ii < EEPROM_MAX_BLOCKS; ii++) { - EepromBlockPm eeprom_block_pm = _eeprom_blocks_pm[ii]; + int ret_val = PX4_OK; - uint16_t dev_type = eeprom_block_pm.dev_type; - set_max_current(dev_type, eeprom_block_pm.max_current); - set_shunt_value(dev_type, eeprom_block_pm.shunt_value); - int ret_val = start_pm(_buses[i].bus_number, dev_type, eeprom_block_pm.i2c_addr, _buses[i].id); + for (uint32_t ii = 0U; ii < _eeprom_num_decoded_blocks && ii < EEPROM_MAX_BLOCKS; ii++) { + DecodedBlock decoded_block = _eeprom_decoded_blocks[ii]; - if (ret_val == PX4_OK) { - _buses[i].started = true; + switch (decoded_block.block_type) { + case TYPE_PM: { + EepromBlockPm block_pm = decoded_block.block_pm; + uint16_t dev_type = block_pm.dev_type; + set_max_current(dev_type, block_pm.max_current); + set_shunt_value(dev_type, block_pm.shunt_value); + ret_val |= start_pm(_buses[i].bus_number, dev_type, block_pm.i2c_addr, _buses[i].id); + break; + } + + case TYPE_PWM_EXPANDER: { + EepromBlockPwmExpander block_pwm_expader = decoded_block.block_pwm_expander; + uint16_t dev_type = block_pwm_expader.dev_type; + ret_val |= start_i2c_driver(_buses[i].bus_number, dev_type, block_pwm_expader.i2c_addr); + break; + } } } + + if (ret_val == PX4_OK) { + _buses[i].started = true; + } } } } } -void PowerMonitorSelectorAuterion::try_probe_start() +void AuterionAutostarter::try_probe_start() { static_assert(sizeof(_sensors) / sizeof(Sensor) == SENSORS_NUMBER, "Unexpected number of sensors"); @@ -133,7 +148,7 @@ void PowerMonitorSelectorAuterion::try_probe_start() } } -int PowerMonitorSelectorAuterion::ina226_probe(const uint32_t instance) const +int AuterionAutostarter::ina226_probe(const uint32_t instance) const { I2CWrapper i2c{_sensors[instance].bus_number}; int ret = PX4_ERROR; @@ -177,10 +192,10 @@ int PowerMonitorSelectorAuterion::ina226_probe(const uint32_t instance) const return ret; } -int PowerMonitorSelectorAuterion::eeprom_read(const uint32_t instance) +int AuterionAutostarter::eeprom_read(const uint32_t instance) { I2CWrapper i2c{_buses[instance].bus_number}; - _eeprom_valid_blocks_pm = 0; + _eeprom_num_decoded_blocks = 0; EepromHeader eeprom_header; if (i2c.is_valid()) { @@ -228,7 +243,7 @@ int PowerMonitorSelectorAuterion::eeprom_read(const uint32_t instance) return PX4_ERROR; } - _eeprom_valid_blocks_pm = transferred_blocks; + _eeprom_num_decoded_blocks = transferred_blocks; return PX4_OK; } else { @@ -236,7 +251,7 @@ int PowerMonitorSelectorAuterion::eeprom_read(const uint32_t instance) } } -bool PowerMonitorSelectorAuterion::is_eeprom_header_valid(EepromHeader *eeprom_header) const +bool AuterionAutostarter::is_eeprom_header_valid(EepromHeader *eeprom_header) const { if (eeprom_header->magic != EEPROM_MAGIC || eeprom_header->version != EEPROM_VERSION @@ -247,8 +262,8 @@ bool PowerMonitorSelectorAuterion::is_eeprom_header_valid(EepromHeader *eeprom_h return true; } -int PowerMonitorSelectorAuterion::eeprom_read_block(struct i2c_master_s *i2c, const uint32_t instance, - const uint16_t transferred_blocks, uint16_t &crc) +int AuterionAutostarter::eeprom_read_block(struct i2c_master_s *i2c, const uint32_t instance, const uint16_t transferred_blocks, + uint16_t &crc) { int ret = PX4_ERROR; EepromBlockHeader eeprom_block_header; @@ -263,44 +278,76 @@ int PowerMonitorSelectorAuterion::eeprom_read_block(struct i2c_master_s *i2c, co ret = I2C_TRANSFER(i2c, &msg, 1); - if (ret == PX4_OK && is_eeprom_block_header_valid(&eeprom_block_header)) { - EepromBlockPm &eeprom_block_pm = _eeprom_blocks_pm[transferred_blocks]; - eeprom_block_pm.block_header = eeprom_block_header; + if (ret == PX4_OK) { + DecodedBlock &block = _eeprom_decoded_blocks[transferred_blocks]; - /* Already read the header, so just need to read the block itself now */ - uint8_t *data_ptr = reinterpret_cast(&eeprom_block_pm) + offsetof(EepromBlockPm, dev_type); - size_t data_size = sizeof(EepromBlockPm) - offsetof(EepromBlockPm, dev_type); + switch (eeprom_block_header.block_type) { + case TYPE_PM: { + if (!is_eeprom_block_header_valid(&eeprom_block_header)) { return PX4_ERROR; } - /* Read the actual block data from the EEPROM */ - msg.buffer = data_ptr; - msg.length = data_size; + EepromBlockPm *block_pm = &block.block_pm; + block.block_type = TYPE_PM; + block_pm->block_header = eeprom_block_header; + ret |= eeprom_read_block_data(i2c, instance, crc, block_pm); + break; + } - ret = I2C_TRANSFER(i2c, &msg, 1); + case TYPE_PWM_EXPANDER: { + if (!is_eeprom_block_header_valid(&eeprom_block_header)) { return PX4_ERROR; } - if (ret == PX4_OK) { - crc = crc16_update(crc, reinterpret_cast(&eeprom_block_pm), sizeof(EepromBlockPm)); + EepromBlockPwmExpander *block_pwm_expander = &block.block_pwm_expander; + block.block_type = TYPE_PWM_EXPANDER; + block_pwm_expander->block_header = eeprom_block_header; + ret |= eeprom_read_block_data(i2c, instance, crc, block_pwm_expander); + break; + } + + default: + return PX4_ERROR; } - - } else { - ret = PX4_ERROR; } return ret; } -bool PowerMonitorSelectorAuterion::is_eeprom_block_header_valid(EepromBlockHeader *eeprom_block_header) const +template +int AuterionAutostarter::eeprom_read_block_data(struct i2c_master_s *i2c, const uint32_t instance, uint16_t &crc, T *block) { - if (eeprom_block_header->block_type != BlockType::TYPE_PM + /* Already read the header, so just need to read the block itself now */ + uint8_t *data_ptr = reinterpret_cast(block) + offsetof(T, dev_type); + size_t data_size = sizeof(T) - offsetof(T, dev_type); + + /* Read the actual block data from the EEPROM */ + struct i2c_msg_s msg; + msg.frequency = I2C_SPEED_STANDARD; + msg.addr = _buses[instance].eeprom_i2c_addr; + msg.flags = I2C_M_READ; + msg.buffer = data_ptr; + msg.length = data_size; + + int ret = I2C_TRANSFER(i2c, &msg, 1); + + if (ret == PX4_OK) { + crc = crc16_update(crc, reinterpret_cast(block), sizeof(T)); + } + + return ret; +} + +template +bool AuterionAutostarter::is_eeprom_block_header_valid(EepromBlockHeader *eeprom_block_header) const +{ + if (eeprom_block_header->block_type != ExpectedBlockType || eeprom_block_header->block_type_version != EEPROM_BLOCK_TYPE_VERSION - || eeprom_block_header->block_length != sizeof(EepromBlockPm)) { + || eeprom_block_header->block_length != sizeof(T)) { return false; } return true; } -int PowerMonitorSelectorAuterion::start_pm(const uint8_t bus_number, const uint16_t dev_type, - const uint16_t i2c_addr, const uint16_t id) const +int AuterionAutostarter::start_pm(const uint8_t bus_number, const uint16_t dev_type, + const uint16_t i2c_addr, const uint16_t id) const { char bus_number_str[BUS_MAX_LEN]; snprintf(bus_number_str, sizeof(bus_number_str), "%u", bus_number); @@ -312,33 +359,51 @@ int PowerMonitorSelectorAuterion::start_pm(const uint8_t bus_number, const uint1 snprintf(id_str, sizeof(id_str), "%u", id); const char *start_command = get_start_command(dev_type); - - if (start_command == nullptr) { - return PX4_ERROR; - } - const char *start_argv[] { start_command, "-X", "-b", bus_number_str, "-a", i2c_addr_str, "-t", id_str, "-q", "start", NULL }; - int status = PX4_ERROR; - int pid = exec_builtin(start_command, (char **)start_argv, NULL, 0); + return start(start_command, start_argv); +} - if (pid != -1) { - waitpid(pid, &status, WUNTRACED); +int AuterionAutostarter::start_i2c_driver(const uint8_t bus_number, const uint16_t dev_type, const uint16_t i2c_addr) const +{ + char bus_number_str[BUS_MAX_LEN]; + snprintf(bus_number_str, sizeof(bus_number_str), "%u", bus_number); + + char i2c_addr_str[I2C_ADDR_MAX_LEN]; + snprintf(i2c_addr_str, sizeof(i2c_addr_str), "%u", i2c_addr); + + const char *start_command = get_start_command(dev_type); + const char *start_argv[] { + start_command, + "-X", "-b", bus_number_str, "-a", i2c_addr_str, + "-q", "start", NULL + }; + + return start(start_command, start_argv); +} + +int AuterionAutostarter::start(const char *start_command, const char **start_argv) const +{ + int status = PX4_ERROR; + + if (start_command != nullptr) { + int pid = exec_builtin(start_command, (char **)start_argv, NULL, 0); + + if (pid != -1) { + waitpid(pid, &status, WUNTRACED); + } } return status; } -const char *PowerMonitorSelectorAuterion::get_start_command(const uint16_t dev_type) const +const char *AuterionAutostarter::get_start_command(const uint16_t dev_type) const { switch (dev_type) { - case DRV_POWER_DEVTYPE_INA220: - return "ina220"; - case DRV_POWER_DEVTYPE_INA226: return "ina226"; @@ -348,12 +413,15 @@ const char *PowerMonitorSelectorAuterion::get_start_command(const uint16_t dev_t case DRV_POWER_DEVTYPE_INA238: return "ina238"; + case DRV_PWM_DEVTYPE_PCA9685: + return "pca9685_pwm_out"; + default: return nullptr; } } -bool PowerMonitorSelectorAuterion::is_user_configured(const uint16_t dev_type) const +bool AuterionAutostarter::is_user_configured(const uint16_t dev_type) const { const char *ina_type = get_ina_type(dev_type); @@ -369,7 +437,7 @@ bool PowerMonitorSelectorAuterion::is_user_configured(const uint16_t dev_type) c return sens_en != 0; } -void PowerMonitorSelectorAuterion::set_max_current(const uint16_t dev_type, const float max_current) const +void AuterionAutostarter::set_max_current(const uint16_t dev_type, const float max_current) const { const char *ina_type = get_ina_type(dev_type); @@ -382,7 +450,7 @@ void PowerMonitorSelectorAuterion::set_max_current(const uint16_t dev_type, cons set_float_param(param_name, max_current); } -void PowerMonitorSelectorAuterion::set_shunt_value(const uint16_t dev_type, const float shunt_value) const +void AuterionAutostarter::set_shunt_value(const uint16_t dev_type, const float shunt_value) const { const char *ina_type = get_ina_type(dev_type); @@ -395,7 +463,7 @@ void PowerMonitorSelectorAuterion::set_shunt_value(const uint16_t dev_type, cons set_float_param(param_name, shunt_value); } -void PowerMonitorSelectorAuterion::set_float_param(const char *param_name, const float param_val) const +void AuterionAutostarter::set_float_param(const char *param_name, const float param_val) const { float current_param_value = 0; param_get(param_find(param_name), ¤t_param_value); @@ -405,12 +473,9 @@ void PowerMonitorSelectorAuterion::set_float_param(const char *param_name, const } } -const char *PowerMonitorSelectorAuterion::get_ina_type(const uint16_t dev_type) const +const char *AuterionAutostarter::get_ina_type(const uint16_t dev_type) const { switch (dev_type) { - case DRV_POWER_DEVTYPE_INA220: - return "220"; - case DRV_POWER_DEVTYPE_INA226: return "226"; @@ -425,7 +490,7 @@ const char *PowerMonitorSelectorAuterion::get_ina_type(const uint16_t dev_type) } } -uint16_t PowerMonitorSelectorAuterion::crc16_update(const uint16_t current_crc, const uint8_t *data_p, +uint16_t AuterionAutostarter::crc16_update(const uint16_t current_crc, const uint8_t *data_p, size_t length) const { uint8_t x; @@ -440,9 +505,9 @@ uint16_t PowerMonitorSelectorAuterion::crc16_update(const uint16_t current_crc, return crc; } -int PowerMonitorSelectorAuterion::task_spawn(int argc, char *argv[]) +int AuterionAutostarter::task_spawn(int argc, char *argv[]) { - PowerMonitorSelectorAuterion *instance = new PowerMonitorSelectorAuterion(); + AuterionAutostarter *instance = new AuterionAutostarter(); if (instance) { _object.store(instance); @@ -463,12 +528,12 @@ int PowerMonitorSelectorAuterion::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -int PowerMonitorSelectorAuterion::custom_command(int argc, char *argv[]) +int AuterionAutostarter::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int PowerMonitorSelectorAuterion::print_usage(const char *reason) +int AuterionAutostarter::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); @@ -481,14 +546,14 @@ Driver for starting and auto-detecting different power monitors. )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("pm_selector_auterion", "driver"); + PRINT_MODULE_USAGE_NAME("auterion_autostarter", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } -extern "C" __EXPORT int pm_selector_auterion_main(int argc, char *argv[]) +extern "C" __EXPORT int auterion_autostarter_main(int argc, char *argv[]) { - return PowerMonitorSelectorAuterion::main(argc, argv); + return AuterionAutostarter::main(argc, argv); } diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h b/src/drivers/auterion_autostarter/AuterionAutostarter.h similarity index 78% rename from src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h rename to src/drivers/auterion_autostarter/AuterionAutostarter.h index 23910cf39a..f83107db3b 100644 --- a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h +++ b/src/drivers/auterion_autostarter/AuterionAutostarter.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2026 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -71,12 +71,12 @@ private: struct i2c_master_s *_i2c {nullptr}; }; -class PowerMonitorSelectorAuterion : public ModuleBase, public px4::ScheduledWorkItem +class AuterionAutostarter : public ModuleBase, public px4::ScheduledWorkItem { public: - PowerMonitorSelectorAuterion(); - virtual ~PowerMonitorSelectorAuterion(); + AuterionAutostarter(); + virtual ~AuterionAutostarter(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -89,7 +89,14 @@ public: private: enum BlockType : uint8_t { - TYPE_PM = 0 + TYPE_PM = 0, + TYPE_PWM_EXPANDER = 1, + }; + + enum ClockType : uint8_t { + CLOCK_NONE = 0, + CLOCK_INTERNAL = 1, + CLOCK_EXTERNAL_GENERIC = 2, }; struct Buses { @@ -117,7 +124,7 @@ private: uint16_t flags; /**< offset 6 */ uint16_t num_blocks; /**< offset 8 */ uint8_t _reserved1[2]; /**< offset 10 */ - }; + } __attribute__((aligned(4))); #pragma pack(pop) #pragma pack(push, 1) @@ -125,11 +132,11 @@ private: uint8_t block_type; /**< offset 0 */ uint8_t block_type_version; /**< offset 1 */ uint16_t block_length; /**< offset 2 */ - }; + } __attribute__((aligned(4))); #pragma pack(pop) #pragma pack(push, 1) - /* Block n starts at 12 + (n * 20) */ + /* Block n starts at 12 + (n * 20), each block needs to contain a block_header and dev_type! */ struct EepromBlockPm { EepromBlockHeader block_header; /**< offset 0 */ uint16_t dev_type; /**< offset 4 */ @@ -138,9 +145,33 @@ private: uint8_t _padding1[2]; /**< offset 10 */ float max_current; /**< offset 12 */ float shunt_value; /**< offset 16 */ - }; + } __attribute__((aligned(4))); #pragma pack(pop) +#pragma pack(push, 1) + /* Block n starts at 12 + (n * 20), each block needs to contain a block_header and dev_type! */ + struct EepromBlockPwmExpander { + EepromBlockHeader block_header; /**< offset 0 */ + uint16_t dev_type; /**< offset 4 */ + uint16_t sensor_type; /**< offset 6 */ + uint16_t i2c_addr; /**< offset 8 */ + uint16_t num_channels; /**< offset 10 */ + uint16_t offset_channels; /**< offset 12 */ + ClockType clock_type; /**< offset 14 */ + uint8_t _padding1; /**< offset 15 */ + float signal_level; /**< offset 16 */ + } __attribute__((aligned(4))); +#pragma pack(pop) + + struct DecodedBlock { + BlockType block_type; + + union { + EepromBlockPm block_pm; + EepromBlockPwmExpander block_pwm_expander; + }; + }; + void Run() override; bool init(); @@ -152,12 +183,16 @@ private: int eeprom_read(const uint32_t instance); bool is_eeprom_header_valid(EepromHeader *eeprom_header) const; - int eeprom_read_block(struct i2c_master_s *i2c, const uint32_t instance, const uint16_t transferred_blocks, - uint16_t &crc); + int eeprom_read_block(struct i2c_master_s *i2c, const uint32_t instance, const uint16_t transferred_blocks, uint16_t &crc); + template + int eeprom_read_block_data(struct i2c_master_s *i2c, const uint32_t instance, uint16_t &crc, T *block); + template bool is_eeprom_block_header_valid(EepromBlockHeader *eeprom_block_header) const; int start_pm(const uint8_t bus_number, const uint16_t dev_type, const uint16_t i2c_addr, const uint16_t id) const; + int start_i2c_driver(const uint8_t bus_number, const uint16_t dev_type, const uint16_t i2c_addr) const; const char *get_start_command(const uint16_t dev_type) const; + int start(const char *start_command, const char **start_argv) const; bool is_user_configured(const uint16_t dev_type) const; void set_max_current(const uint16_t dev_type, const float max_current) const; @@ -234,6 +269,6 @@ private: } }; - EepromBlockPm _eeprom_blocks_pm[EEPROM_MAX_BLOCKS] = { 0 }; - uint16_t _eeprom_valid_blocks_pm = 0; + DecodedBlock _eeprom_decoded_blocks[EEPROM_MAX_BLOCKS]; + uint16_t _eeprom_num_decoded_blocks = 0; }; diff --git a/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt b/src/drivers/auterion_autostarter/CMakeLists.txt similarity index 93% rename from src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt rename to src/drivers/auterion_autostarter/CMakeLists.txt index b715c5cec5..d6b3f208a5 100644 --- a/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt +++ b/src/drivers/auterion_autostarter/CMakeLists.txt @@ -31,10 +31,10 @@ # ############################################################################ px4_add_module( - MODULE drivers__power_monitor_pm_selector_auterion - MAIN pm_selector_auterion + MODULE drivers__auterion_autostarter + MAIN auterion_autostarter SRCS - PowerMonitorSelectorAuterion.cpp + AuterionAutostarter.cpp DEPENDS px4_work_queue ) diff --git a/src/drivers/auterion_autostarter/Kconfig b/src/drivers/auterion_autostarter/Kconfig new file mode 100644 index 0000000000..36c633da1c --- /dev/null +++ b/src/drivers/auterion_autostarter/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_AUTERION_AUTOSTARTER + bool "auterion_autostarter" + default n + ---help--- + Enable support for auterion_autostarter diff --git a/src/drivers/power_monitor/pm_selector_auterion/Kconfig b/src/drivers/power_monitor/pm_selector_auterion/Kconfig deleted file mode 100644 index e9d1c3f16f..0000000000 --- a/src/drivers/power_monitor/pm_selector_auterion/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION - bool "pm_selector_auterion" - default n - ---help--- - Enable support for pm_selector_auterion