From e2337a34eb1bb59e4befc1e197cb8a7359e35041 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 14 Jan 2021 13:59:43 +0100 Subject: [PATCH] system_power: add comp_5v_valid and can1_gps1_5v_valid And fill it in from the v5x GPIO expander. --- boards/px4/fmu-v5x/src/board_config.h | 4 ++ msg/system_power.msg | 2 + src/drivers/adc/board_adc/ADC.cpp | 57 +++++++++++++++++++++++++++ src/drivers/adc/board_adc/ADC.hpp | 11 ++++++ 4 files changed, 74 insertions(+) diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 7b5abbfb50..64bcd8e4fb 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -242,6 +242,10 @@ #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN2) #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_VALID "/dev/gpin4" +#define BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID "/dev/gpin5" + /* Spare GPIO */ #define GPIO_PH11 /* PH11 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTH|GPIO_PIN11) diff --git a/msg/system_power.msg b/msg/system_power.msg index 96fc76135d..3a1d3942a5 100644 --- a/msg/system_power.msg +++ b/msg/system_power.msg @@ -8,6 +8,8 @@ uint8 usb_valid # USB is valid when 1 uint8 servo_valid # servo power is good when 1 uint8 periph_5v_oc # peripheral overcurrent when 1 uint8 hipower_5v_oc # high power peripheral overcurrent when 1 +uint8 comp_5v_valid # 5V to companion valid +uint8 can1_gps1_5v_valid # 5V for CAN1/GPS1 valid uint8 BRICK1_VALID_SHIFTS=0 uint8 BRICK1_VALID_MASK=1 diff --git a/src/drivers/adc/board_adc/ADC.cpp b/src/drivers/adc/board_adc/ADC.cpp index 63cad5f147..b9c0939b05 100644 --- a/src/drivers/adc/board_adc/ADC.cpp +++ b/src/drivers/adc/board_adc/ADC.cpp @@ -35,6 +35,10 @@ #include "ADC.hpp" +#ifdef CONFIG_DEV_GPIO +#include +#endif + ADC::ADC(uint32_t base_address, uint32_t channels) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), @@ -80,6 +84,7 @@ ADC::~ADC() perf_free(_sample_perf); px4_arch_adc_uninit(_base_address); + close_gpio_devices(); } int ADC::init() @@ -99,6 +104,11 @@ int ADC::init() void ADC::Run() { + if (_first_run) { + open_gpio_devices(); + _first_run = false; + } + hrt_abstime now = hrt_absolute_time(); /* scan the channel set and sample each */ @@ -110,6 +120,26 @@ void ADC::Run() update_system_power(now); } +void ADC::open_gpio_devices() +{ +#ifdef BOARD_GPIO_VDD_5V_COMP_VALID + _5v_comp_valid_fd = open(BOARD_GPIO_VDD_5V_COMP_VALID, O_RDONLY); +#endif +#ifdef BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID + _5v_can1_gps1_valid_fd = open(BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID, O_RDONLY); +#endif +} + +void ADC::close_gpio_devices() +{ +#ifdef BOARD_GPIO_VDD_5V_COMP_VALID + close(_5v_comp_valid_fd); +#endif +#ifdef BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID + close(_5v_can1_gps1_valid_fd); +#endif +} + void ADC::update_adc_report(hrt_abstime now) { adc_report_s adc = {}; @@ -139,6 +169,26 @@ void ADC::update_adc_report(hrt_abstime now) _to_adc_report.publish(adc); } +uint8_t ADC::read_gpio_value(int fd) +{ +#ifdef CONFIG_DEV_GPIO + + if (fd == -1) { + return 0xff; + } + + bool value; + + if (ioctl(fd, GPIOC_READ, (long)&value) != 0) { + return 0xff; + } + + return value; +#else + return 0xff; +#endif /* CONFIG_DEV_GPIO */ +} + void ADC::update_system_power(hrt_abstime now) { #if defined (BOARD_ADC_USB_CONNECTED) @@ -217,6 +267,13 @@ void ADC::update_system_power(hrt_abstime now) system_power.hipower_5v_oc = BOARD_ADC_HIPOWER_5V_OC; #endif +#ifdef BOARD_GPIO_VDD_5V_COMP_VALID + system_power.comp_5v_valid = read_gpio_value(_5v_comp_valid_fd); +#endif +#ifdef BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID + system_power.can1_gps1_5v_valid = read_gpio_value(_5v_can1_gps1_valid_fd); +#endif + system_power.timestamp = hrt_absolute_time(); _to_system_power.publish(system_power); diff --git a/src/drivers/adc/board_adc/ADC.hpp b/src/drivers/adc/board_adc/ADC.hpp index 659fa40a75..b53fe3d47e 100644 --- a/src/drivers/adc/board_adc/ADC.hpp +++ b/src/drivers/adc/board_adc/ADC.hpp @@ -95,6 +95,10 @@ private: void update_adc_report(hrt_abstime now); void update_system_power(hrt_abstime now); + void open_gpio_devices(); + void close_gpio_devices(); + uint8_t read_gpio_value(int fd); + static const hrt_abstime kINTERVAL{10_ms}; /**< 100Hz base rate */ perf_counter_t _sample_perf; @@ -105,4 +109,11 @@ private: uORB::Publication _to_adc_report{ORB_ID(adc_report)}; uORB::Publication _to_system_power{ORB_ID(system_power)}; +#ifdef BOARD_GPIO_VDD_5V_COMP_VALID + int _5v_comp_valid_fd {-1}; +#endif +#ifdef BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID + int _5v_can1_gps1_valid_fd {-1}; +#endif + bool _first_run {true}; };