diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index 1880b95d6a..57f2620cfe 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -5,7 +5,7 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 -param set-default SENS_EN_INA226 1 +param set-default SENS_EN_INA226 0 if ver hwbasecmp 008 009 00a 010 011 then diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp index baa54bcd70..bd852b82c9 100644 --- a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp +++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp @@ -50,17 +50,6 @@ PowerMonitorSelectorAuterion::~PowerMonitorSelectorAuterion() = default; bool PowerMonitorSelectorAuterion::init() { - int32_t sens_en = 0; - param_get(param_find("SENS_EN_INA226"), &sens_en); - - if (sens_en == 1) { - - sens_en = 0; - param_set(param_find("SENS_EN_INA226"), &sens_en); - const char *stop_argv[] {"ina226", "stop", NULL}; - exec_builtin("ina226", (char **)stop_argv, NULL, 0); - } - ScheduleNow(); return true; } @@ -80,87 +69,359 @@ void PowerMonitorSelectorAuterion::Run() return; } - for (uint32_t i = 0U; i < SENSORS_NUMBER; ++i) { + /* Always try to start with an EEPROM that contains PM information */ + try_eeprom_start(); - if (!_sensors[i].started) { + if (_eeprom_only_run >= EEPROM_ONLY_RUNS) { + /* Give preference to the EEPROM-based initialization; use probing as a fallback */ + try_probe_start(); - int ret_val = ina226_probe(i); - - if (ret_val == PX4_OK) { - char bus_number[4] = {0}; - itoa(_sensors[i].bus_number, bus_number, 10); - const char *start_argv[] { - _sensors[i].name, - "-X", "-b", bus_number, "-a", _sensors[i].i2c_addr, - "-t", _sensors[i].id, "-q", "start", NULL - }; - - int status = PX4_ERROR; - int pid = exec_builtin(_sensors[i].name, (char **)start_argv, NULL, 0); - - if (pid != -1) { - waitpid(pid, &status, WUNTRACED); - } - - if (status == PX4_OK) { - _sensors[i].started = true; - } - } - } + } else { + _eeprom_only_run++; } ScheduleDelayed(RUN_INTERVAL); } -int PowerMonitorSelectorAuterion::ina226_probe(uint32_t instance) +void PowerMonitorSelectorAuterion::try_eeprom_start() { - struct i2c_master_s *i2c = px4_i2cbus_initialize(_sensors[instance].bus_number); + static_assert(sizeof(_buses) / sizeof(Buses) == BUSES_NUMBERS, "Unexpected number of buses"); + static_assert(sizeof(_eeprom_blocks_pm) / sizeof(EepromBlockPm) == 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]; + + uint16_t dev_type = eeprom_block_pm.dev_type; + 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); + + if (ret_val == PX4_OK) { + _buses[i].started = true; + } + } + } + } + } +} + +void PowerMonitorSelectorAuterion::try_probe_start() +{ + static_assert(sizeof(_sensors) / sizeof(Sensor) == SENSORS_NUMBER, "Unexpected number of sensors"); + + for (uint32_t i = 0U; i < SENSORS_NUMBER; ++i) { + if (i >= BUSES_NUMBERS || !_buses[i].started) { + if (!_sensors[i].started) { + uint16_t dev_type = _sensors[i].dev_type; + + if (!is_user_configured(dev_type)) { + if (ina226_probe(i) == PX4_OK) { + set_shunt_value(dev_type, _sensors[i].shunt_value); + int ret_val = start_pm(_sensors[i].bus_number, dev_type, _sensors[i].i2c_addr, _sensors[i].id); + + if (ret_val == PX4_OK) { + _sensors[i].started = true; + } + } + } + } + } + } +} + +int PowerMonitorSelectorAuterion::ina226_probe(const uint32_t instance) const +{ + I2CWrapper i2c{_sensors[instance].bus_number}; int ret = PX4_ERROR; - if (i2c != nullptr) { - + if (i2c.is_valid()) { struct i2c_msg_s msgv[2]; uint8_t txdata[1] = {0}; uint8_t rxdata[2] = {0}; msgv[0].frequency = I2C_SPEED_STANDARD; - msgv[0].addr = static_cast(strtol(_sensors[instance].i2c_addr, NULL, 0)); + msgv[0].addr = _sensors[instance].i2c_addr; msgv[0].flags = 0; msgv[0].buffer = txdata; msgv[0].length = sizeof(txdata); msgv[1].frequency = I2C_SPEED_STANDARD; - msgv[1].addr = static_cast(strtol(_sensors[instance].i2c_addr, NULL, 0)); + msgv[1].addr = _sensors[instance].i2c_addr; msgv[1].flags = I2C_M_READ; msgv[1].buffer = rxdata; msgv[1].length = sizeof(rxdata); txdata[0] = {INA226_MFG_ID}; - ret = I2C_TRANSFER(i2c, msgv, 2); + ret = I2C_TRANSFER(i2c.get(), msgv, 2); uint16_t value = static_cast(rxdata[1] | rxdata[0] << 8); if (ret != PX4_OK || value != INA226_MFG_ID_TI) { - ret = PX4_ERROR; } else { - txdata[0] = {INA226_MFG_DIEID}; - ret = I2C_TRANSFER(i2c, msgv, 2); + ret = I2C_TRANSFER(i2c.get(), msgv, 2); value = static_cast(rxdata[1] | rxdata[0] << 8); if (ret != PX4_OK || value != INA226_MFG_DIE) { ret = PX4_ERROR; } } - - px4_i2cbus_uninitialize(i2c); } return ret; } +int PowerMonitorSelectorAuterion::eeprom_read(const uint32_t instance) +{ + I2CWrapper i2c{_buses[instance].bus_number}; + _eeprom_valid_blocks_pm = 0; + EepromHeader eeprom_header; + + if (i2c.is_valid()) { + struct i2c_msg_s msgv[2]; + uint8_t txdata[2] = {0}; + + /* Sets EEPROM address pointer to 0 */ + msgv[0].frequency = I2C_SPEED_STANDARD; + msgv[0].addr = _buses[instance].eeprom_i2c_addr; + msgv[0].flags = 0; + msgv[0].buffer = txdata; + msgv[0].length = sizeof(txdata); + + /* Read the header from the EEPROM. */ + msgv[1].frequency = I2C_SPEED_STANDARD; + msgv[1].addr = _buses[instance].eeprom_i2c_addr; + msgv[1].flags = I2C_M_READ; + /* Assumes that the EEPROM was also written on a little-endian platform */ + msgv[1].buffer = reinterpret_cast(&eeprom_header); + msgv[1].length = sizeof(EepromHeader); + + int header_read_ret = I2C_TRANSFER(i2c.get(), msgv, 2); + + if (header_read_ret != PX4_OK || !is_eeprom_header_valid(&eeprom_header)) { + return PX4_ERROR; + } + + int transferred_blocks = 0; + + /* CRC computation starts from the 'flags' field and contains everything that follows (in the header and the blocks) */ + uint8_t *crc_start_ptr = reinterpret_cast(&eeprom_header) + offsetof(EepromHeader, flags); + size_t crc_header_length = sizeof(EepromHeader) - offsetof(EepromHeader, flags); + uint16_t crc = crc16_update(0, crc_start_ptr, crc_header_length); + + while (transferred_blocks < eeprom_header.num_blocks && transferred_blocks < EEPROM_MAX_BLOCKS) { + int block_read_ret = eeprom_read_block(i2c.get(), instance, transferred_blocks, crc); + transferred_blocks++; + + if (block_read_ret != PX4_OK) { + return PX4_ERROR; + } + } + + if (crc != eeprom_header.crc) { + return PX4_ERROR; + } + + _eeprom_valid_blocks_pm = transferred_blocks; + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +bool PowerMonitorSelectorAuterion::is_eeprom_header_valid(EepromHeader *eeprom_header) const +{ + if (eeprom_header->magic != EEPROM_MAGIC + || eeprom_header->version != EEPROM_VERSION + || eeprom_header->num_blocks > EEPROM_MAX_BLOCKS) { + return false; + } + + 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 ret = PX4_ERROR; + EepromBlockHeader eeprom_block_header; + + /* Read the block header 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 = reinterpret_cast(&eeprom_block_header); + msg.length = sizeof(EepromBlockHeader); + + 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; + + /* 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); + + /* Read the actual block data from the EEPROM */ + msg.buffer = data_ptr; + msg.length = data_size; + + ret = I2C_TRANSFER(i2c, &msg, 1); + + if (ret == PX4_OK) { + crc = crc16_update(crc, reinterpret_cast(&eeprom_block_pm), sizeof(EepromBlockPm)); + } + + } else { + ret = PX4_ERROR; + } + + return ret; +} + +bool PowerMonitorSelectorAuterion::is_eeprom_block_header_valid(EepromBlockHeader *eeprom_block_header) const +{ + if (eeprom_block_header->block_type != BlockType::TYPE_PM + || eeprom_block_header->block_type_version != EEPROM_BLOCK_TYPE_VERSION + || eeprom_block_header->block_length != sizeof(EepromBlockPm)) { + 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 +{ + 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); + + char id_str[ID_MAX_LEN]; + 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); + + if (pid != -1) { + waitpid(pid, &status, WUNTRACED); + } + + return status; +} + +const char *PowerMonitorSelectorAuterion::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"; + + case DRV_POWER_DEVTYPE_INA228: + return "ina228"; + + case DRV_POWER_DEVTYPE_INA238: + return "ina238"; + + default: + return nullptr; + } +} + +bool PowerMonitorSelectorAuterion::is_user_configured(const uint16_t dev_type) const +{ + const char *ina_type = get_ina_type(dev_type); + + if (ina_type == nullptr) { + return false; + } + + char param_name[PARAM_MAX_LEN]; + snprintf(param_name, sizeof(param_name), "SENS_EN_INA%s", ina_type); + + int32_t sens_en = 0; + param_get(param_find(param_name), &sens_en); + return sens_en != 0; +} + +void PowerMonitorSelectorAuterion::set_shunt_value(const uint16_t dev_type, const float shunt_value) const +{ + const char *ina_type = get_ina_type(dev_type); + + if (ina_type == nullptr) { + return; + } + + char param_name[PARAM_MAX_LEN]; + snprintf(param_name, sizeof(param_name), "INA%s_SHUNT", ina_type); + + float current_shunt_value = 0; + param_get(param_find(param_name), ¤t_shunt_value); + + if (fabsf(current_shunt_value - shunt_value) > FLT_EPSILON) { + param_set(param_find(param_name), &(shunt_value)); + } +} + +const char *PowerMonitorSelectorAuterion::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"; + + case DRV_POWER_DEVTYPE_INA228: + return "228"; + + case DRV_POWER_DEVTYPE_INA238: + return "238"; + + default: + return nullptr; + } +} + +uint16_t PowerMonitorSelectorAuterion::crc16_update(const uint16_t current_crc, const uint8_t *data_p, + size_t length) const +{ + uint8_t x; + uint16_t crc = current_crc; + + while (length--) { + x = crc >> 8 ^ *data_p++; + x ^= x >> 4; + crc = static_cast((crc << 8) ^ (x << 12) ^ (x << 5) ^ x); + } + + return crc; +} + int PowerMonitorSelectorAuterion::task_spawn(int argc, char *argv[]) { PowerMonitorSelectorAuterion *instance = new PowerMonitorSelectorAuterion(); diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h index 2bc9bffbe3..0099ff04fc 100644 --- a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h +++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h @@ -35,6 +35,8 @@ #include #include +#include +#include #include #include #include @@ -43,6 +45,32 @@ using namespace time_literals; +class I2CWrapper +{ + +public: + explicit I2CWrapper(int bus_number) + { + _i2c = px4_i2cbus_initialize(bus_number); + } + + ~I2CWrapper() + { + if (_i2c != nullptr) { + px4_i2cbus_uninitialize(_i2c); + } + } + + I2CWrapper(const I2CWrapper &) = delete; + I2CWrapper &operator=(const I2CWrapper &) = delete; + + struct i2c_master_s *get() const { return _i2c; } + bool is_valid() const { return _i2c != nullptr; } + +private: + struct i2c_master_s *_i2c {nullptr}; +}; + class PowerMonitorSelectorAuterion : public ModuleBase, public px4::ScheduledWorkItem { @@ -60,59 +88,150 @@ public: static int print_usage(const char *reason = nullptr); private: + enum BlockType : uint8_t { + TYPE_PM = 0 + }; + + struct Buses { + const uint16_t eeprom_i2c_addr; + const uint8_t bus_number; + bool started; + const uint16_t id; + }; + + struct Sensor { + const uint16_t dev_type; + const uint16_t i2c_addr; + const uint8_t bus_number; + const float shunt_value; + bool started; + const uint16_t id; + }; + +#pragma pack(push, 1) + /* Careful when changing the layout. When doing so, adapt CRC calculation! */ + struct EepromHeader { + uint16_t magic; /**< offset 0 */ + uint16_t version; /**< offset 2 */ + uint16_t crc; /**< offset 4 */ + uint16_t flags; /**< offset 6 */ + uint16_t num_blocks; /**< offset 8 */ + uint8_t _reserved1[2]; /**< offset 10 */ + }; +#pragma pack(pop) + +#pragma pack(push, 1) + struct EepromBlockHeader { + uint8_t block_type; /**< offset 0 */ + uint8_t block_type_version; /**< offset 1 */ + uint16_t block_length; /**< offset 2 */ + }; +#pragma pack(pop) + +#pragma pack(push, 1) + /* Block n starts at 12 + (n * 20) */ + struct EepromBlockPm { + EepromBlockHeader block_header; /**< offset 0 */ + uint16_t dev_type; /**< offset 4 */ + uint16_t sensor_type; /**< offset 6 */ + uint16_t i2c_addr; /**< offset 8 */ + uint8_t _padding1[2]; /**< offset 10 */ + float max_current; /**< offset 12 */ + float shunt_value; /**< offset 16 */ + }; +#pragma pack(pop) void Run() override; bool init(); - int ina226_probe(uint32_t instance); + void try_eeprom_start(); + void try_probe_start(); + + int ina226_probe(const uint32_t instance) const; + 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); + 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; + const char *get_start_command(const uint16_t dev_type) const; + + bool is_user_configured(const uint16_t dev_type) const; + void set_shunt_value(const uint16_t dev_type, const float shunt_value) const; + const char *get_ina_type(const uint16_t dev_type) const; + uint16_t crc16_update(const uint16_t current_crc, const uint8_t *data_p, size_t length) const; uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; ///< system armed control topic - - struct Sensor { - const char *name; - const char *i2c_addr; - const uint8_t bus_number; - float shunt_value; - bool started; - const char *id; - }; + int _eeprom_only_run{0}; static constexpr int RUN_INTERVAL{500_ms}; + static constexpr int EEPROM_ONLY_RUNS{2}; + static constexpr int BUSES_NUMBERS{2}; static constexpr int SENSORS_NUMBER{4}; + static constexpr uint16_t EEPROM_MAGIC = 0xAFFA; + static constexpr uint16_t EEPROM_VERSION = 1; + static constexpr uint16_t EEPROM_MAX_BLOCKS = 5; + static constexpr uint8_t EEPROM_BLOCK_TYPE_VERSION = 1; + + static constexpr size_t BUS_MAX_LEN = 4; + static constexpr size_t I2C_ADDR_MAX_LEN = 4; + static constexpr size_t ID_MAX_LEN = 6; + static constexpr size_t PARAM_MAX_LEN = 17; + + Buses _buses[BUSES_NUMBERS] = { + { + .eeprom_i2c_addr = 0x50, + .bus_number = 1, + .started = false, + .id = 1 + }, + { + .eeprom_i2c_addr = 0x50, + .bus_number = 2, + .started = false, + .id = 2 + } + }; + Sensor _sensors[SENSORS_NUMBER] = { { - .name = "ina226", - .i2c_addr = "0x41", + .dev_type = DRV_POWER_DEVTYPE_INA226, + .i2c_addr = 0x41, .bus_number = 1, .shunt_value = 0.0008f, .started = false, - .id = "1" + .id = 1 }, { - .name = "ina226", - .i2c_addr = "0x40", + .dev_type = DRV_POWER_DEVTYPE_INA226, + .i2c_addr = 0x40, .bus_number = 1, .shunt_value = 0.0005f, .started = false, - .id = "1" + .id = 1 }, { - .name = "ina226", - .i2c_addr = "0x41", + .dev_type = DRV_POWER_DEVTYPE_INA226, + .i2c_addr = 0x41, .bus_number = 2, .shunt_value = 0.0008f, .started = false, - .id = "2" + .id = 2 }, { - .name = "ina226", - .i2c_addr = "0x40", + .dev_type = DRV_POWER_DEVTYPE_INA226, + .i2c_addr = 0x40, .bus_number = 2, .shunt_value = 0.0005f, .started = false, - .id = "2" + .id = 2 } }; + + EepromBlockPm _eeprom_blocks_pm[EEPROM_MAX_BLOCKS] = { 0 }; + uint16_t _eeprom_valid_blocks_pm = 0; };