From 04abdd85349bf5ade3f27c8aa94975f374e56362 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 06:50:53 +0100
Subject: [PATCH 01/18] Adding separators for better readability.

---
 src/Braccio++.cpp | 24 ++++++++++++++++++++++++
 src/Braccio++.h   | 16 ++++++++++++++++
 2 files changed, 40 insertions(+)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index b1c9384..57978b8 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -1,3 +1,7 @@
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
 #include "Braccio++.h"
 
 #if LVGL_VERSION_MAJOR < 8 || (LVGL_VERSION_MAJOR == 8 && LVGL_VERSION_MINOR < 1)
@@ -6,6 +10,10 @@
 
 #include "mbed.h"
 
+/**************************************************************************************
+ * FUNCTION DEFINITION
+ **************************************************************************************/
+
 #if LV_USE_LOG
 void lvgl_my_print(const char * dsc)
 {
@@ -18,8 +26,16 @@ extern "C" {
   void braccio_read_keypad(lv_indev_drv_t * indev, lv_indev_data_t * data);
 };
 
+/**************************************************************************************
+ * NAMESPACE
+ **************************************************************************************/
+
 using namespace std::chrono_literals;
 
+/**************************************************************************************
+ * CTOR/DTOR
+ **************************************************************************************/
+
 BraccioClass::BraccioClass()
 : _i2c_mtx{}
 , serial485{Serial1, 0, 7, 8} /* TX, DE, RE */
@@ -43,6 +59,10 @@ BraccioClass::BraccioClass()
 
 }
 
+/**************************************************************************************
+ * PUBLIC MEMBER FUNCTIONS
+ **************************************************************************************/
+
 bool BraccioClass::begin(voidFuncPtr custom_menu)
 {
   SPI.begin();
@@ -229,6 +249,10 @@ void BraccioClass::lvgl_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, l
   lv_disp_flush_ready(disp);
 }
 
+/**************************************************************************************
+ * PRIVATE MEMBER FUNCTIONS
+ **************************************************************************************/
+
 void BraccioClass::pd_thread() {
   start_pd_burst = millis();
   size_t last_time_ask_pps = 0;
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 5c66713..d63cf5a 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -1,6 +1,10 @@
 #ifndef __BRACCIO_PLUSPLUS_H__
 #define __BRACCIO_PLUSPLUS_H__
 
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
 #include "Arduino.h"
 #include "lib/powerdelivery/PD_UFP.h"
 #include "lib/ioexpander/TCA6424A.h"
@@ -11,6 +15,10 @@
 #include "lib/TFT_eSPI/TFT_eSPI.h" // Hardware-specific library
 #include <lvgl.h>
 
+/**************************************************************************************
+ * TYPEDEF
+ **************************************************************************************/
+
 enum speed_grade_t {
   FAST = 10,
   MEDIUM = 100,
@@ -20,8 +28,16 @@ enum speed_grade_t {
 #include <chrono>
 using namespace std::chrono;
 
+/**************************************************************************************
+ * FORWARD DECLARATION
+ **************************************************************************************/
+
 class Servo;
 
+/**************************************************************************************
+ * CLASS DECLARATION
+ **************************************************************************************/
+
 class BraccioClass
 {
 public:

From 162b39da9573c42c3be93bb9d8148d640ca72920 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 06:51:32 +0100
Subject: [PATCH 02/18] Moving inclusion of <chrono> up to other includes.

---
 src/Braccio++.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/Braccio++.h b/src/Braccio++.h
index d63cf5a..d8f49c3 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -15,6 +15,9 @@
 #include "lib/TFT_eSPI/TFT_eSPI.h" // Hardware-specific library
 #include <lvgl.h>
 
+#include <chrono>
+using namespace std::chrono;
+
 /**************************************************************************************
  * TYPEDEF
  **************************************************************************************/
@@ -25,9 +28,6 @@ enum speed_grade_t {
   SLOW = 1000,
 };
 
-#include <chrono>
-using namespace std::chrono;
-
 /**************************************************************************************
  * FORWARD DECLARATION
  **************************************************************************************/

From 979ad8cb8a3d527bc67b36d609acc7f7795b5cea Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 06:54:35 +0100
Subject: [PATCH 03/18] Rearranging functions to confom to order in header
 file. This allows to find them real fast.

---
 src/Braccio++.cpp | 194 +++++++++++++++++++++++-----------------------
 1 file changed, 97 insertions(+), 97 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 57978b8..b437951 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -230,6 +230,28 @@ void BraccioClass::positions(float & a1, float & a2, float & a3, float & a4, flo
   a6 = servos.getPosition(6);
 }
 
+int BraccioClass::getKey() {
+  if (isJoystickPressed_LEFT()) {
+    return 1;
+  }
+  if (isJoystickPressed_RIGHT()) {
+    return 2;
+  }
+  if (isJoystickPressed_SELECT()) {
+    return 3;
+  }
+  if (isJoystickPressed_UP()) {
+    return 4;
+  }
+  if (isJoystickPressed_DOWN()) {
+    return 5;
+  }
+  if (isButtonPressed_ENTER()) {
+    return 6;
+  }
+  return 0;
+}
+
 void BraccioClass::connectJoystickTo(lv_obj_t* obj)
 {
   lv_group_add_obj(_lvgl_p_obj_group, obj);
@@ -253,86 +275,6 @@ void BraccioClass::lvgl_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, l
  * PRIVATE MEMBER FUNCTIONS
  **************************************************************************************/
 
-void BraccioClass::pd_thread() {
-  start_pd_burst = millis();
-  size_t last_time_ask_pps = 0;
-  while (1) {
-    auto ret = pd_events.wait_any(0xFF);
-    if ((ret & 1) && (millis() - start_pd_burst > 1000)) {
-      pd_timer.detach();
-      pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 5s);
-    }
-    if (ret & 2) {
-      pd_timer.detach();
-      pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 50ms);
-    }
-    _i2c_mtx.lock();
-    if (millis() - last_time_ask_pps > 5000) {
-      PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
-      last_time_ask_pps = millis();
-    }
-    PD_UFP.run();
-    _i2c_mtx.unlock();
-    if (PD_UFP.is_power_ready() && PD_UFP.is_PPS_ready()) {
-
-    }
-  }
-}
-
-void BraccioClass::display_thread_func()
-{
-  for(;;)
-  {
-    lv_task_handler();
-    lv_tick_inc(LV_DISP_DEF_REFR_PERIOD);
-    delay(LV_DISP_DEF_REFR_PERIOD);
-  }
-}
-
-void BraccioClass::lvgl_splashScreen(unsigned long const duration_ms, std::function<void()> check_power_func)
-{
-  extern const lv_img_dsc_t img_bulb_gif;
-
-  LV_IMG_DECLARE(img_bulb_gif);
-  lv_obj_t* img = lv_gif_create(lv_scr_act());
-  lv_gif_set_src(img, &img_bulb_gif);
-  lv_obj_align(img, LV_ALIGN_CENTER, 0, 0);
-
-  /* Wait until the splash screen duration is over.
-   * Meanwhile use the wait time for checking the
-   * power.
-   */
-  for (unsigned long const start = millis(); millis() - start < duration_ms;)
-  {
-    check_power_func();
-  }
-
-  lv_obj_del(img);
-}
-
-void BraccioClass::lvgl_pleaseConnectPower()
-{
-  lv_style_set_text_font(&_lv_style, &lv_font_montserrat_32);
-  lv_obj_t * label1 = lv_label_create(lv_scr_act());
-  lv_obj_add_style(label1, &_lv_style, 0);
-  lv_label_set_text(label1, "Please\nconnect\npower.");
-  lv_label_set_long_mode(label1, LV_LABEL_LONG_SCROLL);
-  lv_obj_set_align(label1, LV_ALIGN_CENTER);
-  lv_obj_set_pos(label1, 0, 0);
-}
-
-void BraccioClass::lvgl_defaultMenu()
-{
-  // TODO: create a meaningful default menu
-  lv_style_set_text_font(&_lv_style, &lv_font_montserrat_32);
-  lv_obj_t * label1 = lv_label_create(lv_scr_act());
-  lv_obj_add_style(label1, &_lv_style, 0);
-  lv_label_set_text(label1, "Braccio++");
-  lv_label_set_long_mode(label1, LV_LABEL_LONG_SCROLL);
-  lv_obj_set_align(label1, LV_ALIGN_CENTER);
-  lv_obj_set_pos(label1, 0, 0);
-}
-
 bool BraccioClass::expander_init()
 {
   static uint16_t constexpr EXPANDER_RS485_SLEW_PIN   = 21; /* P25 = 8 * 2 + 5 = 21 */
@@ -403,26 +345,84 @@ void BraccioClass::motorConnectedThreadFunc()
   }
 }
 
-int BraccioClass::getKey() {
-  if (isJoystickPressed_LEFT()) {
-    return 1;
-  }
-  if (isJoystickPressed_RIGHT()) {
-    return 2;
-  }
-  if (isJoystickPressed_SELECT()) {
-    return 3;
-  }
-  if (isJoystickPressed_UP()) {
-    return 4;
+void BraccioClass::display_thread_func()
+{
+  for(;;)
+  {
+    lv_task_handler();
+    lv_tick_inc(LV_DISP_DEF_REFR_PERIOD);
+    delay(LV_DISP_DEF_REFR_PERIOD);
   }
-  if (isJoystickPressed_DOWN()) {
-    return 5;
+}
+
+void BraccioClass::lvgl_splashScreen(unsigned long const duration_ms, std::function<void()> check_power_func)
+{
+  extern const lv_img_dsc_t img_bulb_gif;
+
+  LV_IMG_DECLARE(img_bulb_gif);
+  lv_obj_t* img = lv_gif_create(lv_scr_act());
+  lv_gif_set_src(img, &img_bulb_gif);
+  lv_obj_align(img, LV_ALIGN_CENTER, 0, 0);
+
+  /* Wait until the splash screen duration is over.
+   * Meanwhile use the wait time for checking the
+   * power.
+   */
+  for (unsigned long const start = millis(); millis() - start < duration_ms;)
+  {
+    check_power_func();
   }
-  if (isButtonPressed_ENTER()) {
-    return 6;
+
+  lv_obj_del(img);
+}
+
+void BraccioClass::lvgl_pleaseConnectPower()
+{
+  lv_style_set_text_font(&_lv_style, &lv_font_montserrat_32);
+  lv_obj_t * label1 = lv_label_create(lv_scr_act());
+  lv_obj_add_style(label1, &_lv_style, 0);
+  lv_label_set_text(label1, "Please\nconnect\npower.");
+  lv_label_set_long_mode(label1, LV_LABEL_LONG_SCROLL);
+  lv_obj_set_align(label1, LV_ALIGN_CENTER);
+  lv_obj_set_pos(label1, 0, 0);
+}
+
+void BraccioClass::lvgl_defaultMenu()
+{
+  // TODO: create a meaningful default menu
+  lv_style_set_text_font(&_lv_style, &lv_font_montserrat_32);
+  lv_obj_t * label1 = lv_label_create(lv_scr_act());
+  lv_obj_add_style(label1, &_lv_style, 0);
+  lv_label_set_text(label1, "Braccio++");
+  lv_label_set_long_mode(label1, LV_LABEL_LONG_SCROLL);
+  lv_obj_set_align(label1, LV_ALIGN_CENTER);
+  lv_obj_set_pos(label1, 0, 0);
+}
+
+void BraccioClass::pd_thread() {
+  start_pd_burst = millis();
+  size_t last_time_ask_pps = 0;
+  while (1) {
+    auto ret = pd_events.wait_any(0xFF);
+    if ((ret & 1) && (millis() - start_pd_burst > 1000)) {
+      pd_timer.detach();
+      pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 5s);
+    }
+    if (ret & 2) {
+      pd_timer.detach();
+      pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 50ms);
+    }
+    _i2c_mtx.lock();
+    if (millis() - last_time_ask_pps > 5000) {
+      PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
+      last_time_ask_pps = millis();
+    }
+    PD_UFP.run();
+    _i2c_mtx.unlock();
+    if (PD_UFP.is_power_ready() && PD_UFP.is_PPS_ready()) {
+
+    }
   }
-  return 0;
 }
 
 /* Display flushing */

From 332055fb9e5c95d70d5edc95ea8b284287da3860 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 06:56:29 +0100
Subject: [PATCH 04/18] Separating function definition and implementation of
 module-private functions.

---
 src/Braccio++.cpp | 21 +++++++++++++++------
 1 file changed, 15 insertions(+), 6 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index b437951..26a9fec 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -11,17 +11,15 @@
 #include "mbed.h"
 
 /**************************************************************************************
- * FUNCTION DEFINITION
+ * FUNCTION DECLARATION
  **************************************************************************************/
 
 #if LV_USE_LOG
-void lvgl_my_print(const char * dsc)
-{
-  Serial.println(dsc);
-}
+void lvgl_my_print(const char * dsc);
 #endif /* #if LV_USE_LOG */
 
-extern "C" {
+extern "C"
+{
   void braccio_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p);
   void braccio_read_keypad(lv_indev_drv_t * indev, lv_indev_data_t * data);
 };
@@ -425,6 +423,17 @@ void BraccioClass::pd_thread() {
   }
 }
 
+/**************************************************************************************
+ * FUNCTION DEFINITION
+ **************************************************************************************/
+
+#if LV_USE_LOG
+void lvgl_my_print(const char * dsc)
+{
+  Serial.println(dsc);
+}
+#endif /* #if LV_USE_LOG */
+
 /* Display flushing */
 extern "C" void braccio_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p)
 {

From 58d76028d37f1e61784eee3533f9a8ab0b5526ac Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:00:18 +0100
Subject: [PATCH 05/18] Extract backlight initialisation code into separate
 function.

---
 src/Braccio++.cpp | 16 +++++++++++++---
 src/Braccio++.h   |  1 +
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 26a9fec..894f8f6 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -84,10 +84,8 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
 
   pinMode(1, INPUT_PULLUP);
 
-  _bl.begin();
-  if (_bl.getChipID() != 0xCE)
+  if (!backlight_init())
     return false;
-  _bl.turnOn();
 
   if (!expander_init())
     return false;
@@ -343,6 +341,18 @@ void BraccioClass::motorConnectedThreadFunc()
   }
 }
 
+bool BraccioClass::backlight_init()
+{
+  _bl.begin();
+
+  if (_bl.getChipID() != 0xCE)
+    return false;
+
+  _bl.turnOn();
+
+  return true;
+}
+
 void BraccioClass::display_thread_func()
 {
   for(;;)
diff --git a/src/Braccio++.h b/src/Braccio++.h
index d8f49c3..9f5add2 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -125,6 +125,7 @@ class BraccioClass
   lv_indev_t * _lvgl_kb_indev;
   lv_style_t _lv_style;
   rtos::Thread _display_thd;
+  bool backlight_init();
   void display_thread_func();
   void lvgl_splashScreen(unsigned long const duration_ms, std::function<void()> check_power_func);
   void lvgl_pleaseConnectPower();

From 7571fe01ad1d41b154cd64d05890d8fdc241ed29 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:02:01 +0100
Subject: [PATCH 06/18] Extract button initialisation in separate function.

---
 src/Braccio++.cpp | 17 +++++++++++------
 src/Braccio++.h   |  2 ++
 2 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 894f8f6..0bf7bfd 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -90,12 +90,7 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
   if (!expander_init())
     return false;
 
-  pinMode(BTN_LEFT, INPUT_PULLUP);
-  pinMode(BTN_RIGHT, INPUT_PULLUP);
-  pinMode(BTN_UP, INPUT_PULLUP);
-  pinMode(BTN_DOWN, INPUT_PULLUP);
-  pinMode(BTN_SEL, INPUT_PULLUP);
-  pinMode(BTN_ENTER, INPUT_PULLUP);
+  button_init();
 
 #if LV_USE_LOG
   lv_log_register_print_cb(lvgl_my_print);
@@ -271,6 +266,16 @@ void BraccioClass::lvgl_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, l
  * PRIVATE MEMBER FUNCTIONS
  **************************************************************************************/
 
+void BraccioClass::button_init()
+{
+  pinMode(BTN_LEFT,  INPUT_PULLUP);
+  pinMode(BTN_RIGHT, INPUT_PULLUP);
+  pinMode(BTN_UP,    INPUT_PULLUP);
+  pinMode(BTN_DOWN,  INPUT_PULLUP);
+  pinMode(BTN_SEL,   INPUT_PULLUP);
+  pinMode(BTN_ENTER, INPUT_PULLUP);
+}
+
 bool BraccioClass::expander_init()
 {
   static uint16_t constexpr EXPANDER_RS485_SLEW_PIN   = 21; /* P25 = 8 * 2 + 5 = 21 */
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 9f5add2..ea6c6ea 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -89,6 +89,8 @@ class BraccioClass
 
 private:
 
+  void button_init();
+
   rtos::Mutex _i2c_mtx;
   RS485Class serial485;
   SmartServoClass servos;

From 3825a3980beae52bf9822a8aabe2dac3071c48a0 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:04:53 +0100
Subject: [PATCH 07/18] Extract TFT display initialisation into separate
 function.

---
 src/Braccio++.cpp | 17 ++++++++++-------
 src/Braccio++.h   |  1 +
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 0bf7bfd..add5bf1 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -92,6 +92,8 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
 
   button_init();
 
+  display_init();
+
 #if LV_USE_LOG
   lv_log_register_print_cb(lvgl_my_print);
 #endif
@@ -115,11 +117,6 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
 
   lv_style_init(&_lv_style);
 
-  _gfx.init();
-  _gfx.setRotation(4);
-  _gfx.fillScreen(TFT_WHITE);
-  _gfx.setAddrWindow(0, 0, 240, 240);
-
   _lvgl_p_obj_group = lv_group_create();
   lv_group_set_default(_lvgl_p_obj_group);
 
@@ -349,15 +346,21 @@ void BraccioClass::motorConnectedThreadFunc()
 bool BraccioClass::backlight_init()
 {
   _bl.begin();
-
   if (_bl.getChipID() != 0xCE)
     return false;
 
   _bl.turnOn();
-
   return true;
 }
 
+void BraccioClass::display_init()
+{
+  _gfx.init();
+  _gfx.setRotation(4);
+  _gfx.fillScreen(TFT_WHITE);
+  _gfx.setAddrWindow(0, 0, 240, 240);
+}
+
 void BraccioClass::display_thread_func()
 {
   for(;;)
diff --git a/src/Braccio++.h b/src/Braccio++.h
index ea6c6ea..089ef8d 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -128,6 +128,7 @@ class BraccioClass
   lv_style_t _lv_style;
   rtos::Thread _display_thd;
   bool backlight_init();
+  void display_init();
   void display_thread_func();
   void lvgl_splashScreen(unsigned long const duration_ms, std::function<void()> check_power_func);
   void lvgl_pleaseConnectPower();

From 1d007d1d63de7e39345f2a5352186b218129d283 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:06:47 +0100
Subject: [PATCH 08/18] Extract lvgl initialisation into separate function.

---
 src/Braccio++.cpp | 58 +++++++++++++++++++++++++----------------------
 src/Braccio++.h   |  1 +
 2 files changed, 32 insertions(+), 27 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index add5bf1..1e2fc30 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -92,36 +92,12 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
 
   button_init();
 
-  display_init();
-
-#if LV_USE_LOG
-  lv_log_register_print_cb(lvgl_my_print);
-#endif
-
-  lv_init();
-
-  lv_disp_draw_buf_init(&_lvgl_disp_buf, _lvgl_draw_buf, NULL, LVGL_DRAW_BUFFER_SIZE);
-
-  /*Initialize the display*/
-  lv_disp_drv_init(&_lvgl_disp_drv);
-  _lvgl_disp_drv.hor_res = 240;
-  _lvgl_disp_drv.ver_res = 240;
-  _lvgl_disp_drv.flush_cb = braccio_disp_flush;
-  _lvgl_disp_drv.draw_buf = &_lvgl_disp_buf;
-  lv_disp_drv_register(&_lvgl_disp_drv);
-
-  lv_indev_drv_init(&_lvgl_indev_drv);
-  _lvgl_indev_drv.type = LV_INDEV_TYPE_KEYPAD;
-  _lvgl_indev_drv.read_cb = braccio_read_keypad;
-  _lvgl_kb_indev = lv_indev_drv_register(&_lvgl_indev_drv);
-
-  lv_style_init(&_lv_style);
-
-  _lvgl_p_obj_group = lv_group_create();
-  lv_group_set_default(_lvgl_p_obj_group);
 
+  display_init();
+  lvgl_init();
   _display_thd.start(mbed::callback(this, &BraccioClass::display_thread_func));
 
+
   auto check_power_func = [this]()
   {
     if (!PD_UFP.is_PPS_ready())
@@ -361,6 +337,34 @@ void BraccioClass::display_init()
   _gfx.setAddrWindow(0, 0, 240, 240);
 }
 
+void BraccioClass::lvgl_init()
+{
+  lv_init();
+
+#if LV_USE_LOG
+  lv_log_register_print_cb(lvgl_my_print);
+#endif
+
+  lv_disp_draw_buf_init(&_lvgl_disp_buf, _lvgl_draw_buf, NULL, LVGL_DRAW_BUFFER_SIZE);
+
+  lv_disp_drv_init(&_lvgl_disp_drv);
+  _lvgl_disp_drv.hor_res = 240;
+  _lvgl_disp_drv.ver_res = 240;
+  _lvgl_disp_drv.flush_cb = braccio_disp_flush;
+  _lvgl_disp_drv.draw_buf = &_lvgl_disp_buf;
+  lv_disp_drv_register(&_lvgl_disp_drv);
+
+  lv_indev_drv_init(&_lvgl_indev_drv);
+  _lvgl_indev_drv.type = LV_INDEV_TYPE_KEYPAD;
+  _lvgl_indev_drv.read_cb = braccio_read_keypad;
+  _lvgl_kb_indev = lv_indev_drv_register(&_lvgl_indev_drv);
+
+  lv_style_init(&_lv_style);
+
+  _lvgl_p_obj_group = lv_group_create();
+  lv_group_set_default(_lvgl_p_obj_group);
+}
+
 void BraccioClass::display_thread_func()
 {
   for(;;)
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 089ef8d..573d572 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -129,6 +129,7 @@ class BraccioClass
   rtos::Thread _display_thd;
   bool backlight_init();
   void display_init();
+  void lvgl_init();
   void display_thread_func();
   void lvgl_splashScreen(unsigned long const duration_ms, std::function<void()> check_power_func);
   void lvgl_pleaseConnectPower();

From 5a30ba07ac9186291c9ae438a2d6da5635ba3b46 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:09:56 +0100
Subject: [PATCH 09/18] Reorg init code so that backlight is only turned on
 after the display is initialized.

---
 src/Braccio++.cpp | 11 +++--------
 1 file changed, 3 insertions(+), 8 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 1e2fc30..b9cd34d 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -82,18 +82,13 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
   }
 */
 
-  pinMode(1, INPUT_PULLUP);
-
-  if (!backlight_init())
-    return false;
-
-  if (!expander_init())
-    return false;
-
   button_init();
+  pinMode(1, INPUT_PULLUP);
 
+  if (!expander_init()) return false;
 
   display_init();
+  if (!backlight_init()) return false;
   lvgl_init();
   _display_thd.start(mbed::callback(this, &BraccioClass::display_thread_func));
 

From d4606141194269991e8d8b16c3e5e1820d32295f Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:14:35 +0100
Subject: [PATCH 10/18] Moving implementation of RGB LED setting from header to
 source file.

---
 src/Braccio++.cpp | 18 +++++++++++++++---
 src/Braccio++.h   | 12 ++----------
 2 files changed, 17 insertions(+), 13 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index b9cd34d..6cd3906 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -273,12 +273,24 @@ bool BraccioClass::expander_init()
 
   /* Set all motor status LEDs to red. */
   for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
-    setRed(id);
+    expander_setRed(id);
   }
 
   return true;
 }
 
+void BraccioClass::expander_setGreen(int const i)
+{
+  _expander.writePin(i * 2 - 1, 0);
+  _expander.writePin(i * 2 - 2, 1);
+}
+
+void BraccioClass::expander_setRed(int const i)
+{
+  _expander.writePin(i * 2 - 1, 1);
+  _expander.writePin(i * 2 - 2, 0);
+}
+
 bool BraccioClass::isPingAllowed()
 {
   mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
@@ -305,9 +317,9 @@ void BraccioClass::motorConnectedThreadFunc()
 
       for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
         if (connected(id))
-          setGreen(id);
+          expander_setGreen(id);
         else
-          setRed(id);
+          expander_setRed(id);
       }
     }
     delay(1000);
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 573d572..8c46cbd 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -97,6 +97,8 @@ class BraccioClass
   PD_UFP_log_c PD_UFP;
   TCA6424A _expander;
   bool expander_init();
+  void expander_setGreen(int const i);
+  void expander_setRed(int const i);
 
   bool _is_ping_allowed;
   bool _is_motor_connected[SmartServoClass::NUM_MOTORS];
@@ -150,16 +152,6 @@ class BraccioClass
     pd_events.set(1);
   }
 
-  void setGreen(int i) {
-    _expander.writePin(i * 2 - 1, 0);
-    _expander.writePin(i * 2 - 2, 1);
-  }
-
-  void setRed(int i) {
-    _expander.writePin(i * 2 - 1, 1);
-    _expander.writePin(i * 2 - 2, 0);
-  }
-
   void pd_thread();
 };
 

From 39c89b95d6bd8c2488494863d28d331d7e13a680 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:15:52 +0100
Subject: [PATCH 11/18] Use named constant instead of magic number.

---
 src/Braccio++.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 6cd3906..5f1312b 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -63,11 +63,14 @@ BraccioClass::BraccioClass()
 
 bool BraccioClass::begin(voidFuncPtr custom_menu)
 {
+  static int constexpr RS485_RX_PIN = 1;
+
   SPI.begin();
   Wire.begin();
   Serial.begin(115200);
 
   pinMode(PIN_FUSB302_INT, INPUT_PULLUP);
+  pinMode(RS485_RX_PIN, INPUT_PULLUP);
 
   static rtos::Thread th(osPriorityHigh);
   th.start(mbed::callback(this, &BraccioClass::pd_thread));
@@ -83,7 +86,6 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
 */
 
   button_init();
-  pinMode(1, INPUT_PULLUP);
 
   if (!expander_init()) return false;
 

From aeab746b417a30925ae0ae4227b79a33e8074eb5 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:28:00 +0100
Subject: [PATCH 12/18] Prefix all remaining member variables with a '_'.

---
 src/Braccio++.cpp | 80 +++++++++++++++++++++++------------------------
 src/Braccio++.h   | 24 +++++++-------
 2 files changed, 52 insertions(+), 52 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 5f1312b..6903b8c 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -36,9 +36,9 @@ using namespace std::chrono_literals;
 
 BraccioClass::BraccioClass()
 : _i2c_mtx{}
-, serial485{Serial1, 0, 7, 8} /* TX, DE, RE */
-, servos{serial485}
-, PD_UFP{PD_LOG_LEVEL_VERBOSE}
+, _serial485{Serial1, 0, 7, 8} /* TX, DE, RE */
+, _servos{_serial485}
+, _PD_UFP{PD_LOG_LEVEL_VERBOSE}
 , _expander{TCA6424A_ADDRESS_ADDR_HIGH, _i2c_mtx}
 , _is_ping_allowed{true}
 , _is_motor_connected{false}
@@ -75,13 +75,13 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
   static rtos::Thread th(osPriorityHigh);
   th.start(mbed::callback(this, &BraccioClass::pd_thread));
   attachInterrupt(PIN_FUSB302_INT, mbed::callback(this, &BraccioClass::unlock_pd_semaphore_irq), FALLING);
-  pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 10ms);
+  _pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 10ms);
 
-  PD_UFP.init_PPS(PPS_V(7.2), PPS_A(2.0));
+  _PD_UFP.init_PPS(PPS_V(7.2), PPS_A(2.0));
 
 /*
   while (millis() < 200) {
-    PD_UFP.run();
+    _PD_UFP.run();
   }
 */
 
@@ -97,11 +97,11 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
 
   auto check_power_func = [this]()
   {
-    if (!PD_UFP.is_PPS_ready())
+    if (!_PD_UFP.is_PPS_ready())
     {
       _i2c_mtx.lock();
-      PD_UFP.print_status(Serial);
-      PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
+      _PD_UFP.print_status(Serial);
+      _PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
       delay(10);
       _i2c_mtx.unlock();
     }
@@ -110,11 +110,11 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
   lvgl_splashScreen(2000, check_power_func);
   lv_obj_clean(lv_scr_act());
 
-  if (!PD_UFP.is_PPS_ready())
+  if (!_PD_UFP.is_PPS_ready())
     lvgl_pleaseConnectPower();
 
   /* Loop forever, if no power is attached. */
-  while(!PD_UFP.is_PPS_ready())
+  while(!_PD_UFP.is_PPS_ready())
     check_power_func();
   lv_obj_clean(lv_scr_act());
 
@@ -123,9 +123,9 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
   else
     lvgl_defaultMenu();
 
-  servos.begin();
-  servos.setTime(SmartServoClass::BROADCAST, SLOW);
-  servos.setPositionMode(PositionMode::IMMEDIATE);
+  _servos.begin();
+  _servos.setTime(SmartServoClass::BROADCAST, SLOW);
+  _servos.setPositionMode(PositionMode::IMMEDIATE);
 
   _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc));
 
@@ -152,7 +152,7 @@ bool BraccioClass::connected(int const id)
 
 Servo BraccioClass::move(int const id)
 {
-  Servo wrapper(servos, id);
+  Servo wrapper(_servos, id);
   return wrapper;
 }
 
@@ -163,32 +163,32 @@ Servo BraccioClass::get(int const id)
 
 void BraccioClass::moveTo(float const a1, float const a2, float const a3, float const a4, float const a5, float const a6)
 {
-  servos.setPositionMode(PositionMode::SYNC);
-  servos.setPosition(1, a1);
-  servos.setPosition(2, a2);
-  servos.setPosition(3, a3);
-  servos.setPosition(4, a4);
-  servos.setPosition(5, a5);
-  servos.setPosition(6, a6);
-  servos.synchronize();
-  servos.setPositionMode(PositionMode::IMMEDIATE);
+  _servos.setPositionMode(PositionMode::SYNC);
+  _servos.setPosition(1, a1);
+  _servos.setPosition(2, a2);
+  _servos.setPosition(3, a3);
+  _servos.setPosition(4, a4);
+  _servos.setPosition(5, a5);
+  _servos.setPosition(6, a6);
+  _servos.synchronize();
+  _servos.setPositionMode(PositionMode::IMMEDIATE);
 }
 
 void BraccioClass::positions(float * buffer)
 {
   for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
-    *buffer++ = servos.getPosition(id);
+    *buffer++ = _servos.getPosition(id);
 }
 
 void BraccioClass::positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6)
 {
   // TODO: add check if motors are actually connected
-  a1 = servos.getPosition(1);
-  a2 = servos.getPosition(2);
-  a3 = servos.getPosition(3);
-  a4 = servos.getPosition(4);
-  a5 = servos.getPosition(5);
-  a6 = servos.getPosition(6);
+  a1 = _servos.getPosition(1);
+  a2 = _servos.getPosition(2);
+  a3 = _servos.getPosition(3);
+  a4 = _servos.getPosition(4);
+  a5 = _servos.getPosition(5);
+  a6 = _servos.getPosition(6);
 }
 
 int BraccioClass::getKey() {
@@ -313,7 +313,7 @@ void BraccioClass::motorConnectedThreadFunc()
     {
       for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
       {
-        bool const is_connected = (servos.ping(id) == 0);
+        bool const is_connected = (_servos.ping(id) == 0);
         setMotorConnectionStatus(id, is_connected);
       }
 
@@ -432,23 +432,23 @@ void BraccioClass::pd_thread() {
   start_pd_burst = millis();
   size_t last_time_ask_pps = 0;
   while (1) {
-    auto ret = pd_events.wait_any(0xFF);
+    auto ret = _pd_events.wait_any(0xFF);
     if ((ret & 1) && (millis() - start_pd_burst > 1000)) {
-      pd_timer.detach();
-      pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 5s);
+      _pd_timer.detach();
+      _pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 5s);
     }
     if (ret & 2) {
-      pd_timer.detach();
-      pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 50ms);
+      _pd_timer.detach();
+      _pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 50ms);
     }
     _i2c_mtx.lock();
     if (millis() - last_time_ask_pps > 5000) {
-      PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
+      _PD_UFP.set_PPS(PPS_V(7.2), PPS_A(2.0));
       last_time_ask_pps = millis();
     }
-    PD_UFP.run();
+    _PD_UFP.run();
     _i2c_mtx.unlock();
-    if (PD_UFP.is_power_ready() && PD_UFP.is_PPS_ready()) {
+    if (_PD_UFP.is_power_ready() && _PD_UFP.is_PPS_ready()) {
 
     }
   }
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 8c46cbd..5c5c2f3 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -60,11 +60,11 @@ class BraccioClass
   void positions(float * buffer);
   void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);
 
-  inline void speed(speed_grade_t const speed_grade) { servos.setTime(SmartServoClass::BROADCAST, speed_grade); }
-  inline void speed(int const id, speed_grade_t const speed_grade) { servos.setTime(id, speed_grade); }
+  inline void speed(speed_grade_t const speed_grade) { _servos.setTime(SmartServoClass::BROADCAST, speed_grade); }
+  inline void speed(int const id, speed_grade_t const speed_grade) { _servos.setTime(id, speed_grade); }
 
-  inline void disengage(int const id = SmartServoClass::BROADCAST) { servos.disengage(id); }
-  inline void engage   (int const id = SmartServoClass::BROADCAST) { servos.engage(id); }
+  inline void disengage(int const id = SmartServoClass::BROADCAST) { _servos.disengage(id); }
+  inline void engage   (int const id = SmartServoClass::BROADCAST) { _servos.engage(id); }
 
   int getKey();
   void connectJoystickTo(lv_obj_t* obj);
@@ -85,16 +85,16 @@ class BraccioClass
 
 protected:
 
-  inline void setID(int const id) { servos.setID(id); }
+  inline void setID(int const id) { _servos.setID(id); }
 
 private:
 
   void button_init();
 
   rtos::Mutex _i2c_mtx;
-  RS485Class serial485;
-  SmartServoClass servos;
-  PD_UFP_log_c PD_UFP;
+  RS485Class _serial485;
+  SmartServoClass _servos;
+  PD_UFP_log_c _PD_UFP;
   TCA6424A _expander;
   bool expander_init();
   void expander_setGreen(int const i);
@@ -138,18 +138,18 @@ class BraccioClass
   void lvgl_defaultMenu();
 
 
-  rtos::EventFlags pd_events;
-  mbed::Ticker pd_timer;
+  rtos::EventFlags _pd_events;
+  mbed::Ticker _pd_timer;
 
   unsigned int start_pd_burst = 0xFFFFFFFF;
 
   void unlock_pd_semaphore_irq() {
     start_pd_burst = millis();
-    pd_events.set(2);
+    _pd_events.set(2);
   }
 
   void unlock_pd_semaphore() {
-    pd_events.set(1);
+    _pd_events.set(1);
   }
 
   void pd_thread();

From f6f2c70c05fb96d3283ea3b4d600cf927c2ce528 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:37:36 +0100
Subject: [PATCH 13/18] Eliminate self-made boost::bind.

---
 src/Braccio++.cpp | 20 ++++++++++++++++----
 src/Braccio++.h   | 31 +++++++------------------------
 2 files changed, 23 insertions(+), 28 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 6903b8c..7fac298 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -22,6 +22,8 @@ extern "C"
 {
   void braccio_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p);
   void braccio_read_keypad(lv_indev_drv_t * indev, lv_indev_data_t * data);
+  void braccio_unlock_pd_semaphore_irq();
+  void braccio_unlock_pd_semaphore();
 };
 
 /**************************************************************************************
@@ -74,8 +76,8 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
 
   static rtos::Thread th(osPriorityHigh);
   th.start(mbed::callback(this, &BraccioClass::pd_thread));
-  attachInterrupt(PIN_FUSB302_INT, mbed::callback(this, &BraccioClass::unlock_pd_semaphore_irq), FALLING);
-  _pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 10ms);
+  attachInterrupt(PIN_FUSB302_INT, braccio_unlock_pd_semaphore_irq, FALLING);
+  _pd_timer.attach(braccio_unlock_pd_semaphore, 10ms);
 
   _PD_UFP.init_PPS(PPS_V(7.2), PPS_A(2.0));
 
@@ -435,11 +437,11 @@ void BraccioClass::pd_thread() {
     auto ret = _pd_events.wait_any(0xFF);
     if ((ret & 1) && (millis() - start_pd_burst > 1000)) {
       _pd_timer.detach();
-      _pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 5s);
+      _pd_timer.attach(braccio_unlock_pd_semaphore, 5s);
     }
     if (ret & 2) {
       _pd_timer.detach();
-      _pd_timer.attach(mbed::callback(this, &BraccioClass::unlock_pd_semaphore), 50ms);
+      _pd_timer.attach(braccio_unlock_pd_semaphore, 50ms);
     }
     _i2c_mtx.lock();
     if (millis() - last_time_ask_pps > 5000) {
@@ -510,3 +512,13 @@ extern "C" void braccio_read_keypad(lv_indev_drv_t * drv, lv_indev_data_t* data)
 
     data->key = last_key;
 }
+
+void braccio_unlock_pd_semaphore_irq()
+{
+  Braccio.unlock_pd_semaphore_irq();
+}
+
+void braccio_unlock_pd_semaphore()
+{
+  Braccio.unlock_pd_semaphore();
+}
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 5c5c2f3..7584659 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -82,6 +82,13 @@ class BraccioClass
   }
 
   void lvgl_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p);
+  void unlock_pd_semaphore_irq() {
+    start_pd_burst = millis();
+    _pd_events.set(2);
+  }
+  void unlock_pd_semaphore() {
+    _pd_events.set(1);
+  }
 
 protected:
 
@@ -142,16 +149,6 @@ class BraccioClass
   mbed::Ticker _pd_timer;
 
   unsigned int start_pd_burst = 0xFFFFFFFF;
-
-  void unlock_pd_semaphore_irq() {
-    start_pd_burst = millis();
-    _pd_events.set(2);
-  }
-
-  void unlock_pd_semaphore() {
-    _pd_events.set(1);
-  }
-
   void pd_thread();
 };
 
@@ -184,18 +181,4 @@ class Servo
   int const _id;
 };
 
-struct __callback__container__ {
-  mbed::Callback<void()> fn;
-};
-
-inline void attachInterrupt(pin_size_t interruptNum, mbed::Callback<void()> func, PinStatus mode) {
-  struct __callback__container__* a = new __callback__container__();
-  a->fn = func;
-  auto callback = [](void* a) -> void {
-    ((__callback__container__*)a)->fn();
-  };
-
-  attachInterruptParam(interruptNum, callback, mode, (void*)a);
-}
-
 #endif //__BRACCIO_PLUSPLUS_H__

From e3fe2f9aff5c4e4798c8941e5cc9474b2d4906ff Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:40:02 +0100
Subject: [PATCH 14/18] Separate definition and implementation.

---
 src/Braccio++.cpp | 11 +++++++++++
 src/Braccio++.h   |  9 ++-------
 2 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 7fac298..d6424af 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -234,6 +234,17 @@ void BraccioClass::lvgl_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, l
   lv_disp_flush_ready(disp);
 }
 
+void BraccioClass::unlock_pd_semaphore_irq()
+{
+ start_pd_burst = millis();
+ _pd_events.set(2);
+}
+
+void BraccioClass::unlock_pd_semaphore()
+{
+  _pd_events.set(1);
+}
+
 /**************************************************************************************
  * PRIVATE MEMBER FUNCTIONS
  **************************************************************************************/
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 7584659..51ea7fa 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -82,13 +82,8 @@ class BraccioClass
   }
 
   void lvgl_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p);
-  void unlock_pd_semaphore_irq() {
-    start_pd_burst = millis();
-    _pd_events.set(2);
-  }
-  void unlock_pd_semaphore() {
-    _pd_events.set(1);
-  }
+  void unlock_pd_semaphore_irq();
+  void unlock_pd_semaphore();
 
 protected:
 

From fc7d4e486ec64e2828f645497b28623266a92537 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:40:58 +0100
Subject: [PATCH 15/18] Do not call those functions yourself, they are called
 internally.

---
 src/Braccio++.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/Braccio++.h b/src/Braccio++.h
index 51ea7fa..4e7362b 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -81,6 +81,7 @@ class BraccioClass
     return dev;
   }
 
+  /* Those functions MUST NOT be used by the user. */
   void lvgl_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p);
   void unlock_pd_semaphore_irq();
   void unlock_pd_semaphore();

From a1d4763500221e62b288a3ebd80323f1907c41d0 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:42:04 +0100
Subject: [PATCH 16/18] Mark start_pd_burst as a private member via prefixing
 it with '_'.

---
 src/Braccio++.cpp | 6 +++---
 src/Braccio++.h   | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index d6424af..4073628 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -236,7 +236,7 @@ void BraccioClass::lvgl_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, l
 
 void BraccioClass::unlock_pd_semaphore_irq()
 {
- start_pd_burst = millis();
+ _start_pd_burst = millis();
  _pd_events.set(2);
 }
 
@@ -442,11 +442,11 @@ void BraccioClass::lvgl_defaultMenu()
 }
 
 void BraccioClass::pd_thread() {
-  start_pd_burst = millis();
+  _start_pd_burst = millis();
   size_t last_time_ask_pps = 0;
   while (1) {
     auto ret = _pd_events.wait_any(0xFF);
-    if ((ret & 1) && (millis() - start_pd_burst > 1000)) {
+    if ((ret & 1) && (millis() - _start_pd_burst > 1000)) {
       _pd_timer.detach();
       _pd_timer.attach(braccio_unlock_pd_semaphore, 5s);
     }
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 4e7362b..c354d1d 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -144,7 +144,7 @@ class BraccioClass
   rtos::EventFlags _pd_events;
   mbed::Ticker _pd_timer;
 
-  unsigned int start_pd_burst = 0xFFFFFFFF;
+  unsigned int _start_pd_burst = 0xFFFFFFFF;
   void pd_thread();
 };
 

From 79a84b1abb6128d61e4e3db395bfe9dc14a6c445 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:43:14 +0100
Subject: [PATCH 17/18] Init PD related members within CTOR.

---
 src/Braccio++.cpp | 3 +++
 src/Braccio++.h   | 3 +--
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 4073628..63adfe8 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -55,6 +55,9 @@ BraccioClass::BraccioClass()
 , _lvgl_p_obj_group{nullptr}
 , _lvgl_kb_indev{nullptr}
 , _display_thd{}
+, _pd_events{}
+, _pd_timer{}
+, _start_pd_burst{0xFFFFFFFF}
 {
 
 }
diff --git a/src/Braccio++.h b/src/Braccio++.h
index c354d1d..afefb6c 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -143,8 +143,7 @@ class BraccioClass
 
   rtos::EventFlags _pd_events;
   mbed::Ticker _pd_timer;
-
-  unsigned int _start_pd_burst = 0xFFFFFFFF;
+  unsigned int _start_pd_burst;
   void pd_thread();
 };
 

From b02da4ef3b929376fe070248c626ce650716b143 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 26 Jan 2022 07:46:47 +0100
Subject: [PATCH 18/18] Turn static thread declaration into private member.

---
 src/Braccio++.cpp | 18 +++++++-----------
 src/Braccio++.h   |  3 ++-
 2 files changed, 9 insertions(+), 12 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 63adfe8..8116a8f 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -58,6 +58,7 @@ BraccioClass::BraccioClass()
 , _pd_events{}
 , _pd_timer{}
 , _start_pd_burst{0xFFFFFFFF}
+, _pd_thd{osPriorityHigh}
 {
 
 }
@@ -77,19 +78,11 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
   pinMode(PIN_FUSB302_INT, INPUT_PULLUP);
   pinMode(RS485_RX_PIN, INPUT_PULLUP);
 
-  static rtos::Thread th(osPriorityHigh);
-  th.start(mbed::callback(this, &BraccioClass::pd_thread));
+  _pd_thd.start(mbed::callback(this, &BraccioClass::pd_thread_func));
   attachInterrupt(PIN_FUSB302_INT, braccio_unlock_pd_semaphore_irq, FALLING);
   _pd_timer.attach(braccio_unlock_pd_semaphore, 10ms);
-
   _PD_UFP.init_PPS(PPS_V(7.2), PPS_A(2.0));
 
-/*
-  while (millis() < 200) {
-    _PD_UFP.run();
-  }
-*/
-
   button_init();
 
   if (!expander_init()) return false;
@@ -444,10 +437,13 @@ void BraccioClass::lvgl_defaultMenu()
   lv_obj_set_pos(label1, 0, 0);
 }
 
-void BraccioClass::pd_thread() {
+void BraccioClass::pd_thread_func()
+{
   _start_pd_burst = millis();
   size_t last_time_ask_pps = 0;
-  while (1) {
+
+  for(;;)
+  {
     auto ret = _pd_events.wait_any(0xFF);
     if ((ret & 1) && (millis() - _start_pd_burst > 1000)) {
       _pd_timer.detach();
diff --git a/src/Braccio++.h b/src/Braccio++.h
index afefb6c..068bf67 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -144,7 +144,8 @@ class BraccioClass
   rtos::EventFlags _pd_events;
   mbed::Ticker _pd_timer;
   unsigned int _start_pd_burst;
-  void pd_thread();
+  rtos::Thread _pd_thd;
+  void pd_thread_func();
 };
 
 #define Braccio BraccioClass::get_default_instance()