InfiniTime.git

commit 0ce98c7ac7ba66acaf504be9bb042796e12f2733

Author: JF002 <JF002@users.noreply.github.com>

Merge pull request #415 from JF002/move-heap-to-static

Move dynamically allocated variables to static variables.

 src/CMakeLists.txt | 4 
 src/FreeRTOSConfig.h | 2 
 src/components/ble/AlertNotificationClient.cpp | 2 
 src/components/ble/AlertNotificationService.cpp | 2 
 src/components/ble/DfuService.cpp | 6 
 src/components/ble/ImmediateAlertService.cpp | 2 
 src/components/ble/NimbleController.cpp | 2 
 src/components/datetime/DateTimeController.cpp | 10 +
 src/components/datetime/DateTimeController.h | 6 
 src/components/heartrate/HeartRateController.cpp | 3 
 src/components/heartrate/HeartRateController.h | 4 
 src/components/heartrate/Ppg.cpp | 11 -
 src/components/heartrate/Ppg.h | 3 
 src/components/timer/TimerController.cpp | 29 ++--
 src/components/timer/TimerController.h | 13 +-
 src/displayapp/DisplayApp.cpp | 67 +++++++---
 src/displayapp/DisplayApp.h | 10 +
 src/displayapp/DisplayAppRecovery.cpp | 10 +
 src/displayapp/DisplayAppRecovery.h | 2 
 src/displayapp/LittleVgl.cpp | 4 
 src/displayapp/LittleVgl.h | 2 
 src/displayapp/screens/FlashLight.cpp | 4 
 src/displayapp/screens/HeartRate.cpp | 8 
 src/displayapp/screens/ScreenList.h | 13 +
 src/displayapp/screens/SystemInfo.cpp | 10 
 src/displayapp/screens/settings/QuickSettings.cpp | 4 
 src/displayapp/screens/settings/Settings.cpp | 4 
 src/displayapp/screens/settings/Settings.h | 1 
 src/drivers/Bma421.cpp | 2 
 src/drivers/SpiMaster.cpp | 15 +-
 src/drivers/SpiMaster.h | 3 
 src/drivers/TwiMaster.cpp | 4 
 src/drivers/TwiMaster.h | 2 
 src/heartratetask/HeartRateTask.cpp | 7 
 src/main.cpp | 72 ++++++++---
 src/systemtask/Messages.h | 26 ++++
 src/systemtask/SystemTask.cpp | 100 +++++++++-------
 src/systemtask/SystemTask.h | 53 +++-----


diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index cd72992168eecf5010069eace716dfa038665bc4..c72f7c9c0b69f57b54bbcca7c7886d643a099ed8 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -750,8 +750,8 @@ add_definitions(-DNIMBLE_CFG_CONTROLLER)
 add_definitions(-DOS_CPUTIME_FREQ)
 add_definitions(-DNRF52 -DNRF52832 -DNRF52832_XXAA -DNRF52_PAN_74 -DNRF52_PAN_64 -DNRF52_PAN_12 -DNRF52_PAN_58 -DNRF52_PAN_54 -DNRF52_PAN_31 -DNRF52_PAN_51 -DNRF52_PAN_36 -DNRF52_PAN_15 -DNRF52_PAN_20 -DNRF52_PAN_55 -DBOARD_PCA10040)
 add_definitions(-DFREERTOS)
-add_definitions(-D__STACK_SIZE=8192)
-add_definitions(-D__HEAP_SIZE=8192)
+add_definitions(-D__STACK_SIZE=1024)
+add_definitions(-D__HEAP_SIZE=4096)
 
 # NOTE : Add the following defines to enable debug mode of the NRF SDK:
 #add_definitions(-DDEBUG)




diff --git a/src/FreeRTOSConfig.h b/src/FreeRTOSConfig.h
index 15185766b4bb7b451a0a8c22ec7c6e2f3f4e6adf..07c152dc2aa1ca540b560205c9f487e1be6fa271 100644
--- a/src/FreeRTOSConfig.h
+++ b/src/FreeRTOSConfig.h
@@ -62,7 +62,7 @@ #define configCPU_CLOCK_HZ                      (SystemCoreClock)
 #define configTICK_RATE_HZ                      1024
 #define configMAX_PRIORITIES                    (3)
 #define configMINIMAL_STACK_SIZE                (120)
-#define configTOTAL_HEAP_SIZE                   (1024 * 16)
+#define configTOTAL_HEAP_SIZE                   (1024 * 17)
 #define configMAX_TASK_NAME_LEN                 (4)
 #define configUSE_16_BIT_TICKS                  0
 #define configIDLE_SHOULD_YIELD                 1




diff --git a/src/components/ble/AlertNotificationClient.cpp b/src/components/ble/AlertNotificationClient.cpp
index 6043a129bac75b3dee3d951c4a91aeb0d14df57a..c3d1d69a64309614b326519a58cbd84b9309b4ca 100644
--- a/src/components/ble/AlertNotificationClient.cpp
+++ b/src/components/ble/AlertNotificationClient.cpp
@@ -159,7 +159,7 @@     notif.size = messageSize;
     notif.category = Pinetime::Controllers::NotificationManager::Categories::SimpleAlert;
     notificationManager.Push(std::move(notif));
 
-    systemTask.PushMessage(Pinetime::System::SystemTask::Messages::OnNewNotification);
+    systemTask.PushMessage(Pinetime::System::Messages::OnNewNotification);
   }
 }
 




diff --git a/src/components/ble/AlertNotificationService.cpp b/src/components/ble/AlertNotificationService.cpp
index e9f5941e846ce384bf5bfca5c5fd65243407b2d5..d5fc7f65d68a74c7bb72bb6683afdaf7cd9df395 100644
--- a/src/components/ble/AlertNotificationService.cpp
+++ b/src/components/ble/AlertNotificationService.cpp
@@ -79,7 +79,7 @@         notif.category = Pinetime::Controllers::NotificationManager::Categories::SimpleAlert;
         break;
     }
 
-    auto event = Pinetime::System::SystemTask::Messages::OnNewNotification;
+    auto event = Pinetime::System::Messages::OnNewNotification;
     notificationManager.Push(std::move(notif));
     systemTask.PushMessage(event);
   }




diff --git a/src/components/ble/DfuService.cpp b/src/components/ble/DfuService.cpp
index 2031668e6f53b466f9d2a8b00b04b477fed8f7c8..cec194cce94942aa68966f675bae829d28b49652 100644
--- a/src/components/ble/DfuService.cpp
+++ b/src/components/ble/DfuService.cpp
@@ -205,7 +205,7 @@         bleController.StartFirmwareUpdate();
         bleController.State(Pinetime::Controllers::Ble::FirmwareUpdateStates::Running);
         bleController.FirmwareUpdateTotalBytes(0xffffffffu);
         bleController.FirmwareUpdateCurrentBytes(0);
-        systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleFirmwareUpdateStarted);
+        systemTask.PushMessage(Pinetime::System::Messages::BleFirmwareUpdateStarted);
         return 0;
       } else {
         NRF_LOG_INFO("[DFU] -> Start DFU, mode %d not supported!", imageType);
@@ -279,7 +279,7 @@         return 0;
       }
       NRF_LOG_INFO("[DFU] -> Activate image and reset!");
       bleController.StopFirmwareUpdate();
-      systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleFirmwareUpdateFinished);
+      systemTask.PushMessage(Pinetime::System::Messages::BleFirmwareUpdateFinished);
       Reset();
       bleController.State(Pinetime::Controllers::Ble::FirmwareUpdateStates::Validated);
       return 0;
@@ -304,7 +304,7 @@   expectedCrc = 0;
   notificationManager.Reset();
   bleController.State(Pinetime::Controllers::Ble::FirmwareUpdateStates::Error);
   bleController.StopFirmwareUpdate();
-  systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleFirmwareUpdateFinished);
+  systemTask.PushMessage(Pinetime::System::Messages::BleFirmwareUpdateFinished);
 }
 
 DfuService::NotificationManager::NotificationManager() {




diff --git a/src/components/ble/ImmediateAlertService.cpp b/src/components/ble/ImmediateAlertService.cpp
index fd6430afb186f02e4ad53fa6b0acebe532c99f5f..820d3b6e23b395ce299d882c49c606c30d859da5 100644
--- a/src/components/ble/ImmediateAlertService.cpp
+++ b/src/components/ble/ImmediateAlertService.cpp
@@ -67,7 +67,7 @@       std::memcpy(notif.message.data(), alertString, strlen(alertString));
       notif.category = Pinetime::Controllers::NotificationManager::Categories::SimpleAlert;
       notificationManager.Push(std::move(notif));
 
-      systemTask.PushMessage(Pinetime::System::SystemTask::Messages::OnNewNotification);
+      systemTask.PushMessage(Pinetime::System::Messages::OnNewNotification);
     }
   }
 




diff --git a/src/components/ble/NimbleController.cpp b/src/components/ble/NimbleController.cpp
index 67a6d691bf8ea9ca9d66c3cdf29d72001d80a155..2c1d0f9952fe1d5964f9901169049d8c2416b4fe 100644
--- a/src/components/ble/NimbleController.cpp
+++ b/src/components/ble/NimbleController.cpp
@@ -149,7 +149,7 @@         StartAdvertising();
         bleController.Disconnect();
       } else {
         bleController.Connect();
-        systemTask.PushMessage(Pinetime::System::SystemTask::Messages::BleConnected);
+        systemTask.PushMessage(Pinetime::System::Messages::BleConnected);
         connectionHandle = event->connect.conn_handle;
         // Service discovery is deffered via systemtask
       }




diff --git a/src/components/datetime/DateTimeController.cpp b/src/components/datetime/DateTimeController.cpp
index 4f9302ebce7731bcb0a94b3b8971d30b386c6b42..28a70abc9bba8a37e417057f0f7553f66b2abdbd 100644
--- a/src/components/datetime/DateTimeController.cpp
+++ b/src/components/datetime/DateTimeController.cpp
@@ -5,9 +5,6 @@ #include 
 
 using namespace Pinetime::Controllers;
 
-DateTime::DateTime(System::SystemTask& systemTask) : systemTask {systemTask} {
-}
-
 void DateTime::SetTime(
   uint16_t year, uint8_t month, uint8_t day, uint8_t dayOfWeek, uint8_t hour, uint8_t minute, uint8_t second, uint32_t systickCounter) {
   std::tm tm = {
@@ -70,7 +67,8 @@
   // Notify new day to SystemTask
   if (hour == 0 and not isMidnightAlreadyNotified) {
     isMidnightAlreadyNotified = true;
-    systemTask.PushMessage(System::SystemTask::Messages::OnNewDay);
+    if(systemTask != nullptr)
+      systemTask->PushMessage(System::Messages::OnNewDay);
   } else if (hour != 0) {
     isMidnightAlreadyNotified = false;
   }
@@ -102,6 +100,10 @@ }
 
 const char* DateTime::DayOfWeekShortToStringLow() {
   return DateTime::DaysStringShortLow[(uint8_t) dayOfWeek];
+}
+
+void DateTime::Register(Pinetime::System::SystemTask* systemTask) {
+  this->systemTask = systemTask;
 }
 
 char const* DateTime::DaysStringLow[] = {"--", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday", "Sunday"};




diff --git a/src/components/datetime/DateTimeController.h b/src/components/datetime/DateTimeController.h
index d0ae727eab51802d696a2e43b1b55977a30e3082..265d6e9def7d768dc75a457a20367846c8b6050f 100644
--- a/src/components/datetime/DateTimeController.h
+++ b/src/components/datetime/DateTimeController.h
@@ -27,8 +27,6 @@         November,
         December
       };
 
-      DateTime(System::SystemTask& systemTask);
-
       void SetTime(uint16_t year,
                    uint8_t month,
                    uint8_t day,
@@ -75,8 +73,9 @@       std::chrono::seconds Uptime() const {
         return uptime;
       }
 
+      void Register(System::SystemTask* systemTask);
+
     private:
-      System::SystemTask& systemTask;
       uint16_t year = 0;
       Months month = Months::Unknown;
       uint8_t day = 0;
@@ -90,6 +89,7 @@       std::chrono::time_point currentDateTime;
       std::chrono::seconds uptime {0};
 
       bool isMidnightAlreadyNotified = false;
+      System::SystemTask* systemTask = nullptr;
 
       static char const* DaysString[];
       static char const* DaysStringShort[];




diff --git a/src/components/heartrate/HeartRateController.cpp b/src/components/heartrate/HeartRateController.cpp
index e84d665af1b0fb52574b4aa6c5b7974694c62f87..716813b3efba48d5d254fb4df44e6fd9756f62b8 100644
--- a/src/components/heartrate/HeartRateController.cpp
+++ b/src/components/heartrate/HeartRateController.cpp
@@ -4,9 +4,6 @@ #include 
 
 using namespace Pinetime::Controllers;
 
-HeartRateController::HeartRateController(Pinetime::System::SystemTask& systemTask) : systemTask {systemTask} {
-}
-
 void HeartRateController::Update(HeartRateController::States newState, uint8_t heartRate) {
   this->state = newState;
   if (this->heartRate != heartRate) {




diff --git a/src/components/heartrate/HeartRateController.h b/src/components/heartrate/HeartRateController.h
index d3a8460d884ea556866df99d4b2b82ed7d24c5e0..a63f1a70c3d601f3864befe796003e351fb413b9 100644
--- a/src/components/heartrate/HeartRateController.h
+++ b/src/components/heartrate/HeartRateController.h
@@ -15,8 +15,7 @@     class HeartRateController {
     public:
       enum class States { Stopped, NotEnoughData, NoTouch, Running };
 
-      explicit HeartRateController(System::SystemTask& systemTask);
-
+      HeartRateController() = default;
       void Start();
       void Stop();
       void Update(States newState, uint8_t heartRate);
@@ -32,7 +31,6 @@
       void SetService(Pinetime::Controllers::HeartRateService* service);
 
     private:
-      System::SystemTask& systemTask;
       Applications::HeartRateTask* task = nullptr;
       States state = States::Stopped;
       uint8_t heartRate = 0;




diff --git a/src/components/heartrate/Ppg.cpp b/src/components/heartrate/Ppg.cpp
index da0789e0377a8f2641b62e57e1a3778994b05e0d..fcba381500ff0bf669b8993f5f44dec5df6e843d 100644
--- a/src/components/heartrate/Ppg.cpp
+++ b/src/components/heartrate/Ppg.cpp
@@ -38,9 +38,8 @@     return -1;
   }
 }
 
-Ppg::Ppg(float spl)
-  : offset {spl},
-    hpf {0.87033078, -1.74066156, 0.87033078, -1.72377617, 0.75754694},
+Ppg::Ppg()
+  : hpf {0.87033078, -1.74066156, 0.87033078, -1.72377617, 0.75754694},
     agc {20, 0.971, 2},
     lpf {0.11595249, 0.23190498, 0.11595249, -0.72168143, 0.18549138} {
 }
@@ -67,13 +66,7 @@   auto hr = ProcessHeartRate();
   dataIndex = 0;
   return hr;
 }
-
-int cccount = 0;
 float Ppg::ProcessHeartRate() {
-
-  if (cccount > 2)
-    asm("nop");
-  cccount++;
   auto t0 = Trough(data.data(), dataIndex, 7, 48);
   if (t0 < 0)
     return 0;




diff --git a/src/components/heartrate/Ppg.h b/src/components/heartrate/Ppg.h
index ee07dfcfc027ae34b08c8c609b8da1d11c44142b..000141624f9277f55eb7e84165ba5510793d3d80 100644
--- a/src/components/heartrate/Ppg.h
+++ b/src/components/heartrate/Ppg.h
@@ -8,8 +8,7 @@ namespace Pinetime {
   namespace Controllers {
     class Ppg {
     public:
-      explicit Ppg(float spl);
-
+      Ppg();
       int8_t Preprocess(float spl);
       float HeartRate();
 




diff --git a/src/components/timer/TimerController.cpp b/src/components/timer/TimerController.cpp
index 3b25901c7ecf6879c8cd72de9e2fd6b10801081a..8d5f5c33b7d625852ce9c2296536358513590de3 100644
--- a/src/components/timer/TimerController.cpp
+++ b/src/components/timer/TimerController.cpp
@@ -12,14 +12,17 @@
 
 APP_TIMER_DEF(timerAppTimer);
 
-
-TimerController::TimerController(System::SystemTask& systemTask) : systemTask{systemTask} {
+namespace {
+  void TimerEnd(void* p_context) {
+    auto* controller = static_cast<Pinetime::Controllers::TimerController*> (p_context);
+    if(controller != nullptr)
+      controller->OnTimerEnd();
+  }
 }
 
 
 void TimerController::Init() {
-  app_timer_create(&timerAppTimer, APP_TIMER_MODE_SINGLE_SHOT, timerEnd);
-  
+  app_timer_create(&timerAppTimer, APP_TIMER_MODE_SINGLE_SHOT, TimerEnd);
 }
 
 void TimerController::StartTimer(uint32_t duration) {
@@ -47,13 +50,6 @@   
   return (static_cast<TickType_t>(deltaTicks) / static_cast<TickType_t>(configTICK_RATE_HZ)) * 1000;
 }
 
-void TimerController::timerEnd(void* p_context) {
-  
-  auto* controller = static_cast<Controllers::TimerController*> (p_context);
-  controller->timerRunning = false;
-  controller->systemTask.PushMessage(System::SystemTask::Messages::OnTimerDone);
-}
-
 void TimerController::StopTimer() {
   app_timer_stop(timerAppTimer);
   timerRunning = false;
@@ -61,4 +57,13 @@ }
 
 bool TimerController::IsRunning() {
   return timerRunning;
-}
\ No newline at end of file
+}
+void TimerController::OnTimerEnd() {
+  timerRunning = false;
+  if(systemTask != nullptr)
+    systemTask->PushMessage(System::Messages::OnTimerDone);
+}
+
+void TimerController::Register(Pinetime::System::SystemTask* systemTask) {
+  this->systemTask = systemTask;
+}




diff --git a/src/components/timer/TimerController.h b/src/components/timer/TimerController.h
index 5a0b293ee52d853e38dbac6a0795a5c99b0d7dc0..fa7bc90dda242da673d35ccda79460789170a51d 100644
--- a/src/components/timer/TimerController.h
+++ b/src/components/timer/TimerController.h
@@ -12,7 +12,7 @@   namespace Controllers {
     
     class TimerController {
     public:
-      TimerController(Pinetime::System::SystemTask& systemTask);
+      TimerController() = default;
       
       void Init();
       
@@ -23,12 +23,13 @@       
       uint32_t GetTimeRemaining();
       
       bool IsRunning();
-    
+
+      void OnTimerEnd();
+
+      void Register(System::SystemTask* systemTask);
+
     private:
-      System::SystemTask& systemTask;
-      
-      static void timerEnd(void* p_context);
-  
+      System::SystemTask* systemTask = nullptr;
       TickType_t endTicks;
       bool timerRunning = false;
     };




diff --git a/src/displayapp/DisplayApp.cpp b/src/displayapp/DisplayApp.cpp
index 7b03d569c4020f03e06ab3c3351f3518f1609263..05f171be31cf08ae6cac7bc6aa3bcc732d2879da 100644
--- a/src/displayapp/DisplayApp.cpp
+++ b/src/displayapp/DisplayApp.cpp
@@ -32,6 +32,7 @@ #include "drivers/Cst816s.h"
 #include "drivers/St7789.h"
 #include "drivers/Watchdog.h"
 #include "systemtask/SystemTask.h"
+#include "systemtask/Messages.h"
 
 #include "displayapp/screens/settings/QuickSettings.h"
 #include "displayapp/screens/settings/Settings.h"
@@ -44,6 +45,12 @@
 using namespace Pinetime::Applications;
 using namespace Pinetime::Applications::Display;
 
+namespace {
+  static inline bool in_isr(void) {
+    return (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) != 0;
+  }
+}
+
 DisplayApp::DisplayApp(Drivers::St7789& lcd,
                        Components::LittleVgl& lvgl,
                        Drivers::Cst816S& touchPanel,
@@ -51,7 +58,6 @@                        Controllers::Battery& batteryController,
                        Controllers::Ble& bleController,
                        Controllers::DateTime& dateTimeController,
                        Drivers::WatchdogView& watchdog,
-                       System::SystemTask& systemTask,
                        Pinetime::Controllers::NotificationManager& notificationManager,
                        Pinetime::Controllers::HeartRateController& heartRateController,
                        Controllers::Settings& settingsController,
@@ -65,19 +71,20 @@     batteryController {batteryController},
     bleController {bleController},
     dateTimeController {dateTimeController},
     watchdog {watchdog},
-    systemTask {systemTask},
     notificationManager {notificationManager},
     heartRateController {heartRateController},
     settingsController {settingsController},
     motorController {motorController},
     motionController {motionController},
     timerController {timerController} {
+}
+
+void DisplayApp::Start() {
   msgQueue = xQueueCreate(queueSize, itemSize);
+
   // Start clock when smartwatch boots
   LoadApp(Apps::Clock, DisplayApp::FullRefreshDirections::None);
-}
 
-void DisplayApp::Start() {
   if (pdPASS != xTaskCreate(DisplayApp::Process, "displayapp", 800, this, 0, &taskHandle)) {
     APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
   }
@@ -130,7 +137,7 @@           brightnessController.Lower();
           vTaskDelay(100);
         }
         lcd.DisplayOff();
-        systemTask.PushMessage(System::SystemTask::Messages::OnDisplayTaskSleeping);
+        PushMessageToSystemTask(Pinetime::System::Messages::OnDisplayTaskSleeping);
         state = States::Idle;
         break;
       case Messages::GoToRunning:
@@ -139,7 +146,7 @@         brightnessController.Restore();
         state = States::Running;
         break;
       case Messages::UpdateTimeOut:
-        systemTask.PushMessage(System::SystemTask::Messages::UpdateTimeOut);
+        PushMessageToSystemTask(System::Messages::UpdateTimeOut);
         break;
       case Messages::UpdateBleConnection:
         //        clockScreen.SetBleConnectionState(bleController.IsConnected() ? Screens::Clock::BleConnectionStates::Connected :
@@ -176,7 +183,7 @@               case TouchEvents::SwipeRight:
                 LoadApp(Apps::QuickSettings, DisplayApp::FullRefreshDirections::RightAnim);
                 break;
               case TouchEvents::DoubleTap:
-                systemTask.PushMessage(System::SystemTask::Messages::GoToSleep);
+                PushMessageToSystemTask(System::Messages::GoToSleep);
                 break;
               default:
                 break;
@@ -188,7 +195,7 @@         }
       } break;
       case Messages::ButtonPushed:
         if (currentApp == Apps::Clock) {
-          systemTask.PushMessage(System::SystemTask::Messages::GoToSleep);
+          PushMessageToSystemTask(System::Messages::GoToSleep);
         } else {
           if (!currentScreen->OnButtonPushed()) {
             LoadApp(returnToApp, returnDirection);
@@ -206,6 +213,11 @@         break;
     }
   }
 
+  if(nextApp != Apps::None) {
+    LoadApp(nextApp, nextDirection);
+    nextApp = Apps::None;
+  }
+
   if (state != States::Idle && touchMode == TouchModes::Polling) {
     auto info = touchPanel.GetTouchInfo();
     if (info.action == 2) { // 2 = contact
@@ -224,7 +236,8 @@   lv_task_handler();
 }
 
 void DisplayApp::StartApp(Apps app, DisplayApp::FullRefreshDirections direction) {
-  LoadApp(app, direction);
+  nextApp = app;
+  nextDirection = direction;
 }
 
 void DisplayApp::ReturnApp(Apps app, DisplayApp::FullRefreshDirections direction, TouchEvents touchEvent) {
@@ -267,12 +280,12 @@       break;
 
     case Apps::Notifications:
       currentScreen = std::make_unique<Screens::Notifications>(
-        this, notificationManager, systemTask.nimble().alertService(), Screens::Notifications::Modes::Normal);
+        this, notificationManager, systemTask->nimble().alertService(), Screens::Notifications::Modes::Normal);
       ReturnApp(Apps::Clock, FullRefreshDirections::Up, TouchEvents::SwipeUp);
       break;
     case Apps::NotificationsPreview:
       currentScreen = std::make_unique<Screens::Notifications>(
-        this, notificationManager, systemTask.nimble().alertService(), Screens::Notifications::Modes::Preview);
+        this, notificationManager, systemTask->nimble().alertService(), Screens::Notifications::Modes::Preview);
       ReturnApp(Apps::Clock, FullRefreshDirections::Up, TouchEvents::SwipeUp);
       break;
     case Apps::Timer:
@@ -321,7 +334,7 @@       break;
       //
 
     case Apps::FlashLight:
-      currentScreen = std::make_unique<Screens::FlashLight>(this, systemTask, brightnessController);
+      currentScreen = std::make_unique<Screens::FlashLight>(this, *systemTask, brightnessController);
       ReturnApp(Apps::Clock, FullRefreshDirections::Down, TouchEvents::None);
       break;
     case Apps::StopWatch:
@@ -337,13 +350,13 @@     case Apps::Paddle:
       currentScreen = std::make_unique<Screens::Paddle>(this, lvgl);
       break;
     case Apps::Music:
-      currentScreen = std::make_unique<Screens::Music>(this, systemTask.nimble().music());
+      currentScreen = std::make_unique<Screens::Music>(this, systemTask->nimble().music());
       break;
     case Apps::Navigation:
-      currentScreen = std::make_unique<Screens::Navigation>(this, systemTask.nimble().navigation());
+      currentScreen = std::make_unique<Screens::Navigation>(this, systemTask->nimble().navigation());
       break;
     case Apps::HeartRate:
-      currentScreen = std::make_unique<Screens::HeartRate>(this, heartRateController, systemTask);
+      currentScreen = std::make_unique<Screens::HeartRate>(this, heartRateController, *systemTask);
       break;
     case Apps::Motion:
       currentScreen = std::make_unique<Screens::Motion>(this, motionController);
@@ -359,12 +372,15 @@ void DisplayApp::IdleState() {
 }
 
 void DisplayApp::PushMessage(Messages msg) {
-  BaseType_t xHigherPriorityTaskWoken;
-  xHigherPriorityTaskWoken = pdFALSE;
-  xQueueSendFromISR(msgQueue, &msg, &xHigherPriorityTaskWoken);
-  if (xHigherPriorityTaskWoken) {
-    /* Actual macro used here is port specific. */
-    // TODO : should I do something here?
+  if(in_isr()) {
+    BaseType_t xHigherPriorityTaskWoken;
+    xHigherPriorityTaskWoken = pdFALSE;
+    xQueueSendFromISR(msgQueue, &msg, &xHigherPriorityTaskWoken);
+    if (xHigherPriorityTaskWoken) {
+      portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    }
+  } else {
+    xQueueSend(msgQueue, &msg, portMAX_DELAY);
   }
 }
 
@@ -425,3 +441,12 @@
 void DisplayApp::SetTouchMode(DisplayApp::TouchModes mode) {
   touchMode = mode;
 }
+
+void DisplayApp::PushMessageToSystemTask(Pinetime::System::Messages message) {
+  if(systemTask != nullptr)
+    systemTask->PushMessage(message);
+}
+
+void DisplayApp::Register(Pinetime::System::SystemTask* systemTask) {
+  this->systemTask = systemTask;
+}




diff --git a/src/displayapp/DisplayApp.h b/src/displayapp/DisplayApp.h
index 0c7bd21622f9b11ddcd2e70ae45327b18059703f..73a7cc36a4335cf0ba82f74c0f32775baaba8a49 100644
--- a/src/displayapp/DisplayApp.h
+++ b/src/displayapp/DisplayApp.h
@@ -4,6 +4,7 @@ #include 
 #include <queue.h>
 #include <task.h>
 #include <memory>
+#include <systemtask/Messages.h>
 #include "Apps.h"
 #include "LittleVgl.h"
 #include "TouchEvents.h"
@@ -49,7 +50,6 @@                  Controllers::Battery& batteryController,
                  Controllers::Ble& bleController,
                  Controllers::DateTime& dateTimeController,
                  Drivers::WatchdogView& watchdog,
-                 System::SystemTask& systemTask,
                  Pinetime::Controllers::NotificationManager& notificationManager,
                  Pinetime::Controllers::HeartRateController& heartRateController,
                  Controllers::Settings& settingsController,
@@ -64,6 +64,8 @@
       void SetFullRefresh(FullRefreshDirections direction);
       void SetTouchMode(TouchModes mode);
 
+      void Register(Pinetime::System::SystemTask* systemTask);
+
     private:
       Pinetime::Drivers::St7789& lcd;
       Pinetime::Components::LittleVgl& lvgl;
@@ -72,7 +74,7 @@       Pinetime::Controllers::Battery& batteryController;
       Pinetime::Controllers::Ble& bleController;
       Pinetime::Controllers::DateTime& dateTimeController;
       Pinetime::Drivers::WatchdogView& watchdog;
-      Pinetime::System::SystemTask& systemTask;
+      Pinetime::System::SystemTask* systemTask = nullptr;
       Pinetime::Controllers::NotificationManager& notificationManager;
       Pinetime::Controllers::HeartRateController& heartRateController;
       Pinetime::Controllers::Settings& settingsController;
@@ -108,6 +110,10 @@       void InitHw();
       void Refresh();
       void ReturnApp(Apps app, DisplayApp::FullRefreshDirections direction, TouchEvents touchEvent);
       void LoadApp(Apps app, DisplayApp::FullRefreshDirections direction);
+      void PushMessageToSystemTask(Pinetime::System::Messages message);
+
+      Apps nextApp = Apps::None;
+      DisplayApp::FullRefreshDirections nextDirection;
     };
   }
 }




diff --git a/src/displayapp/DisplayAppRecovery.cpp b/src/displayapp/DisplayAppRecovery.cpp
index 856eafd82a64cf8d456bdd72f520b9f310ad8131..fd517b11a0fe44826b2a062aacaf0e788cf5b61b 100644
--- a/src/displayapp/DisplayAppRecovery.cpp
+++ b/src/displayapp/DisplayAppRecovery.cpp
@@ -14,7 +14,6 @@                        Controllers::Battery& batteryController,
                        Controllers::Ble& bleController,
                        Controllers::DateTime& dateTimeController,
                        Drivers::WatchdogView& watchdog,
-                       System::SystemTask& systemTask,
                        Pinetime::Controllers::NotificationManager& notificationManager,
                        Pinetime::Controllers::HeartRateController& heartRateController,
                        Controllers::Settings& settingsController,
@@ -22,10 +21,11 @@                        Pinetime::Controllers::MotorController& motorController,
                        Pinetime::Controllers::MotionController& motionController,
                        Pinetime::Controllers::TimerController& timerController)
   : lcd {lcd}, bleController {bleController} {
-  msgQueue = xQueueCreate(queueSize, itemSize);
+
 }
 
 void DisplayApp::Start() {
+  msgQueue = xQueueCreate(queueSize, itemSize);
   if (pdPASS != xTaskCreate(DisplayApp::Process, "displayapp", 512, this, 0, &taskHandle))
     APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
 }
@@ -113,4 +113,8 @@   if (xHigherPriorityTaskWoken) {
     /* Actual macro used here is port specific. */
     // TODO : should I do something here?
   }
-}
\ No newline at end of file
+}
+
+void DisplayApp::Register(Pinetime::System::SystemTask* systemTask) {
+
+}




diff --git a/src/displayapp/DisplayAppRecovery.h b/src/displayapp/DisplayAppRecovery.h
index 2c5a36fbcefad1c1d6bafb2aa2f55bcd38e7089a..638c0071dadd7c4fe7277b9cae75ec9a24815f9a 100644
--- a/src/displayapp/DisplayAppRecovery.h
+++ b/src/displayapp/DisplayAppRecovery.h
@@ -39,7 +39,6 @@                  Controllers::Battery& batteryController,
                  Controllers::Ble& bleController,
                  Controllers::DateTime& dateTimeController,
                  Drivers::WatchdogView& watchdog,
-                 System::SystemTask& systemTask,
                  Pinetime::Controllers::NotificationManager& notificationManager,
                  Pinetime::Controllers::HeartRateController& heartRateController,
                  Controllers::Settings& settingsController,
@@ -48,6 +47,7 @@                  Pinetime::Controllers::MotionController& motionController,
                  Pinetime::Controllers::TimerController& timerController);
       void Start();
       void PushMessage(Pinetime::Applications::Display::Messages msg);
+      void Register(Pinetime::System::SystemTask* systemTask);
 
     private:
       TaskHandle_t taskHandle;




diff --git a/src/displayapp/LittleVgl.cpp b/src/displayapp/LittleVgl.cpp
index 36df51b4f56cd393f456eb731caa61b039ca33c6..c069afa2fd949a76239ec7ce7356bdef056ce525 100644
--- a/src/displayapp/LittleVgl.cpp
+++ b/src/displayapp/LittleVgl.cpp
@@ -23,6 +23,10 @@ }
 
 LittleVgl::LittleVgl(Pinetime::Drivers::St7789& lcd, Pinetime::Drivers::Cst816S& touchPanel)
   : lcd {lcd}, touchPanel {touchPanel}, previousClick {0, 0} {
+
+}
+
+void LittleVgl::Init() {
   lv_init();
   InitTheme();
   InitDisplay();




diff --git a/src/displayapp/LittleVgl.h b/src/displayapp/LittleVgl.h
index 7f7b76e0d7ce2e14ae8713cdfc8b49beea16a927..41f934a751bb4a5397427945257a70c4ce2f138f 100644
--- a/src/displayapp/LittleVgl.h
+++ b/src/displayapp/LittleVgl.h
@@ -19,6 +19,8 @@       LittleVgl& operator=(const LittleVgl&) = delete;
       LittleVgl(LittleVgl&&) = delete;
       LittleVgl& operator=(LittleVgl&&) = delete;
 
+      void Init();
+
       void FlushDisplay(const lv_area_t* area, lv_color_t* color_p);
       bool GetTouchPadInfo(lv_indev_data_t* ptr);
       void SetFullRefresh(FullRefreshDirections direction);




diff --git a/src/displayapp/screens/FlashLight.cpp b/src/displayapp/screens/FlashLight.cpp
index 4568db40327eb0fd050e4442ab898c97a9b5d987..7db2c6c8f0f377ed9d6fbbbddccadaa35f8d916d 100644
--- a/src/displayapp/screens/FlashLight.cpp
+++ b/src/displayapp/screens/FlashLight.cpp
@@ -39,14 +39,14 @@   lv_obj_set_click(backgroundAction, true);
   backgroundAction->user_data = this;
   lv_obj_set_event_cb(backgroundAction, event_handler);
 
-  systemTask.PushMessage(Pinetime::System::SystemTask::Messages::DisableSleeping);
+  systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
 }
 
 FlashLight::~FlashLight() {
   lv_obj_clean(lv_scr_act());
   lv_obj_set_style_local_bg_color(lv_scr_act(), LV_OBJ_PART_MAIN, LV_STATE_DEFAULT, lv_color_hex(0x000000));
   brightness.Restore();
-  systemTask.PushMessage(Pinetime::System::SystemTask::Messages::EnableSleeping);
+  systemTask.PushMessage(Pinetime::System::Messages::EnableSleeping);
 }
 
 void FlashLight::OnClickEvent(lv_obj_t* obj, lv_event_t event) {




diff --git a/src/displayapp/screens/HeartRate.cpp b/src/displayapp/screens/HeartRate.cpp
index 90f6bc26befd4acc9377aed91744b8c67689855c..5689b63e42190710ac29c67497c48ebb09669e0d 100644
--- a/src/displayapp/screens/HeartRate.cpp
+++ b/src/displayapp/screens/HeartRate.cpp
@@ -63,12 +63,12 @@
   label_startStop = lv_label_create(btn_startStop, nullptr);
   UpdateStartStopButton(isHrRunning);
   if (isHrRunning)
-    systemTask.PushMessage(Pinetime::System::SystemTask::Messages::DisableSleeping);
+    systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
 }
 
 HeartRate::~HeartRate() {
   lv_obj_clean(lv_scr_act());
-  systemTask.PushMessage(Pinetime::System::SystemTask::Messages::EnableSleeping);
+  systemTask.PushMessage(Pinetime::System::Messages::EnableSleeping);
 }
 
 bool HeartRate::Refresh() {
@@ -95,12 +95,12 @@   if (event == LV_EVENT_CLICKED) {
     if (heartRateController.State() == Controllers::HeartRateController::States::Stopped) {
       heartRateController.Start();
       UpdateStartStopButton(heartRateController.State() != Controllers::HeartRateController::States::Stopped);
-      systemTask.PushMessage(Pinetime::System::SystemTask::Messages::DisableSleeping);
+      systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
       lv_obj_set_style_local_text_color(label_hr, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, LV_COLOR_GREEN);
     } else {
       heartRateController.Stop();
       UpdateStartStopButton(heartRateController.State() != Controllers::HeartRateController::States::Stopped);
-      systemTask.PushMessage(Pinetime::System::SystemTask::Messages::EnableSleeping);
+      systemTask.PushMessage(Pinetime::System::Messages::EnableSleeping);
       lv_obj_set_style_local_text_color(label_hr, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, LV_COLOR_GRAY);
     }
   }




diff --git a/src/displayapp/screens/ScreenList.h b/src/displayapp/screens/ScreenList.h
index 73ea461010973c09e125782afc63d964fb1e0965..ea66bfb2374a6d85a18bf072bc595f8d41e9aa00 100644
--- a/src/displayapp/screens/ScreenList.h
+++ b/src/displayapp/screens/ScreenList.h
@@ -15,11 +15,16 @@       template  class ScreenList : public Screen {
       public:
         ScreenList(DisplayApp* app,
                    uint8_t initScreen,
-                   std::array<std::function<std::unique_ptr<Screen>()>, N>&& screens,
+                   const std::array<std::function<std::unique_ptr<Screen>()>, N>&& screens,
                    ScreenListModes mode)
-          : Screen(app), initScreen {initScreen}, screens {std::move(screens)}, mode {mode}, current {this->screens[initScreen]()} {
-          screenIndex = initScreen;
+          : Screen(app), initScreen {initScreen}, screens {std::move(screens)}, mode {mode}, screenIndex{initScreen}, current {this->screens[initScreen]()} {
+
         }
+
+        ScreenList(const ScreenList&) = delete;
+        ScreenList& operator=(const ScreenList&) = delete;
+        ScreenList(ScreenList&&) = delete;
+        ScreenList& operator=(ScreenList&&) = delete;
 
         ~ScreenList() override {
           lv_obj_clean(lv_scr_act());
@@ -97,7 +102,7 @@         }
 
       private:
         uint8_t initScreen = 0;
-        std::array<std::function<std::unique_ptr<Screen>()>, N> screens;
+        const std::array<std::function<std::unique_ptr<Screen>()>, N> screens;
         ScreenListModes mode = ScreenListModes::UpDown;
 
         uint8_t screenIndex = 0;




diff --git a/src/displayapp/screens/SystemInfo.cpp b/src/displayapp/screens/SystemInfo.cpp
index f61d2ff1aaf0411ac0b0ac4e36848ba12d701053..a7387dac764c1ad5184409157e6d1a863c70ce01 100644
--- a/src/displayapp/screens/SystemInfo.cpp
+++ b/src/displayapp/screens/SystemInfo.cpp
@@ -81,7 +81,7 @@                         __DATE__,
                         __TIME__);
   lv_label_set_align(label, LV_LABEL_ALIGN_CENTER);
   lv_obj_align(label, lv_scr_act(), LV_ALIGN_CENTER, 0, 0);
-  return std::unique_ptr<Screen>(new Screens::Label(0, 5, app, label));
+  return std::make_unique<Screens::Label>(0, 5, app, label);
 }
 
 std::unique_ptr<Screen> SystemInfo::CreateScreen2() {
@@ -161,7 +161,7 @@                         batteryVoltageBytes[0],
                         brightnessController.ToString(),
                         resetReason);
   lv_obj_align(label, lv_scr_act(), LV_ALIGN_CENTER, 0, 0);
-  return std::unique_ptr<Screen>(new Screens::Label(1, 4, app, label));
+  return std::make_unique<Screens::Label>(1, 4, app, label);
 }
 
 std::unique_ptr<Screen> SystemInfo::CreateScreen3() {
@@ -195,7 +195,7 @@                         mon.frag_pct,
                         (int) mon.free_biggest_size,
                         0);
   lv_obj_align(label, lv_scr_act(), LV_ALIGN_CENTER, 0, 0);
-  return std::unique_ptr<Screen>(new Screens::Label(2, 5, app, label));
+  return std::make_unique<Screens::Label>(2, 5, app, label);
 }
 
 bool sortById(const TaskStatus_t& lhs, const TaskStatus_t& rhs) {
@@ -229,7 +229,7 @@     } else {
       lv_table_set_cell_value(infoTask, i + 1, 2, std::to_string(tasksStatus[i].usStackHighWaterMark).c_str());
     }
   }
-  return std::unique_ptr<Screen>(new Screens::Label(3, 5, app, infoTask));
+  return std::make_unique<Screens::Label>(3, 5, app, infoTask);
 }
 
 std::unique_ptr<Screen> SystemInfo::CreateScreen5() {
@@ -245,5 +245,5 @@                            "#FFFF00 https://github.com/#\n"
                            "#FFFF00 JF002/InfiniTime#");
   lv_label_set_align(label, LV_LABEL_ALIGN_CENTER);
   lv_obj_align(label, lv_scr_act(), LV_ALIGN_CENTER, 0, 0);
-  return std::unique_ptr<Screen>(new Screens::Label(4, 5, app, label));
+  return std::make_unique<Screens::Label>(4, 5, app, label);
 }




diff --git a/src/displayapp/screens/settings/QuickSettings.cpp b/src/displayapp/screens/settings/QuickSettings.cpp
index 3994794dd3a777bacc047f7d93adc8943ca8621e..5db7468c0ff5a59cf1dec7dd6e419c2e272d3040 100644
--- a/src/displayapp/screens/settings/QuickSettings.cpp
+++ b/src/displayapp/screens/settings/QuickSettings.cpp
@@ -7,12 +7,12 @@ using namespace Pinetime::Applications::Screens;
 
 namespace {
   static void ButtonEventHandler(lv_obj_t* obj, lv_event_t event) {
-    QuickSettings* screen = static_cast<QuickSettings*>(obj->user_data);
+    auto* screen = static_cast<QuickSettings*>(obj->user_data);
     screen->OnButtonEvent(obj, event);
   }
 
   static void lv_update_task(struct _lv_task_t* task) {
-    auto user_data = static_cast<QuickSettings*>(task->user_data);
+    auto* user_data = static_cast<QuickSettings*>(task->user_data);
     user_data->UpdateScreen();
   }
 }




diff --git a/src/displayapp/screens/settings/Settings.cpp b/src/displayapp/screens/settings/Settings.cpp
index 2c72c832ca99b7398f3247d12f7074eab0a5ec20..e63a35846e7f7c1eece2498456561de9fd831517 100644
--- a/src/displayapp/screens/settings/Settings.cpp
+++ b/src/displayapp/screens/settings/Settings.cpp
@@ -46,7 +46,7 @@     {Symbols::clock, "Time format", Apps::SettingTimeFormat},
     {Symbols::clock, "Watch face", Apps::SettingWatchFace},
   }};
 
-  return std::unique_ptr<Screen>(new Screens::List(0, 2, app, settingsController, applications));
+  return std::make_unique<Screens::List>(0, 2, app, settingsController, applications);
 }
 
 std::unique_ptr<Screen> Settings::CreateScreen2() {
@@ -58,5 +58,5 @@     {Symbols::check, "Firmware", Apps::FirmwareValidation},
     {Symbols::list, "About", Apps::SysInfo},
   }};
 
-  return std::unique_ptr<Screen>(new Screens::List(1, 2, app, settingsController, applications));
+  return std::make_unique<Screens::List>(1, 2, app, settingsController, applications);
 }




diff --git a/src/displayapp/screens/settings/Settings.h b/src/displayapp/screens/settings/Settings.h
index 7e332dfe66d38c89b039dce5e80907f92c334616..711a6be6d0acaee16c63c0fb85092fdf09122867 100644
--- a/src/displayapp/screens/settings/Settings.h
+++ b/src/displayapp/screens/settings/Settings.h
@@ -16,7 +16,6 @@         ~Settings() override;
 
         bool Refresh() override;
 
-        void OnButtonEvent(lv_obj_t* object, lv_event_t event);
         bool OnTouchEvent(Pinetime::Applications::TouchEvents event) override;
 
       private:




diff --git a/src/drivers/Bma421.cpp b/src/drivers/Bma421.cpp
index 925b66c7f917f4965c50ce420759c81c290d9c18..35b2c1057f7495efbb4f0f380d5a0f046ba43024 100644
--- a/src/drivers/Bma421.cpp
+++ b/src/drivers/Bma421.cpp
@@ -103,8 +103,6 @@
   uint8_t activity = 0;
   bma423_activity_output(&activity, &bma);
 
-  NRF_LOG_INFO("MOTION : %d - %d/%d/%d", steps, data.x, data.y, data.z);
-
   // X and Y axis are swapped because of the way the sensor is mounted in the PineTime
   return {steps, data.y, data.x, data.z};
 }




diff --git a/src/drivers/SpiMaster.cpp b/src/drivers/SpiMaster.cpp
index 34fcc08a8585f4b6f5affd0ee1719e0f9404d0c4..0d5bb83c1602366b7fbf5bc0755a460c7eb2d2e6 100644
--- a/src/drivers/SpiMaster.cpp
+++ b/src/drivers/SpiMaster.cpp
@@ -7,11 +7,14 @@
 using namespace Pinetime::Drivers;
 
 SpiMaster::SpiMaster(const SpiMaster::SpiModule spi, const SpiMaster::Parameters& params) : spi {spi}, params {params} {
-  mutex = xSemaphoreCreateBinary();
-  ASSERT(mutex != NULL);
 }
 
 bool SpiMaster::Init() {
+  if(mutex == nullptr) {
+    mutex = xSemaphoreCreateBinary();
+    ASSERT(mutex != nullptr);
+  }
+
   /* Configure GPIO pins used for pselsck, pselmosi, pselmiso and pselss for SPI0 */
   nrf_gpio_pin_set(params.pinSCK);
   nrf_gpio_cfg_output(params.pinSCK);
@@ -132,17 +135,17 @@     currentBufferSize -= currentSize;
 
     spiBaseAddress->TASKS_START = 1;
   } else {
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
     if (taskToNotify != nullptr) {
-      BaseType_t xHigherPriorityTaskWoken = pdFALSE;
       vTaskNotifyGiveFromISR(taskToNotify, &xHigherPriorityTaskWoken);
       portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
     }
 
     nrf_gpio_pin_set(this->pinCsn);
     currentBufferAddr = 0;
-    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-    xSemaphoreGiveFromISR(mutex, &xHigherPriorityTaskWoken);
-    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    BaseType_t xHigherPriorityTaskWoken2 = pdFALSE;
+    xSemaphoreGiveFromISR(mutex, &xHigherPriorityTaskWoken2);
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken | xHigherPriorityTaskWoken2);
   }
 }
 




diff --git a/src/drivers/SpiMaster.h b/src/drivers/SpiMaster.h
index dfc195b7dfd3bea3583f98d0e19849c97e98ad12..5ea624f2553070e6378d4830089b3167e57e3d73 100644
--- a/src/drivers/SpiMaster.h
+++ b/src/drivers/SpiMaster.h
@@ -10,7 +10,6 @@ namespace Pinetime {
   namespace Drivers {
     class SpiMaster {
     public:
-      ;
       enum class SpiModule : uint8_t { SPI0, SPI1 };
       enum class BitOrder : uint8_t { Msb_Lsb, Lsb_Msb };
       enum class Modes : uint8_t { Mode0, Mode1, Mode2, Mode3 };
@@ -60,7 +59,7 @@
       volatile uint32_t currentBufferAddr = 0;
       volatile size_t currentBufferSize = 0;
       volatile TaskHandle_t taskToNotify;
-      SemaphoreHandle_t mutex;
+      SemaphoreHandle_t mutex = nullptr;
     };
   }
 }




diff --git a/src/drivers/TwiMaster.cpp b/src/drivers/TwiMaster.cpp
index 7b6582dd2e41ebb4ad38e8293b8a67128dd9559a..fc9edf810fd53ac795ca80f0623066b65c40e146 100644
--- a/src/drivers/TwiMaster.cpp
+++ b/src/drivers/TwiMaster.cpp
@@ -9,10 +9,12 @@ // TODO use shortcut to automatically send STOP when receive LastTX, for example
 // TODO use DMA/IRQ
 
 TwiMaster::TwiMaster(const Modules module, const Parameters& params) : module {module}, params {params} {
-  mutex = xSemaphoreCreateBinary();
 }
 
 void TwiMaster::Init() {
+  if(mutex == nullptr)
+    mutex = xSemaphoreCreateBinary();
+  
   NRF_GPIO->PIN_CNF[params.pinScl] =
     ((uint32_t) GPIO_PIN_CNF_DIR_Input << GPIO_PIN_CNF_DIR_Pos) | ((uint32_t) GPIO_PIN_CNF_INPUT_Connect << GPIO_PIN_CNF_INPUT_Pos) |
     ((uint32_t) GPIO_PIN_CNF_PULL_Pullup << GPIO_PIN_CNF_PULL_Pos) | ((uint32_t) GPIO_PIN_CNF_DRIVE_S0D1 << GPIO_PIN_CNF_DRIVE_Pos) |




diff --git a/src/drivers/TwiMaster.h b/src/drivers/TwiMaster.h
index 1c0648a25b86d93a6eba2dbeb606560bd0f6ade2..6175b99b443d9b7195825d8349c62bde8a6d42d2 100644
--- a/src/drivers/TwiMaster.h
+++ b/src/drivers/TwiMaster.h
@@ -31,7 +31,7 @@       ErrorCodes Read(uint8_t deviceAddress, uint8_t* buffer, size_t size, bool stop);
       ErrorCodes Write(uint8_t deviceAddress, const uint8_t* data, size_t size, bool stop);
       void FixHwFreezed();
       NRF_TWIM_Type* twiBaseAddress;
-      SemaphoreHandle_t mutex;
+      SemaphoreHandle_t mutex = nullptr;
       const Modules module;
       const Parameters params;
       static constexpr uint8_t maxDataSize {16};




diff --git a/src/heartratetask/HeartRateTask.cpp b/src/heartratetask/HeartRateTask.cpp
index 36c8cc1806e78dd25a763589586449d9980d6eef..fddc05d700f23e32fc3b5e80c69978f1be816b4d 100644
--- a/src/heartratetask/HeartRateTask.cpp
+++ b/src/heartratetask/HeartRateTask.cpp
@@ -6,12 +6,13 @@
 using namespace Pinetime::Applications;
 
 HeartRateTask::HeartRateTask(Drivers::Hrs3300& heartRateSensor, Controllers::HeartRateController& controller)
-  : heartRateSensor {heartRateSensor}, controller {controller}, ppg {static_cast<float>(heartRateSensor.ReadHrs())} {
+  : heartRateSensor {heartRateSensor}, controller {controller}, ppg{} {
+}
+
+void HeartRateTask::Start() {
   messageQueue = xQueueCreate(10, 1);
   controller.SetHeartRateTask(this);
-}
 
-void HeartRateTask::Start() {
   if (pdPASS != xTaskCreate(HeartRateTask::Process, "Heartrate", 500, this, 0, &taskHandle))
     APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
 }




diff --git a/src/main.cpp b/src/main.cpp
index 61194b956de17645d8e837bc1b59e67f20334e33..4c2c5de89707c3fd3b44824f8f12df938c260d05 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -33,7 +33,7 @@ #include "components/ble/BleController.h"
 #include "components/ble/NotificationManager.h"
 #include "components/motor/MotorController.h"
 #include "components/datetime/DateTimeController.h"
-#include "components/settings/Settings.h"
+#include "components/heartrate/HeartRateController.h"
 #include "drivers/Spi.h"
 #include "drivers/SpiMaster.h"
 #include "drivers/SpiNorFlash.h"
@@ -49,8 +49,6 @@ #else
   #include "logging/DummyLogger.h"
 Pinetime::Logging::DummyLogger logger;
 #endif
-
-#include <memory>
 
 static constexpr uint8_t pinSpiSck = 2;
 static constexpr uint8_t pinSpiMosi = 3;
@@ -108,15 +106,59 @@ void ble_manager_set_ble_connection_callback(void (*connection)());
 void ble_manager_set_ble_disconnection_callback(void (*disconnection)());
 static constexpr uint8_t pinTouchIrq = 28;
 static constexpr uint8_t pinPowerPresentIrq = 19;
-std::unique_ptr<Pinetime::System::SystemTask> systemTask;
 
 Pinetime::Controllers::Settings settingsController {spiNorFlash};
 
 Pinetime::Controllers::MotorController motorController {settingsController};
 
+Pinetime::Controllers::HeartRateController heartRateController;
+Pinetime::Applications::HeartRateTask heartRateApp(heartRateSensor, heartRateController);
+
+Pinetime::Controllers::DateTime dateTimeController;
+Pinetime::Drivers::Watchdog watchdog;
+Pinetime::Drivers::WatchdogView watchdogView(watchdog);
+Pinetime::Controllers::NotificationManager notificationManager;
+Pinetime::Controllers::MotionController motionController;
+Pinetime::Controllers::TimerController timerController;
+
+Pinetime::Applications::DisplayApp displayApp(lcd,
+                                              lvgl,
+                                              touchPanel,
+                                              batteryController,
+                                              bleController,
+                                              dateTimeController,
+                                              watchdogView,
+                                              notificationManager,
+                                              heartRateController,
+                                              settingsController,
+                                              motorController,
+                                              motionController,
+                                              timerController);
+
+Pinetime::System::SystemTask systemTask(spi,
+                                        lcd,
+                                        spiNorFlash,
+                                        twiMaster,
+                                        touchPanel,
+                                        lvgl,
+                                        batteryController,
+                                        bleController,
+                                        dateTimeController,
+                                        timerController,
+                                        watchdog,
+                                        notificationManager,
+                                        motorController,
+                                        heartRateSensor,
+                                        motionController,
+                                        motionSensor,
+                                        settingsController,
+                                        heartRateController,
+                                        displayApp,
+                                        heartRateApp);
+
 void nrfx_gpiote_evt_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_polarity_t action) {
   if (pin == pinTouchIrq) {
-    systemTask->OnTouchEvent();
+    systemTask.OnTouchEvent();
     return;
   }
 
@@ -141,12 +183,12 @@ }
 
 void DebounceTimerChargeCallback(TimerHandle_t xTimer) {
   xTimerStop(xTimer, 0);
-  systemTask->PushMessage(Pinetime::System::SystemTask::Messages::OnChargingEvent);
+  systemTask.PushMessage(Pinetime::System::Messages::OnChargingEvent);
 }
 
 void DebounceTimerCallback(TimerHandle_t xTimer) {
   xTimerStop(xTimer, 0);
-  systemTask->OnButtonPushed();
+  systemTask.OnButtonPushed();
 }
 
 void SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0_IRQHandler(void) {
@@ -264,19 +306,9 @@
   debounceTimer = xTimerCreate("debounceTimer", 200, pdFALSE, (void*) 0, DebounceTimerCallback);
   debounceChargeTimer = xTimerCreate("debounceTimerCharge", 200, pdFALSE, (void*) 0, DebounceTimerChargeCallback);
 
-  systemTask = std::make_unique<Pinetime::System::SystemTask>(spi,
-                                                              lcd,
-                                                              spiNorFlash,
-                                                              twiMaster,
-                                                              touchPanel,
-                                                              lvgl,
-                                                              batteryController,
-                                                              bleController,
-                                                              motorController,
-                                                              heartRateSensor,
-                                                              motionSensor,
-                                                              settingsController);
-  systemTask->Start();
+  lvgl.Init();
+
+  systemTask.Start();
   nimble_port_init();
 
   vTaskStartScheduler();




diff --git a/src/systemtask/Messages.h b/src/systemtask/Messages.h
new file mode 100644
index 0000000000000000000000000000000000000000..3a195e2d411b42b5037740d71f7878f5572f24a8
--- /dev/null
+++ b/src/systemtask/Messages.h
@@ -0,0 +1,26 @@
+#pragma once
+
+namespace Pinetime {
+  namespace System {
+      enum class Messages {
+        GoToSleep,
+        GoToRunning,
+        TouchWakeUp,
+        OnNewTime,
+        OnNewNotification,
+        OnTimerDone,
+        OnNewCall,
+        BleConnected,
+        UpdateTimeOut,
+        BleFirmwareUpdateStarted,
+        BleFirmwareUpdateFinished,
+        OnTouchEvent,
+        OnButtonEvent,
+        OnDisplayTaskSleeping,
+        EnableSleeping,
+        DisableSleeping,
+        OnNewDay,
+        OnChargingEvent
+      };
+    }
+}




diff --git a/src/systemtask/SystemTask.cpp b/src/systemtask/SystemTask.cpp
index 58377764031e5dd49ae08512be158a3e45772206..38e9793c06a9565ec399d7f497cc82709503feba 100644
--- a/src/systemtask/SystemTask.cpp
+++ b/src/systemtask/SystemTask.cpp
@@ -27,6 +27,12 @@ #include 
 
 using namespace Pinetime::System;
 
+namespace {
+  static inline bool in_isr(void) {
+    return (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) != 0;
+  }
+}
+
 void IdleTimerCallback(TimerHandle_t xTimer) {
 
   NRF_LOG_INFO("IdleTimerCallback");
@@ -42,10 +48,18 @@                        Drivers::Cst816S& touchPanel,
                        Components::LittleVgl& lvgl,
                        Controllers::Battery& batteryController,
                        Controllers::Ble& bleController,
+                       Controllers::DateTime& dateTimeController,
+                       Controllers::TimerController& timerController,
+                       Drivers::Watchdog& watchdog,
+                       Pinetime::Controllers::NotificationManager& notificationManager,
                        Pinetime::Controllers::MotorController& motorController,
                        Pinetime::Drivers::Hrs3300& heartRateSensor,
+                       Pinetime::Controllers::MotionController& motionController,
                        Pinetime::Drivers::Bma421& motionSensor,
-                       Controllers::Settings& settingsController)
+                       Controllers::Settings& settingsController,
+                       Pinetime::Controllers::HeartRateController& heartRateController,
+                       Pinetime::Applications::DisplayApp& displayApp,
+                       Pinetime::Applications::HeartRateTask& heartRateApp)
   : spi {spi},
     lcd {lcd},
     spiNorFlash {spiNorFlash},
@@ -53,21 +67,25 @@     twiMaster {twiMaster},
     touchPanel {touchPanel},
     lvgl {lvgl},
     batteryController {batteryController},
-    heartRateController {*this},
     bleController {bleController},
-    dateTimeController {*this},
-    timerController {*this},
-    watchdog {},
-    watchdogView {watchdog},
+    dateTimeController {dateTimeController},
+    timerController {timerController},
+    watchdog {watchdog},
+    notificationManager{notificationManager},
     motorController {motorController},
     heartRateSensor {heartRateSensor},
     motionSensor {motionSensor},
     settingsController {settingsController},
-    nimbleController(*this, bleController, dateTimeController, notificationManager, batteryController, spiNorFlash, heartRateController) {
-  systemTasksMsgQueue = xQueueCreate(10, 1);
+    heartRateController{heartRateController},
+    nimbleController(*this, bleController, dateTimeController, notificationManager, batteryController, spiNorFlash, heartRateController),
+    motionController{motionController},
+    displayApp{displayApp},
+    heartRateApp(heartRateApp) {
+
 }
 
 void SystemTask::Start() {
+  systemTasksMsgQueue = xQueueCreate(10, 1);
   if (pdPASS != xTaskCreate(SystemTask::Process, "MAIN", 350, this, 0, &taskHandle))
     APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
 }
@@ -96,9 +114,11 @@   lcd.Init();
 
   twiMaster.Init();
   touchPanel.Init();
+  dateTimeController.Register(this);
   batteryController.Init();
   motorController.Init();
   motionSensor.SoftReset();
+  timerController.Register(this);
   timerController.Init();
 
   // Reset the TWI device because the motion sensor chip most probably crashed it...
@@ -108,28 +128,14 @@
   motionSensor.Init();
   settingsController.Init();
 
-  displayApp = std::make_unique<Pinetime::Applications::DisplayApp>(lcd,
-                                                                    lvgl,
-                                                                    touchPanel,
-                                                                    batteryController,
-                                                                    bleController,
-                                                                    dateTimeController,
-                                                                    watchdogView,
-                                                                    *this,
-                                                                    notificationManager,
-                                                                    heartRateController,
-                                                                    settingsController,
-                                                                    motorController,
-                                                                    motionController,
-                                                                    timerController);
-  displayApp->Start();
+  displayApp.Register(this);
+  displayApp.Start();
 
-  displayApp->PushMessage(Pinetime::Applications::Display::Messages::UpdateBatteryLevel);
+  displayApp.PushMessage(Pinetime::Applications::Display::Messages::UpdateBatteryLevel);
 
   heartRateSensor.Init();
   heartRateSensor.Disable();
-  heartRateApp = std::make_unique<Pinetime::Applications::HeartRateTask>(heartRateSensor, heartRateController);
-  heartRateApp->Start();
+  heartRateApp.Start();
 
   nrf_gpio_cfg_sense_input(pinButton, (nrf_gpio_pin_pull_t) GPIO_PIN_CNF_PULL_Pulldown, (nrf_gpio_pin_sense_t) GPIO_PIN_CNF_SENSE_High);
   nrf_gpio_cfg_output(15);
@@ -208,9 +214,9 @@           xTimerStart(idleTimer, 0);
           spiNorFlash.Wakeup();
           lcd.Wakeup();
 
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::GoToRunning);
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::UpdateBatteryLevel);
-          heartRateApp->PushMessage(Pinetime::Applications::HeartRateTask::Messages::WakeUp);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::GoToRunning);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::UpdateBatteryLevel);
+          heartRateApp.PushMessage(Pinetime::Applications::HeartRateTask::Messages::WakeUp);
 
           isSleeping = false;
           isWakingUp = false;
@@ -230,26 +236,26 @@         case Messages::GoToSleep:
           isGoingToSleep = true;
           NRF_LOG_INFO("[systemtask] Going to sleep");
           xTimerStop(idleTimer, 0);
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::GoToSleep);
-          heartRateApp->PushMessage(Pinetime::Applications::HeartRateTask::Messages::GoToSleep);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::GoToSleep);
+          heartRateApp.PushMessage(Pinetime::Applications::HeartRateTask::Messages::GoToSleep);
           break;
         case Messages::OnNewTime:
           ReloadIdleTimer();
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::UpdateDateTime);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::UpdateDateTime);
           break;
         case Messages::OnNewNotification:
           if (isSleeping && !isWakingUp) {
             GoToRunning();
           }
           motorController.SetDuration(35);
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::NewNotification);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::NewNotification);
           break;
         case Messages::OnTimerDone:
           if (isSleeping && !isWakingUp) {
             GoToRunning();
           }
           motorController.SetDuration(35);
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::TimerDone);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::TimerDone);
           break;
         case Messages::BleConnected:
           ReloadIdleTimer();
@@ -260,7 +266,7 @@         case Messages::BleFirmwareUpdateStarted:
           doNotGoToSleep = true;
           if (isSleeping && !isWakingUp)
             GoToRunning();
-          displayApp->PushMessage(Pinetime::Applications::Display::Messages::BleFirmwareUpdateStarted);
+          displayApp.PushMessage(Pinetime::Applications::Display::Messages::BleFirmwareUpdateStarted);
           break;
         case Messages::BleFirmwareUpdateFinished:
           doNotGoToSleep = false;
@@ -359,7 +365,7 @@     return;
   if (!isSleeping) {
     NRF_LOG_INFO("[systemtask] Button pushed");
     PushMessage(Messages::OnButtonEvent);
-    displayApp->PushMessage(Pinetime::Applications::Display::Messages::ButtonPushed);
+    displayApp.PushMessage(Pinetime::Applications::Display::Messages::ButtonPushed);
   } else {
     if (!isWakingUp) {
       NRF_LOG_INFO("[systemtask] Button pushed, waking up");
@@ -380,7 +386,7 @@   if (isGoingToSleep)
     return;
   if (!isSleeping) {
     PushMessage(Messages::OnTouchEvent);
-    displayApp->PushMessage(Pinetime::Applications::Display::Messages::TouchEvent);
+    displayApp.PushMessage(Pinetime::Applications::Display::Messages::TouchEvent);
   } else if (!isWakingUp) {
     if (settingsController.getWakeUpMode() == Pinetime::Controllers::Settings::WakeUpMode::None or
         settingsController.getWakeUpMode() == Pinetime::Controllers::Settings::WakeUpMode::RaiseWrist)
@@ -389,16 +395,22 @@     PushMessage(Messages::TouchWakeUp);
   }
 }
 
-void SystemTask::PushMessage(SystemTask::Messages msg) {
+void SystemTask::PushMessage(System::Messages msg) {
   if (msg == Messages::GoToSleep) {
     isGoingToSleep = true;
   }
-  BaseType_t xHigherPriorityTaskWoken;
-  xHigherPriorityTaskWoken = pdFALSE;
-  xQueueSendFromISR(systemTasksMsgQueue, &msg, &xHigherPriorityTaskWoken);
-  if (xHigherPriorityTaskWoken) {
-    /* Actual macro used here is port specific. */
-    // TODO: should I do something here?
+
+  if(in_isr()) {
+    BaseType_t xHigherPriorityTaskWoken;
+    xHigherPriorityTaskWoken = pdFALSE;
+    xQueueSendFromISR(systemTasksMsgQueue, &msg, &xHigherPriorityTaskWoken);
+    if (xHigherPriorityTaskWoken) {
+      /* Actual macro used here is port specific. */
+      portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+
+    }
+  } else {
+    xQueueSend(systemTasksMsgQueue, &msg, portMAX_DELAY);
   }
 }
 




diff --git a/src/systemtask/SystemTask.h b/src/systemtask/SystemTask.h
index ea41a69d67f04583188008bb5f5a74363df44f01..fa6a949cf17d7655acc69ef83910bdc54883fc57 100644
--- a/src/systemtask/SystemTask.h
+++ b/src/systemtask/SystemTask.h
@@ -6,7 +6,6 @@ #include 
 #include <task.h>
 #include <timers.h>
 #include <heartratetask/HeartRateTask.h>
-#include <components/heartrate/HeartRateController.h>
 #include <components/settings/Settings.h>
 #include <drivers/Bma421.h>
 #include <components/motion/MotionController.h>
@@ -27,6 +26,7 @@   #include "displayapp/LittleVgl.h"
 #endif
 
 #include "drivers/Watchdog.h"
+#include "Messages.h"
 
 namespace Pinetime {
   namespace Drivers {
@@ -40,27 +40,6 @@   }
   namespace System {
     class SystemTask {
     public:
-      enum class Messages {
-        GoToSleep,
-        GoToRunning,
-        TouchWakeUp,
-        OnNewTime,
-        OnNewNotification,
-        OnTimerDone,
-        OnNewCall,
-        BleConnected,
-        UpdateTimeOut,
-        BleFirmwareUpdateStarted,
-        BleFirmwareUpdateFinished,
-        OnTouchEvent,
-        OnButtonEvent,
-        OnDisplayTaskSleeping,
-        EnableSleeping,
-        DisableSleeping,
-        OnNewDay,
-        OnChargingEvent
-      };
-
       SystemTask(Drivers::SpiMaster& spi,
                  Drivers::St7789& lcd,
                  Pinetime::Drivers::SpiNorFlash& spiNorFlash,
@@ -69,10 +48,18 @@                  Drivers::Cst816S& touchPanel,
                  Components::LittleVgl& lvgl,
                  Controllers::Battery& batteryController,
                  Controllers::Ble& bleController,
+                 Controllers::DateTime& dateTimeController,
+                 Controllers::TimerController& timerController,
+                 Drivers::Watchdog& watchdog,
+                 Pinetime::Controllers::NotificationManager& notificationManager,
                  Pinetime::Controllers::MotorController& motorController,
                  Pinetime::Drivers::Hrs3300& heartRateSensor,
+                 Pinetime::Controllers::MotionController& motionController,
                  Pinetime::Drivers::Bma421& motionSensor,
-                 Controllers::Settings& settingsController);
+                 Controllers::Settings& settingsController,
+                 Pinetime::Controllers::HeartRateController& heartRateController,
+                 Pinetime::Applications::DisplayApp& displayApp,
+                 Pinetime::Applications::HeartRateTask& heartRateApp);
 
       void Start();
       void PushMessage(Messages msg);
@@ -96,27 +83,29 @@       Pinetime::Drivers::TwiMaster& twiMaster;
       Pinetime::Drivers::Cst816S& touchPanel;
       Pinetime::Components::LittleVgl& lvgl;
       Pinetime::Controllers::Battery& batteryController;
-      std::unique_ptr<Pinetime::Applications::DisplayApp> displayApp;
-      Pinetime::Controllers::HeartRateController heartRateController;
-      std::unique_ptr<Pinetime::Applications::HeartRateTask> heartRateApp;
+
 
       Pinetime::Controllers::Ble& bleController;
-      Pinetime::Controllers::DateTime dateTimeController;
-      Pinetime::Controllers::TimerController timerController;
+      Pinetime::Controllers::DateTime& dateTimeController;
+      Pinetime::Controllers::TimerController& timerController;
       QueueHandle_t systemTasksMsgQueue;
       std::atomic<bool> isSleeping {false};
       std::atomic<bool> isGoingToSleep {false};
       std::atomic<bool> isWakingUp {false};
-      Pinetime::Drivers::Watchdog watchdog;
-      Pinetime::Drivers::WatchdogView watchdogView;
-      Pinetime::Controllers::NotificationManager notificationManager;
+      Pinetime::Drivers::Watchdog& watchdog;
+      Pinetime::Controllers::NotificationManager& notificationManager;
       Pinetime::Controllers::MotorController& motorController;
       Pinetime::Drivers::Hrs3300& heartRateSensor;
       Pinetime::Drivers::Bma421& motionSensor;
       Pinetime::Controllers::Settings& settingsController;
+      Pinetime::Controllers::HeartRateController& heartRateController;
       Pinetime::Controllers::NimbleController nimbleController;
       Controllers::BrightnessController brightnessController;
-      Pinetime::Controllers::MotionController motionController;
+      Pinetime::Controllers::MotionController& motionController;
+
+      Pinetime::Applications::DisplayApp& displayApp;
+      Pinetime::Applications::HeartRateTask& heartRateApp;
+
 
       static constexpr uint8_t pinSpiSck = 2;
       static constexpr uint8_t pinSpiMosi = 3;