From f13682223a1b5c5a427f0a67da7cf6ba68b1ca7d Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Thu, 29 Jun 2017 12:22:22 -1000
Subject: [PATCH] Added FMUv5 System Power related system_power.msg fields

   voltage3V3_v - the sensor 3.3V voltage rail
   v3v3_valid   - the value of voltage3V3_v  may be 0. This
                  field is a 1 when the HW provides voltage3V3_v

   brick_valid - is now a bit mask. A 1 in the postion inticate the
                 Power controler HW has a valid supply voltage
                 present (in V window) on that priority
                 (channel V1..Vn).
                 The mapping is formed by 1<<battery_status.msg.priority
                 or using the manifest constanst BRICKn_VALID_MASK

   usb_vaild - is now indicated from the Power controler HW or
               the usb_connected if Power controler is
               not present.

   brick_valid == 0 and usb_vaild = 1 implies the FMU is powered
   from USB only

   brick_valid != 0  and usb_vaild = 1 implies the FMU is powered
   from the higest priority brick, providing a 1 bit in brick_valid
   and from USB
---
 msg/system_power.msg          | 14 +++++++++-
 src/drivers/stm32/adc/adc.cpp | 49 ++++++++++++++++++++++++++++++-----
 2 files changed, 56 insertions(+), 7 deletions(-)

diff --git a/msg/system_power.msg b/msg/system_power.msg
index e6648ceec7..06af78ce41 100644
--- a/msg/system_power.msg
+++ b/msg/system_power.msg
@@ -1,6 +1,18 @@
 float32 voltage5V_v		# peripheral 5V rail voltage
+float32 voltage3V3_v	# Sensor 3V3 rail voltage
+uint8 v3v3_valid		# Sensor 3V3 rail voltage was read.
 uint8 usb_connected		# USB is connected when 1
-uint8 brick_valid		# brick power is good when 1
+uint8 brick_valid		# brick bits power is good when bit 1
+uint8 usb_vaild 		# 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		# hi power peripheral overcurrent when 1
+
+uint8 BRICK1_VALID_SHIFTS=0
+uint8 BRICK1_VALID_MASK=1
+uint8 BRICK2_VALID_SHIFTS=1
+uint8 BRICK2_VALID_MASK=2
+uint8 BRICK3_VALID_SHIFTS=2
+uint8 BRICK3_VALID_MASK=4
+uint8 BRICK4_VALID_SHIFTS=3
+uint8 BRICK4_VALID_MASK=8
\ No newline at end of file
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 9c0d5fa4d2..93e14f9672 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -351,21 +351,43 @@ ADC::update_system_power(hrt_abstime now)
 	system_power.timestamp = now;
 
 	system_power.voltage5V_v = 0;
+	system_power.voltage3V3_v = 0;
+	system_power.v3v3_valid = 0;
 
-	/* HW provides ADC_SCALED_V5_SENSE */
-
-#  if defined(ADC_SCALED_V5_SENSE)
+	/* Assume HW provides only ADC_SCALED_V5_SENSE */
+	int cnt = 1;
+	/* HW provides both ADC_SCALED_V5_SENSE and ADC_SCALED_V3V3_SENSORS_SENSE */
+#  if defined(ADC_SCALED_V5_SENSE) && defined(ADC_SCALED_V3V3_SENSORS_SENSE)
+	cnt++;
+#  endif
 
 	for (unsigned i = 0; i < _channel_count; i++) {
+#  if defined(ADC_SCALED_V5_SENSE)
+
 		if (_samples[i].am_channel == ADC_SCALED_V5_SENSE) {
 			// it is 2:1 scaled
 			system_power.voltage5V_v = _samples[i].am_data * (ADC_V5_V_FULL_SCALE / 4096);
+			cnt--;
+
+		} else
+#  endif
+#  if defined(ADC_SCALED_V3V3_SENSORS_SENSE)
+		{
+			if (_samples[i].am_channel == ADC_SCALED_V3V3_SENSORS_SENSE) {
+				// it is 2:1 scaled
+				system_power.voltage3V3_v = _samples[i].am_data * (ADC_3V3_SCALE * (3.3f / 4096.0f));
+				system_power.v3v3_valid = 1;
+				cnt--;
+			}
+		}
+
+#  endif
+
+		if (cnt == 0) {
 			break;
 		}
 	}
 
-#  endif
-
 	/* Note once the board_config.h provides BOARD_ADC_USB_CONNECTED,
 	 * It must provide the true logic GPIO BOARD_ADC_xxxx macros.
 	 */
@@ -373,8 +395,23 @@ ADC::update_system_power(hrt_abstime now)
 	// publish these to the same topic
 
 	system_power.usb_connected = BOARD_ADC_USB_CONNECTED;
+	/* If provided used the Valid signal from HW*/
+#if defined(BOARD_ADC_USB_VALID)
+	system_power.usb_vaild = BOARD_ADC_USB_VALID;
+#else
+	/* If not provided then use connected */
+	system_power.usb_vaild  = system_power.usb_connected;
+#endif
+
+	/* The valid signals (HW dependent) are associated with each brick */
+
+	bool  valid_chan[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
+	system_power.brick_valid = 0;
+
+	for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
+		system_power.brick_valid |=  valid_chan[b] ? 1 << b : 0;
+	}
 
-	system_power.brick_valid   = BOARD_ADC_BRICK_VALID;
 	system_power.servo_valid   = BOARD_ADC_SERVO_VALID;
 
 	// OC pins are active low