Merge branch 'norbert-walter:master' into system

lib/obp60task/OBP60Extensions.cpp

@@ -107,6 +107,27 @@ void hardwareInit(GwApi *api)
     }
 }
 
+void powerInit(String powermode) {
+    // Max Power | Only 5.0V | Min Power
+    if (powermode == "Max Power" || powermode == "Only 5.0V") {
+#ifdef HARDWARE_V21
+        setPortPin(OBP_POWER_50, true); // Power on 5.0V rail
+#endif
+#ifdef BOARD_OBP40S3
+        setPortPin(OBP_POWER_EPD, true);// Power on ePaper display
+        setPortPin(OBP_POWER_SD, true); // Power on SD card
+#endif
+    } else { // Min Power
+#ifdef HARDWARE_V21
+        setPortPin(OBP_POWER_50, false); // Power off 5.0V rail
+#endif
+#ifdef BOARD_OBP40S3
+        setPortPin(OBP_POWER_EPD, false);// Power off ePaper display
+        setPortPin(OBP_POWER_SD, false); // Power off SD card
+#endif
+    }
+}
+
 void setPortPin(uint pin, bool value){
     pinMode(pin, OUTPUT);
     digitalWrite(pin, value);

lib/obp60task/OBP60Extensions.h

@@ -75,6 +75,7 @@ void deepSleep(CommonData &common);
 uint8_t getLastPage();
 
 void hardwareInit(GwApi *api);
+void powerInit(String powermode);
 
 void setPortPin(uint pin, bool value);          // Set port pin for extension port
 

lib/obp60task/PageSystem.cpp

@@ -25,29 +25,331 @@
  * Consists of some sub-pages with following content:
  *   1. Hard and software information
  *   2. System settings
- *   3. NMEA2000 device list
+ *   3. System configuration: running and NVRAM
+ *   4. NMEA2000 device list
+ *   5. SD Card information if available
+ *
+ * TODO
+ *    - setCpuFrequencyMhz(80|160|240);
+ *    - Accesspoint / ! Änderung im Gatewaycode erforderlich?
+ *         if (! isApActive()) {
+ *           wifiSSID = config->getString(config->wifiSSID);
+ *           wifiPass = config->getString(config->wifiPass);
+ *           wifiSoftAP(wifiSSID, wifiPass);
+ *         }
+ *     - Power mode
+ *         powerInit(powermode);
  */
 
 class PageSystem : public Page
 {
-uint64_t chipid;
-bool simulation;
-bool sdcard;
-String buzzer_mode;
-uint8_t buzzer_power;
-String cpuspeed;
-String rtc_module;
-String gps_module;
-String env_module;
+private:
+    // NVRAM config options
+    String flashLED;
 
-String batt_sensor;
-String solar_sensor;
-String gen_sensor;
-String rot_sensor;
-double homelat;
-double homelon;
+    // Generic data access
 
-char mode = 'N'; // (N)ormal, (S)ettings, (D)evice list, (C)ard
+    uint64_t chipid;
+    bool simulation;
+    bool sdcard;
+    String buzzer_mode;
+    uint8_t buzzer_power;
+    String cpuspeed;
+    String rtc_module;
+    String gps_module;
+    String env_module;
+
+    String batt_sensor;
+    String solar_sensor;
+    String gen_sensor;
+    String rot_sensor;
+    double homelat;
+    double homelon;
+
+    char mode = 'N'; // (N)ormal, (S)ettings, (C)onfiguration, (D)evice list, c(A)rd
+
+    void incMode() {
+        if (mode == 'N') {        // Normal
+            mode = 'S';
+        } else if (mode == 'S') { // Settings
+            mode = 'C';
+        } else if (mode == 'C') { // Config
+            mode = 'D';
+        } else if (mode == 'D') { // Device list
+            if (sdcard) {
+                mode = 'A';       // SD-Card
+            } else {
+                mode = 'N';
+            }
+        } else {
+            mode = 'N';
+        }
+    }
+
+    void decMode() {
+        if (mode == 'N') {
+            if (sdcard) {
+                mode = 'A';
+            } else {
+                mode = 'D';
+            }
+        } else if (mode == 'S') { // Settings
+            mode = 'N';
+        } else if (mode == 'C') { // Config
+            mode = 'S';
+        } else if (mode == 'D') { // Device list
+            mode = 'C';
+        } else {
+            mode = 'D';
+        }
+    }
+
+    void displayModeNormal() {
+        // Default system page view
+
+        uint16_t y0 = 155;
+
+        getdisplay().setFont(&Ubuntu_Bold12pt8b);
+        getdisplay().setCursor(8, 48);
+        getdisplay().print("System information");
+
+        getdisplay().drawXBitmap(320, 25, logo64_bits, logo64_width, logo64_height, commonData->fgcolor);
+
+        getdisplay().setFont(&Ubuntu_Bold8pt8b);
+
+        char ssid[13];
+        snprintf(ssid, 13, "%04X%08X", (uint16_t)(chipid >> 32), (uint32_t)chipid);
+        displayBarcode(String(ssid), 320, 200, 2);
+        getdisplay().setCursor(8, 70);
+        getdisplay().print(String("MCUDEVICE-") + String(ssid));
+
+        getdisplay().setCursor(8, 95);
+        getdisplay().print("Firmware version: ");
+        getdisplay().setCursor(150, 95);
+        getdisplay().print(VERSINFO);
+
+        getdisplay().setCursor(8, 113);
+        getdisplay().print("Board version: ");
+        getdisplay().setCursor(150, 113);
+        getdisplay().print(BOARDINFO);
+        getdisplay().print(String(" HW ") + String(PCBINFO));
+
+        getdisplay().setCursor(8, 131);
+        getdisplay().print("Display version: ");
+        getdisplay().setCursor(150, 131);
+        getdisplay().print(DISPLAYINFO);
+        getdisplay().print("; GxEPD2 v");
+        getdisplay().print(GXEPD2INFO);
+
+        getdisplay().setCursor(8, 265);
+#ifdef BOARD_OBP60S3
+        getdisplay().print("Press STBY to enter deep sleep mode");
+#endif
+#ifdef BOARD_OBP40S3
+        getdisplay().print("Press wheel to enter deep sleep mode");
+#endif
+
+        // Flash memory size
+        uint32_t flash_size = ESP.getFlashChipSize();
+        getdisplay().setCursor(8, y0);
+        getdisplay().print("FLASH:");
+        getdisplay().setCursor(90, y0);
+        getdisplay().print(String(flash_size / 1024) + String(" kB"));
+
+        // PSRAM memory size
+        uint32_t psram_size = ESP.getPsramSize();
+        getdisplay().setCursor(8, y0 + 16);
+        getdisplay().print("PSRAM:");
+        getdisplay().setCursor(90, y0 + 16);
+        getdisplay().print(String(psram_size / 1024) + String(" kB"));
+
+        // FRAM available / status
+        getdisplay().setCursor(8, y0 + 32);
+        getdisplay().print("FRAM:");
+        getdisplay().setCursor(90, y0 + 32);
+        getdisplay().print(hasFRAM ? "available" : "not found");
+
+#ifdef BOARD_OBP40S3
+        // SD-Card
+        getdisplay().setCursor(8, y0 + 48);
+        getdisplay().print("SD-Card:");
+        getdisplay().setCursor(90, y0 + 48);
+        if (sdcard) {
+            uint64_t cardsize = SD.cardSize() / (1024 * 1024);
+            getdisplay().print(String(cardsize) + String(" MB"));
+        } else {
+            getdisplay().print("off");
+        }
+#endif
+
+        // CPU speed config / active
+        getdisplay().setCursor(202, y0);
+        getdisplay().print("CPU speed:");
+        getdisplay().setCursor(300, y0);
+        getdisplay().print(cpuspeed);
+        getdisplay().print(" / ");
+        int cpu_freq = esp_clk_cpu_freq() / 1000000;
+        getdisplay().print(String(cpu_freq));
+
+        // total RAM free
+        int Heap_free = esp_get_free_heap_size();
+        getdisplay().setCursor(202, y0 + 16);
+        getdisplay().print("Total free:");
+        getdisplay().setCursor(300, y0 + 16);
+        getdisplay().print(String(Heap_free));
+
+        // RAM free for task
+        int RAM_free = uxTaskGetStackHighWaterMark(NULL);
+        getdisplay().setCursor(202, y0 + 32);
+        getdisplay().print("Task free:");
+        getdisplay().setCursor(300, y0 + 32);
+        getdisplay().print(String(RAM_free));
+    }
+
+    void displayModeConfig() {
+        // Configuration interface
+
+        uint16_t x0 = 16;
+        uint16_t y0 = 80;
+        uint16_t dy = 20;
+
+        getdisplay().setFont(&Ubuntu_Bold12pt8b);
+        getdisplay().setCursor(8, 48);
+        getdisplay().print("System configuration");
+
+        getdisplay().setFont(&Ubuntu_Bold8pt8b);
+
+        getdisplay().setCursor(x0, y0);
+        getdisplay().print("CPU speed: 80 | 160 | 240");
+        getdisplay().setCursor(x0, y0 + 1 * dy);
+        getdisplay().print("Power mode: Max | 5V | Min");
+        getdisplay().setCursor(x0, y0 + 2 * dy);
+        getdisplay().print("Accesspoint: On | Off");
+
+        // TODO Change NVRAM-preferences settings here
+        getdisplay().setCursor(x0, y0 + 4 * dy);
+        getdisplay().print("Simulation: On | Off");
+    }
+
+    void displayModeSettings() {
+        // View some of the current settings
+
+        const uint16_t x0 = 8;
+        const uint16_t y0 = 72;
+
+        getdisplay().setFont(&Ubuntu_Bold12pt8b);
+        getdisplay().setCursor(x0, 48);
+        getdisplay().print("System settings");
+
+        getdisplay().setFont(&Ubuntu_Bold8pt8b);
+
+        // left column
+        getdisplay().setCursor(x0, y0);
+        getdisplay().print("Simulation:");
+        getdisplay().setCursor(120, y0);
+        getdisplay().print(simulation ? "on" : "off");
+
+        getdisplay().setCursor(x0, y0 + 16);
+        getdisplay().print("Environment:");
+        getdisplay().setCursor(120, y0 + 16);
+        getdisplay().print(env_module);
+
+        getdisplay().setCursor(x0, y0 + 32);
+        getdisplay().print("Buzzer:");
+        getdisplay().setCursor(120, y0 + 32);
+        getdisplay().print(buzzer_mode);
+
+        getdisplay().setCursor(x0, y0 + 64);
+        getdisplay().print("GPS:");
+        getdisplay().setCursor(120, y0 + 64);
+        getdisplay().print(gps_module);
+
+        getdisplay().setCursor(x0, y0 + 80);
+        getdisplay().print("RTC:");
+        getdisplay().setCursor(120, y0 + 80);
+        getdisplay().print(rtc_module);
+
+        getdisplay().setCursor(x0, y0 + 96);
+        getdisplay().print("Wifi:");
+        getdisplay().setCursor(120, y0 + 96);
+        getdisplay().print(commonData->status.wifiApOn ? "on" : "off");
+
+        // Home location
+        getdisplay().setCursor(x0, y0 + 128);
+        getdisplay().print("Home Lat.:");
+        drawTextRalign(230, y0 + 128, formatLatitude(homelat));
+        getdisplay().setCursor(x0, y0 + 144);
+        getdisplay().print("Home Lon.:");
+        drawTextRalign(230, y0 + 144, formatLongitude(homelon));
+
+        // right column
+        getdisplay().setCursor(202, y0);
+        getdisplay().print("Batt. sensor:");
+        getdisplay().setCursor(320, y0);
+        getdisplay().print(batt_sensor);
+
+        // Solar sensor
+        getdisplay().setCursor(202, y0 + 16);
+        getdisplay().print("Solar sensor:");
+        getdisplay().setCursor(320, y0 + 16);
+        getdisplay().print(solar_sensor);
+
+        // Generator sensor
+        getdisplay().setCursor(202, y0 + 32);
+        getdisplay().print("Gen. sensor:");
+        getdisplay().setCursor(320, y0 + 32);
+        getdisplay().print(gen_sensor);
+
+#ifdef BOARD_OBP60S3
+        // Backlight infos
+        getdisplay().setCursor(202, y0 + 64);
+        getdisplay().print("Backlight:");
+        getdisplay().setCursor(320, y0 + 64);
+        getdisplay().printf("%d%%", commonData->backlight.brightness);
+        // TODO test function with OBP60 device
+        getdisplay().setCursor(202, y0 + 80);
+        getdisplay().print("Bl color:");
+        getdisplay().setCursor(320, y0 + 80);
+        getdisplay().print(commonData->backlight.color);
+        getdisplay().setCursor(202, y0 + 96);
+        getdisplay().print("Bl mode:");
+        getdisplay().setCursor(320, y0 + 96);
+        getdisplay().print(commonData->backlight.mode);
+        // TODO Buzzer mode and power
+#endif
+
+        // Gyro sensor
+        // WIP / FEATURE
+    }
+
+    void displayModeSDCard() {
+        // SD card info
+        uint16_t x0 = 20;
+        uint16_t y0 = 72;
+
+        getdisplay().setFont(&Ubuntu_Bold12pt8b);
+        getdisplay().setCursor(8, 48);
+        getdisplay().print("SD Card info");
+
+        getdisplay().setFont(&Ubuntu_Bold8pt8b);
+        getdisplay().setCursor(x0, y0);
+        getdisplay().print("Work in progress...");
+    }
+
+    void displayModeDevicelist() {
+        // NMEA2000 device list
+        getdisplay().setFont(&Ubuntu_Bold12pt8b);
+        getdisplay().setCursor(8, 48);
+        getdisplay().print("NMEA2000 device list");
+
+        getdisplay().setFont(&Ubuntu_Bold8pt8b);
+        getdisplay().setCursor(20, 80);
+        getdisplay().print("RxD: ");
+        getdisplay().print(String(commonData->status.n2kRx));
+        getdisplay().setCursor(20, 100);
+        getdisplay().print("TxD: ");
+        getdisplay().print(String(commonData->status.n2kTx));
+    }
 
 public:
     PageSystem(CommonData &common){
@@ -56,6 +358,8 @@ public:
         if (hasFRAM) {
             mode = fram.read(FRAM_SYSTEM_MODE);
         }
+        flashLED = common.config->getString(common.config->flashLED);
+
         chipid = ESP.getEfuseMac();
         simulation = common.config->getBool(common.config->useSimuData);
 #ifdef BOARD_OBP40S3
@@ -90,19 +394,7 @@ public:
         // Switch display mode
         commonData->logger->logDebug(GwLog::LOG, "System keyboard handler");
         if (key == 2) {
-            if (mode == 'N') {
-                mode = 'S';
-            } else if (mode == 'S') {
-                mode = 'D';
-            } else if (mode == 'D') {
-                if (sdcard) {
-                    mode = 'C';
-                } else {
-                    mode = 'N';
-                }
-            } else {
-                mode = 'N';
-            }
+            incMode();
             if (hasFRAM) fram.write(FRAM_SYSTEM_MODE, mode);
             return 0;
         }
@@ -126,8 +418,13 @@ public:
         }
 #endif
 #ifdef BOARD_OBP40S3
-        // grab cursor keys to disable page navigation
-        if (key == 9 or key == 10) {
+        // use cursor keys for local mode navigation
+        if (key == 9) {
+            incMode();
+            return 0;
+        }
+        if (key == 10) {
+            decMode();
             return 0;
         }
         // standby / deep sleep
@@ -168,225 +465,35 @@ public:
         GwConfigHandler *config = commonData->config;
         GwLog *logger = commonData->logger;
 
-        // Get config data
-        String flashLED = config->getString(config->flashLED);
-
         // Optical warning by limit violation (unused)
-        if(String(flashLED) == "Limit Violation"){
+        if(flashLED == "Limit Violation"){
             setBlinkingLED(false);
             setFlashLED(false); 
         }
 
-        // Logging boat values
-        LOG_DEBUG(GwLog::LOG,"Drawing at PageSystem");
-
-        // Draw page
-        //***********************************************************
-
-        uint16_t x0 = 8;  // left column
-        uint16_t y0 = 48; // data table starts here
+        // Logging page information
+        LOG_DEBUG(GwLog::LOG,"Drawing at PageSystem, Mode=%c", mode);
 
         // Set display in partial refresh mode
-        getdisplay().setPartialWindow(0, 0, getdisplay().width(), getdisplay().height()); // Set partial update
+        getdisplay().setPartialWindow(0, 0, getdisplay().width(), getdisplay().height());
 
-        if (mode == 'N') {
-
-            getdisplay().setFont(&Ubuntu_Bold12pt8b);
-            getdisplay().setCursor(8, 48);
-            getdisplay().print("System Information");
-
-            getdisplay().drawXBitmap(320, 25, logo64_bits, logo64_width, logo64_height, commonData->fgcolor);
-
-            getdisplay().setFont(&Ubuntu_Bold8pt8b);
-            y0 = 155;
-
-            char ssid[13];
-            snprintf(ssid, 13, "%04X%08X", (uint16_t)(chipid >> 32), (uint32_t)chipid);
-            displayBarcode(String(ssid), 320, 200, 2);
-            getdisplay().setCursor(8, 70);
-            getdisplay().print(String("MCUDEVICE-") + String(ssid));
-
-            getdisplay().setCursor(8, 95);
-            getdisplay().print("Firmware version: ");
-            getdisplay().setCursor(150, 95);
-            getdisplay().print(VERSINFO);
-
-            getdisplay().setCursor(8, 113);
-            getdisplay().print("Board version: ");
-            getdisplay().setCursor(150, 113);
-            getdisplay().print(BOARDINFO);
-            getdisplay().print(String(" HW ") + String(PCBINFO));
-
-            getdisplay().setCursor(8, 131);
-            getdisplay().print("Display version: ");
-            getdisplay().setCursor(150, 131);
-            getdisplay().print(DISPLAYINFO);
-            getdisplay().print("; GxEPD2 v");
-            getdisplay().print(GXEPD2INFO);
-
-            getdisplay().setCursor(8, 265);
-#ifdef BOARD_OBP60S3
-            getdisplay().print("Press STBY to enter deep sleep mode");
-#endif
-#ifdef BOARD_OBP40S3
-            getdisplay().print("Press wheel to enter deep sleep mode");
-#endif
-
-            // Flash memory size
-            uint32_t flash_size = ESP.getFlashChipSize();
-            getdisplay().setCursor(8, y0);
-            getdisplay().print("FLASH:");
-            getdisplay().setCursor(90, y0);
-            getdisplay().print(String(flash_size / 1024) + String(" kB"));
-
-            // PSRAM memory size
-            uint32_t psram_size = ESP.getPsramSize();
-            getdisplay().setCursor(8, y0 + 16);
-            getdisplay().print("PSRAM:");
-            getdisplay().setCursor(90, y0 + 16);
-            getdisplay().print(String(psram_size / 1024) + String(" kB"));
-
-            // FRAM available / status
-            getdisplay().setCursor(8, y0 + 32);
-            getdisplay().print("FRAM:");
-            getdisplay().setCursor(90, y0 + 32);
-            getdisplay().print(hasFRAM ? "available" : "not found");
-
-#ifdef BOARD_OBP40S3
-            // SD-Card
-            getdisplay().setCursor(8, y0 + 48);
-            getdisplay().print("SD-Card:");
-            getdisplay().setCursor(90, y0 + 48);
-            if (sdcard) {
-                uint64_t cardsize = SD.cardSize() / (1024 * 1024);
-                getdisplay().print(String(cardsize) + String(" MB"));
-            } else {
-                getdisplay().print("off");
-            }
-#endif
-
-            // CPU speed config / active
-            getdisplay().setCursor(202, y0);
-            getdisplay().print("CPU speed:");
-            getdisplay().setCursor(300, y0);
-            getdisplay().print(cpuspeed);
-            getdisplay().print(" / ");
-            int cpu_freq = esp_clk_cpu_freq() / 1000000;
-            getdisplay().print(String(cpu_freq));
-
-            // total RAM free
-            int Heap_free = esp_get_free_heap_size();
-            getdisplay().setCursor(202, y0 + 16);
-            getdisplay().print("Total free:");
-            getdisplay().setCursor(300, y0 + 16);
-            getdisplay().print(String(Heap_free));
-
-            // RAM free for task
-            int RAM_free = uxTaskGetStackHighWaterMark(NULL);
-            getdisplay().setCursor(202, y0 + 32);
-            getdisplay().print("Task free:");
-            getdisplay().setCursor(300, y0 + 32);
-            getdisplay().print(String(RAM_free));
-
-        } else if (mode == 'S') {
-            // Settings
-            
-            getdisplay().setFont(&Ubuntu_Bold12pt8b);
-            getdisplay().setCursor(x0, 48);
-            getdisplay().print("System settings");
-
-            getdisplay().setFont(&Ubuntu_Bold8pt8b);
-            x0 = 8;
-            y0 = 72;
-
-            // left column
-            getdisplay().setCursor(x0, y0);
-            getdisplay().print("Simulation:");
-            getdisplay().setCursor(120, y0);
-            getdisplay().print(simulation ? "on" : "off");
-
-            getdisplay().setCursor(x0, y0 + 16);
-            getdisplay().print("Environment:");
-            getdisplay().setCursor(120, y0 + 16);
-            getdisplay().print(env_module);
-
-            getdisplay().setCursor(x0, y0 + 32);
-            getdisplay().print("Buzzer:");
-            getdisplay().setCursor(120, y0 + 32);
-            getdisplay().print(buzzer_mode);
-
-            getdisplay().setCursor(x0, y0 + 64);
-            getdisplay().print("GPS:");
-            getdisplay().setCursor(120, y0 + 64);
-            getdisplay().print(gps_module);
-
-            getdisplay().setCursor(x0, y0 + 80);
-            getdisplay().print("RTC:");
-            getdisplay().setCursor(120, y0 + 80);
-            getdisplay().print(rtc_module);
-
-            getdisplay().setCursor(x0, y0 + 96);
-            getdisplay().print("Wifi:");
-            getdisplay().setCursor(120, y0 + 96);
-            getdisplay().print(commonData->status.wifiApOn ? "on" : "off");
-
-            // Home location
-            getdisplay().setCursor(x0, y0 + 128);
-            getdisplay().print("Home Lat.:");
-            getdisplay().setCursor(120, y0 + 128);
-            getdisplay().print(formatLatitude(homelat));
-            getdisplay().setCursor(x0, y0 + 144);
-            getdisplay().print("Home Lon.:");
-            getdisplay().setCursor(120, y0 + 144);
-            getdisplay().print(formatLongitude(homelon));
-
-            // right column
-            getdisplay().setCursor(202, y0);
-            getdisplay().print("Batt. sensor:");
-            getdisplay().setCursor(320, y0);
-            getdisplay().print(batt_sensor);
-
-            // Solar sensor
-            getdisplay().setCursor(202, y0 + 16);
-            getdisplay().print("Solar sensor:");
-            getdisplay().setCursor(320, y0 + 16);
-            getdisplay().print(solar_sensor);
-
-            // Generator sensor
-            getdisplay().setCursor(202, y0 + 32);
-            getdisplay().print("Gen. sensor:");
-            getdisplay().setCursor(320, y0 + 32);
-            getdisplay().print(gen_sensor);
-
-            // Gyro sensor
-
-        } else if (mode == 'C') {
-            // Card info
-            getdisplay().setFont(&Ubuntu_Bold12pt8b);
-            getdisplay().setCursor(8, 48);
-            getdisplay().print("SD Card info");
-
-            getdisplay().setFont(&Ubuntu_Bold8pt8b);
-
-            x0 = 20;
-            y0 = 72;
-            getdisplay().setCursor(x0, y0);
-            getdisplay().print("Work in progress...");
-
-
-        } else {
-            // NMEA2000 device list
-            getdisplay().setFont(&Ubuntu_Bold12pt8b);
-            getdisplay().setCursor(8, 48);
-            getdisplay().print("NMEA2000 device list");
-
-            getdisplay().setFont(&Ubuntu_Bold8pt8b);
-            getdisplay().setCursor(20, 80);
-            getdisplay().print("RxD: ");
-            getdisplay().print(String(commonData->status.n2kRx));
-            getdisplay().setCursor(20, 100);
-            getdisplay().print("TxD: ");
-            getdisplay().print(String(commonData->status.n2kTx));
+        // call current system page
+        switch (mode) {
+            case 'N':
+                displayModeNormal();
+                break;
+            case 'S':
+                displayModeSettings();
+                break;
+            case 'C':
+                displayModeConfig();
+                break;
+            case 'A':
+                displayModeSDCard();
+                break;
+            case 'D':
+                displayModeDevicelist();
+                break;
         }
 
         // Update display

lib/obp60task/obp60task.cpp

@@ -54,31 +54,13 @@ void OBP60Init(GwApi *api){
     
     // Check I2C devices
     
-
     // Init hardware
     hardwareInit(api);
 
-    // Init power rail 5.0V
+    // Init power
     String powermode = api->getConfig()->getConfigItem(api->getConfig()->powerMode,true)->asString();
     api->getLogger()->logDebug(GwLog::DEBUG,"Power Mode is: %s", powermode.c_str());
-    if(powermode == "Max Power" || powermode == "Only 5.0V"){
-        #ifdef HARDWARE_V21
-        setPortPin(OBP_POWER_50, true); // Power on 5.0V rail
-        #endif
-        #ifdef BOARD_OBP40S3
-        setPortPin(OBP_POWER_EPD, true);// Power on ePaper display
-        setPortPin(OBP_POWER_SD, true); // Power on SD card
-        #endif
-    }
-    else{
-        #ifdef HARDWARE_V21
-        setPortPin(OBP_POWER_50, false); // Power off 5.0V rail
-        #endif
-        #ifdef BOARD_OBP40S3
-        setPortPin(OBP_POWER_EPD, false);// Power off ePaper display
-        setPortPin(OBP_POWER_SD, false); // Power off SD card
-        #endif
-    }
+    powerInit(powermode);
 
     #ifdef BOARD_OBP40S3
     bool sdcard = config->getBool(config->useSDCard);