diff --git a/boards/auterion/fmu-v6x/src/CMakeLists.txt b/boards/auterion/fmu-v6x/src/CMakeLists.txt index d0f1b0b1a7..3b1a26099a 100644 --- a/boards/auterion/fmu-v6x/src/CMakeLists.txt +++ b/boards/auterion/fmu-v6x/src/CMakeLists.txt @@ -57,6 +57,7 @@ else() sdio.c spi.cpp timer_config.cpp + system.cpp ) add_dependencies(drivers_board arch_board_hw_info) diff --git a/boards/auterion/fmu-v6x/src/board_config.h b/boards/auterion/fmu-v6x/src/board_config.h index 671a1c3e5f..e0ed25a06f 100644 --- a/boards/auterion/fmu-v6x/src/board_config.h +++ b/boards/auterion/fmu-v6x/src/board_config.h @@ -265,6 +265,8 @@ #define GPIO_VDD_3V3_SD_CARD_EN /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) /* MCP23009 GPIO expander */ +#define BOARD_GPIO_VDD_5V_COMP_nEN "/dev/gpio0" +// #define BOARD_GPIO_VDD_5V_CAN1_GPS1_nEN "/dev/gpio1" // connected to GPIO_VDD_5V_PERIPH_nEN instead #define BOARD_GPIO_VDD_5V_COMP_VALID "/dev/gpio4" #define BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID "/dev/gpio5" @@ -503,6 +505,16 @@ __BEGIN_DECLS int stm32_sdio_initialize(void); +/**************************************************************************** + * Name: skynode_system_initialize + * + * Description: + * Initialize the system for the Skynode board + * + ****************************************************************************/ + +int skynode_system_initialize(void); + /**************************************************************************************************** * Name: stm32_spiinitialize * diff --git a/boards/auterion/fmu-v6x/src/init.cpp b/boards/auterion/fmu-v6x/src/init.cpp index 7e35a407c4..fc57d14487 100644 --- a/boards/auterion/fmu-v6x/src/init.cpp +++ b/boards/auterion/fmu-v6x/src/init.cpp @@ -289,6 +289,8 @@ __EXPORT int board_app_initialize(uintptr_t arg) return ret; } + skynode_system_initialize(); + #endif /* !defined(BOOTLOADER) */ return OK; diff --git a/boards/auterion/fmu-v6x/src/system.cpp b/boards/auterion/fmu-v6x/src/system.cpp new file mode 100644 index 0000000000..6fadb07d1d --- /dev/null +++ b/boards/auterion/fmu-v6x/src/system.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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. + * + ****************************************************************************/ + +#include "board_config.h" +#include +#include +#include +#include +#include + +#define MODULE_NAME "system" +static constexpr uint16_t SYSTEM_CHECK_INTERVAL_MS {500}; +static constexpr uint16_t OVERCURRENT_SLEEP_MS{10}; + +static struct work_s system_work; +uORB::Subscription _system_power_sub{ORB_ID(system_power)}; + +static void system_check(void *arg) +{ + system_power_s system_power; + + if (_system_power_sub.update(&system_power)) { + if (system_power.periph_5v_oc || !system_power.can1_gps1_5v_valid) { + if (system_power.periph_5v_oc) { PX4_ERR("5V Periph overcurrent"); } + + if (!system_power.can1_gps1_5v_valid) { PX4_ERR("5V CAN1 GPS1 overcurrent"); } + + // can1_gps1 and periph share the same efuse enable line + VDD_5V_PERIPH_EN(false); + system_usleep(OVERCURRENT_SLEEP_MS * 1000ul); + VDD_5V_PERIPH_EN(true); + } + + if (system_power.hipower_5v_oc) { + PX4_ERR("5V HiPower overcurrent"); + VDD_5V_HIPOWER_EN(false); + system_usleep(OVERCURRENT_SLEEP_MS * 1000ul); + VDD_5V_HIPOWER_EN(true); + } + } + + work_queue(LPWORK, &system_work, system_check, NULL, USEC2TICK(SYSTEM_CHECK_INTERVAL_MS * 1000ul)); +} + +int skynode_system_initialize(void) +{ + system_check(nullptr); + return 0; +}