From f9fda242fb50db74d15947425040acb235a08f3d Mon Sep 17 00:00:00 2001
From: Martin Vladic <martin.vladic@gmail.com>
Date: Sun, 18 Aug 2019 10:16:16 +0200
Subject: [PATCH] change power state in psu thread

---
 src/eez/apps/psu/init.cpp      |  2 +-
 src/eez/apps/psu/psu.cpp       | 33 +++++++++++++++------------------
 src/eez/apps/psu/psu.h         |  2 +-
 src/eez/apps/psu/scpi/syst.cpp |  8 +++++++-
 4 files changed, 24 insertions(+), 21 deletions(-)

diff --git a/src/eez/apps/psu/init.cpp b/src/eez/apps/psu/init.cpp
index 5e30a3267..f521d10c1 100644
--- a/src/eez/apps/psu/init.cpp
+++ b/src/eez/apps/psu/init.cpp
@@ -32,7 +32,7 @@ void mainLoop(const void *);
 #pragma GCC diagnostic ignored "-Wwrite-strings"
 #endif
 
-osThreadDef(g_psuTask, mainLoop, osPriorityAboveNormal, 0, 1024);
+osThreadDef(g_psuTask, mainLoop, osPriorityAboveNormal, 0, 2048);
 
 #if defined(EEZ_PLATFORM_STM32)
 #pragma GCC diagnostic pop
diff --git a/src/eez/apps/psu/psu.cpp b/src/eez/apps/psu/psu.cpp
index ee14932a3..0aa15ca91 100644
--- a/src/eez/apps/psu/psu.cpp
+++ b/src/eez/apps/psu/psu.cpp
@@ -598,24 +598,23 @@ bool isPowerUp() {
     return g_powerIsUp;
 }
 
-bool changePowerState(bool up) {
-//    if (osThreadGetId() != g_psuTaskHandle) {
-//        osMessagePut(g_psuMessageQueueId, PSU_QUEUE_MESSAGE(PSU_QUEUE_MESSAGE_TYPE_CHANGE_POWER_STATE, up ? 1 : 0), osWaitForever);
-//        osDelay(1000);
-//        return up == g_powerIsUp;
-//    }
-
+void changePowerState(bool up) {
     if (up == g_powerIsUp)
-        return true;
+        return;
 
-    if (up) {
-        // at least MIN_POWER_UP_DELAY seconds shall pass after last power down
-        if (g_testPowerUpDelay) {
-            if (millis() - g_powerDownTime < MIN_POWER_UP_DELAY * 1000)
-                return false;
-            g_testPowerUpDelay = false;
-        }
+    // at least MIN_POWER_UP_DELAY seconds shall pass after last power down
+    if (g_testPowerUpDelay) {
+        if (millis() - g_powerDownTime < MIN_POWER_UP_DELAY * 1000)
+            return;
+        g_testPowerUpDelay = false;
+    }
 
+    if (osThreadGetId() != g_psuTaskHandle) {
+        osMessagePut(g_psuMessageQueueId, PSU_QUEUE_MESSAGE(PSU_QUEUE_MESSAGE_TYPE_CHANGE_POWER_STATE, up ? 1 : 0), osWaitForever);
+        return;
+    }
+
+    if (up) {
         g_bootTestSuccess = true;
 
         // auto recall channels parameters from profile
@@ -624,7 +623,7 @@ bool changePowerState(bool up) {
         auto recall = loadAutoRecallProfile(&profile, &location);
 
         if (!powerUp()) {
-            return false;
+            return;
         }
 
         // auto recall channels parameters from profile
@@ -652,8 +651,6 @@ bool changePowerState(bool up) {
         g_testPowerUpDelay = true;
         g_powerDownTime = millis();
     }
-
-    return true;
 }
 
 void powerDownBySensor() {
diff --git a/src/eez/apps/psu/psu.h b/src/eez/apps/psu/psu.h
index b904ec782..932aa6b69 100644
--- a/src/eez/apps/psu/psu.h
+++ b/src/eez/apps/psu/psu.h
@@ -47,7 +47,7 @@ extern bool g_isBooted;
 bool powerUp();
 void powerDown();
 bool isPowerUp();
-bool changePowerState(bool up);
+void changePowerState(bool up);
 void powerDownBySensor();
 
 bool reset();
diff --git a/src/eez/apps/psu/scpi/syst.cpp b/src/eez/apps/psu/scpi/syst.cpp
index 79043f7e5..9ba4ad2c2 100644
--- a/src/eez/apps/psu/scpi/syst.cpp
+++ b/src/eez/apps/psu/scpi/syst.cpp
@@ -16,6 +16,8 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <eez/system.h>
+
 #include <eez/apps/psu/psu.h>
 
 #include <stdio.h>
@@ -79,7 +81,11 @@ scpi_result_t scpi_cmd_systemPower(scpi_t *context) {
     }
 #endif
 
-    if (!changePowerState(up)) {
+    changePowerState(up);
+
+    osDelay(1000);
+
+    if (!isPowerUp()) {
         SCPI_ErrorPush(context, SCPI_ERROR_EXECUTION_ERROR);
         return SCPI_RES_ERR;
     }