From 43d859313bdfd4b42645c3fa02c5dca9dce89dfd Mon Sep 17 00:00:00 2001 From: Niklas Hauser Date: Tue, 12 Sep 2023 17:42:05 +0200 Subject: [PATCH] px4/fmu-v5/6x: Upstream power manager for FMU Detects and configures it correctly for the Auterion INA226-based power modules. --- boards/px4/fmu-v5x/default.px4board | 1 + boards/px4/fmu-v5x/init/rc.board_sensors | 3 + boards/px4/fmu-v6x/default.px4board | 1 + boards/px4/fmu-v6x/init/rc.board_sensors | 5 + .../pm_selector_auterion/CMakeLists.txt | 40 ++++ .../pm_selector_auterion/Kconfig | 5 + .../PowerMonitorSelectorAuterion.cpp | 223 ++++++++++++++++++ .../PowerMonitorSelectorAuterion.h | 118 +++++++++ 8 files changed, 396 insertions(+) create mode 100644 src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt create mode 100644 src/drivers/power_monitor/pm_selector_auterion/Kconfig create mode 100644 src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp create mode 100644 src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 0b6e7f6d7a4b..c9c4b5216bfb 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -37,6 +37,7 @@ CONFIG_COMMON_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/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 4f7c1d078de5..d6b91f4505fe 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -94,6 +94,9 @@ else # Internal magnetometer on I2c bmm150 -I -R 6 start + # Auto start power monitors + pm_selector_auterion start + fi # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index c639d88b90e8..9dc9d9a444b6 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -35,6 +35,7 @@ CONFIG_COMMON_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/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 000cd0a38c9c..5155fcf96c1e 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -47,6 +47,11 @@ then fi fi +#Start Auterion Power Module selector for Skynode boards +if ver hwtypecmp V6X009010 V6X010010 +then + pm_selector_auterion start +fi if ver hwtypecmp V6X000006 V6X004006 V6X005006 then diff --git a/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt b/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt new file mode 100644 index 000000000000..b715c5cec598 --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2021 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__power_monitor_pm_selector_auterion + MAIN pm_selector_auterion + SRCS + PowerMonitorSelectorAuterion.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/power_monitor/pm_selector_auterion/Kconfig b/src/drivers/power_monitor/pm_selector_auterion/Kconfig new file mode 100644 index 000000000000..e9d1c3f16f05 --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION + bool "pm_selector_auterion" + default n + ---help--- + Enable support for pm_selector_auterion diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp new file mode 100644 index 000000000000..97c89bea611d --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Automatic handling power monitors + */ + +#include "PowerMonitorSelectorAuterion.h" +#include "../ina226/ina226.h" + +#include +#include + +PowerMonitorSelectorAuterion::PowerMonitorSelectorAuterion() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +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; +} + +void PowerMonitorSelectorAuterion::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + actuator_armed_s actuator_armed{}; + _actuator_armed_sub.copy(&actuator_armed); + + if (actuator_armed.armed) { + exit_and_cleanup(); + return; + } + + for (uint32_t i = 0U; i < SENSORS_NUMBER; ++i) { + + if (!_sensors[i].started) { + + int ret_val = ina226_probe(i); + + if (ret_val == PX4_OK) { + + float current_shunt_value = 0.0f; + param_get(param_find("INA226_SHUNT"), ¤t_shunt_value); + + if (fabsf(current_shunt_value - _sensors[i].shunt_value) > FLT_EPSILON) { + param_set(param_find("INA226_SHUNT"), &(_sensors[i].shunt_value)); + } + + 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; + } + } + } + } + + ScheduleDelayed(RUN_INTERVAL); +} + +int PowerMonitorSelectorAuterion::ina226_probe(uint32_t instance) +{ + struct i2c_master_s *i2c = px4_i2cbus_initialize(_sensors[instance].bus_number); + int ret = PX4_ERROR; + + if (i2c != nullptr) { + + 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].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].flags = I2C_M_READ; + msgv[1].buffer = rxdata; + msgv[1].length = sizeof(rxdata); + + txdata[0] = {INA226_MFG_ID}; + ret = I2C_TRANSFER(i2c, 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); + 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::task_spawn(int argc, char *argv[]) +{ + PowerMonitorSelectorAuterion *instance = new PowerMonitorSelectorAuterion(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int PowerMonitorSelectorAuterion::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int PowerMonitorSelectorAuterion::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for starting and auto-detecting different power monitors. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("pm_selector_auterion", "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[]) +{ + return PowerMonitorSelectorAuterion::main(argc, argv); +} diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h new file mode 100644 index 000000000000..2bc9bffbe38c --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class PowerMonitorSelectorAuterion : public ModuleBase, public px4::ScheduledWorkItem +{ + +public: + PowerMonitorSelectorAuterion(); + virtual ~PowerMonitorSelectorAuterion(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + +private: + + void Run() override; + + bool init(); + + int ina226_probe(uint32_t instance); + + 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; + }; + + static constexpr int RUN_INTERVAL{500_ms}; + static constexpr int SENSORS_NUMBER{4}; + + Sensor _sensors[SENSORS_NUMBER] = { + { + .name = "ina226", + .i2c_addr = "0x41", + .bus_number = 1, + .shunt_value = 0.0008f, + .started = false, + .id = "1" + }, + { + .name = "ina226", + .i2c_addr = "0x40", + .bus_number = 1, + .shunt_value = 0.0005f, + .started = false, + .id = "1" + }, + { + .name = "ina226", + .i2c_addr = "0x41", + .bus_number = 2, + .shunt_value = 0.0008f, + .started = false, + .id = "2" + }, + { + .name = "ina226", + .i2c_addr = "0x40", + .bus_number = 2, + .shunt_value = 0.0005f, + .started = false, + .id = "2" + } + }; +};