system_power: add comp_5v_valid and can1_gps1_5v_valid

And fill it in from the v5x GPIO expander.
PX4 · Jan 15, 2021 · e2337a3 · e2337a3
1 parent 704a82a
commit e2337a3
Show file tree

Hide file tree

Showing 4 changed files with 74 additions and 0 deletions.
diff --git 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
@@ -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
@@ -35,6 +35,10 @@
 
 #include "ADC.hpp"
 
+#ifdef CONFIG_DEV_GPIO
+#include <nuttx/ioexpander/gpio.h>
+#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
@@ -95,6 +95,10 @@ class ADC : public ModuleBase<ADC>, public px4::ScheduledWorkItem
 	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 @@ class ADC : public ModuleBase<ADC>, public px4::ScheduledWorkItem
 
 	uORB::Publication<adc_report_s>		_to_adc_report{ORB_ID(adc_report)};
 	uORB::Publication<system_power_s>	_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};
 };