From 6333494d9993c9e31bb044a7910b8785ee5c93ae Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 10 May 2019 18:11:47 +1000
Subject: [PATCH] HAL_ChibiOS: disable peripheral power on boot on fmuv3

then enable after 100ms, and let settle for 20ms. This is to fix an
issue with peripheral power on consistency with all fmuv3 boards

Thanks to Philip for the suggestion
---
 libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c |  4 ++++
 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat  |  4 ++++
 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat     | 14 ++++++++------
 3 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
index c7f382c959..d419fb4d68 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
@@ -291,6 +291,10 @@ void peripheral_power_enable(void)
     // others need it active high
     palWriteLine(HAL_GPIO_PIN_VDD_3V3_SD_CARD_EN, 1);
 #endif
+    for (i=0; i<20; i++) {
+        // give 20ms for sensors to settle
+        chThdSleep(chTimeMS2I(1));
+    }
 #endif
 }
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat
index 195f022a7b..6a95d5eecb 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat
@@ -85,3 +85,7 @@ define HAL_STORAGE_SIZE 16384
 # define CH_DBG_ENABLE_CHECKS TRUE
 # define CH_DBG_SYSTEM_STATE_CHECK TRUE
 # define CH_DBG_ENABLE_STACK_CHECK TRUE
+
+# disable peripheral and sensor power in the bootloader
+PA8 nVDD_5V_PERIPH_EN OUTPUT HIGH
+PE3 VDD_3V3_SENSORS_EN OUTPUT LOW
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat
index 288616bea8..cda1091ead 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat
@@ -213,10 +213,11 @@ PA5 SPI1_SCK SPI1
 PA6 SPI1_MISO SPI1
 PA7 SPI1_MOSI SPI1
 
-# This defines an output pin which will default to output LOW. It is a
-# pin that enables peripheral power on this board.
-
-PA8 VDD_5V_PERIPH_EN OUTPUT LOW
+# This defines an output pin which will default to output HIGH. It is
+# a pin that enables peripheral power on this board. It starts in the
+# off state, then is pulled low to enable peripherals in
+# peripheral_power_enable()
+PA8 nVDD_5V_PERIPH_EN OUTPUT HIGH
 
 # This is the pin that senses USB being connected. It is an input pin
 # setup as OPENDRAIN.
@@ -365,8 +366,9 @@ PE6 SPI4_MOSI SPI4
 
 # This is the pin to enable the sensors rail. It can be used to power
 # cycle sensors to recover them in case there are problems with power on
-# timing affecting sensor stability. We pull it high by default.
-PE3 VDD_3V3_SENSORS_EN OUTPUT HIGH
+# timing affecting sensor stability. We pull it LOW on startup, which
+# means sensors off, then it is pulled HIGH in peripheral_power_enable()
+PE3 VDD_3V3_SENSORS_EN OUTPUT LOW
 
 # UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters).
 PE7 UART7_RX UART7