diff --git a/Core/Src/main.c b/Core/Src/main.c
index 6a98229..4a018ef 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -65,7 +65,7 @@ UART_HandleTypeDef huart3;
 osThreadId_t defaultTaskHandle;
 const osThreadAttr_t defaultTask_attributes = {
   .name = "defaultTask",
-  .stack_size = 128 * 4,
+  .stack_size = 512 * 4,
   .priority = (osPriority_t) osPriorityNormal,
 };
 /* USER CODE BEGIN PV */
diff --git a/KPI_Rover/Database/ulDatabase.h b/KPI_Rover/Database/ulDatabase.h
index 4376309..5746702 100644
--- a/KPI_Rover/Database/ulDatabase.h
+++ b/KPI_Rover/Database/ulDatabase.h
@@ -31,6 +31,10 @@ enum ulDatabase_ParamId {
 	MOTOR_RR_KP,
 	MOTOR_RR_KI,
 	MOTOR_RR_KD,
+	ADC_CAL_CH_11_OFFSET,
+	ADC_CAL_CH_TEMP_OFFSET,
+	ADC_CAL_CH_11_SCALE,
+	ADC_CAL_CH_TEMP_SCALE,
 	MOTOR_FL_SETPOINT,
 	MOTOR_FR_SETPOINT,
 	MOTOR_RL_SETPOINT,
@@ -58,10 +62,6 @@ enum ulDatabase_ParamId {
 	ADC_CALIBRATION_START,
 	ADC_CALIBRATION_POINT,
 	ADC_CALIBRATION_POINT_VALUE,
-	ADC_CAL_CH_11_OFFSET,
-	ADC_CAL_CH_TEMP_OFFSET,
-	ADC_CAL_CH_11_SCALE,
-	ADC_CAL_CH_TEMP_SCALE,
 	ADC_CALIBRATION_CHANNEL_ID,
 	PARAM_BATTERY_VOLTAGE,
 	PARAM_MCU_TEMPERATURE,
diff --git a/KPI_Rover/KPIRover.c b/KPI_Rover/KPIRover.c
index b65e339..c47417c 100644
--- a/KPI_Rover/KPIRover.c
+++ b/KPI_Rover/KPIRover.c
@@ -27,6 +27,10 @@ static struct ulDatabase_ParamMetadata ulDatabase_params[] = {
 		{0, FLOAT, true, 0.046f}, // MOTOR_RR_KP
 		{0, FLOAT, true, 0.013f}, // MOTOR_RR_KI
 		{0, FLOAT, true, 0.0001f}, // MOTOR_RR_KD
+		{0, INT32, true, 0}, // ADC_CAL_CH_11_OFFSET,
+		{0, INT32, true, 0}, // ADC_CAL_CH_TEMP_OFFSET,
+		{0, FLOAT, true, 0.0f}, // ADC_CAL_CH_11_SCALE,
+		{0, FLOAT, true, 0.0f}, // ADC_CAL_CH_TEMP_SCALE,
 		{0, INT32, false, 0}, // MOTOR_FL_SETPOINT,
 		{0, INT32, false, 0}, // MOTOR_FR_SETPOINT,
 		{0, INT32, false, 0}, // MOTOR_RL_SETPOINT,
@@ -54,10 +58,6 @@ static struct ulDatabase_ParamMetadata ulDatabase_params[] = {
 		{0, UINT8, false, 0}, // ADC_CALIBRATION_START,
 		{0, UINT8, false, 0}, // ADC_CALIBRATION_POINT,
 		{0, FLOAT, false, 0}, // ADC_CALIBRATION_POINT_VALUE,
-		{0, INT32, true, 0}, // ADC_CAL_CH_11_OFFSET,
-		{0, INT32, true, 0}, // ADC_CAL_CH_TEMP_OFFSET,
-		{0, FLOAT, true, 0.0f}, // ADC_CAL_CH_11_SCALE,
-		{0, FLOAT, true, 0.0f}, // ADC_CAL_CH_TEMP_SCALE,
 		{0, UINT8, false, 0}, // ADC_CALIBRATION_CHANNEL_ID
 		{0, FLOAT, false, 0}, // PARAM_BATTERY_VOLTAGE,
 		{0, FLOAT, false, 0}, // PARAM_MCU_TEMPERATURE
diff --git a/KPI_Rover/Logger/ul_ulog.c b/KPI_Rover/Logger/ul_ulog.c
index ae4c47d..0e5db41 100644
--- a/KPI_Rover/Logger/ul_ulog.c
+++ b/KPI_Rover/Logger/ul_ulog.c
@@ -36,7 +36,7 @@
 #include <stdio.h>
 #include <string.h>
 
-#define LOG_QUEUE_LENGTH    10
+#define LOG_QUEUE_LENGTH    20
 #define MAX_LOG_MESSAGE_SIZE 256
 
 static void ulogTask(void *argument);
@@ -51,6 +51,10 @@ osEventFlagsId_t ulogFlags;
 #define ULOG_CDC_TRANSMIT_FINISH_FLAG 0x1
 #define ULOG_DEV_CONNECTED_WAKEUP_FLAG 0x2
 
+static StaticSemaphore_t ulUlog_mutex_storage;
+#define ULOG_LOCK() osMutexAcquire(&ulUlog_mutex_storage, osWaitForever)
+#define ULOG_UNLOCK() osMutexRelease(&ulUlog_mutex_storage)
+
 static const osThreadAttr_t ulogTask_attributes = {
   .name = "ulogTask",
   .stack_size = 128 * 4,
@@ -59,8 +63,7 @@ static const osThreadAttr_t ulogTask_attributes = {
 
 
 void ul_ulog_init()
-{ 
-
+{
   xULogQueue = xQueueCreateStatic(LOG_QUEUE_LENGTH, MAX_LOG_MESSAGE_SIZE,
                                ucUlogQueueStorage, &xUlogQueueBuffer);
   if (xULogQueue == NULL)
@@ -76,16 +79,21 @@ void ul_ulog_init()
 
   ulogFlags = osEventFlagsNew(NULL);
   ulogUsbIsEstablished = 0;
+
+  {
+    const static osMutexAttr_t mutex_attrs = {NULL, 0, &ulUlog_mutex_storage, sizeof(ulUlog_mutex_storage)};
+    (void) osMutexNew(&mutex_attrs);
+  }
 }
 
-void ul_ulog_send(ulog_level_t level, const char *filename, char *msg)
+static inline void ul_ulog_send_critical(ulog_level_t level, const char *filename, char *msg)
 {
   if (xULogQueue == NULL || msg == NULL)
   {
     return;
   }
 
-  char logBuffer[MAX_LOG_MESSAGE_SIZE];
+  static char logBuffer[MAX_LOG_MESSAGE_SIZE];
   int len;
   size_t msg_len = strlen(msg);
 
@@ -136,11 +144,18 @@ void ul_ulog_send(ulog_level_t level, const char *filename, char *msg)
   }
 }
 
+void ul_ulog_send(ulog_level_t level, const char *filename, char *msg)
+{
+	ULOG_LOCK();
+	ul_ulog_send_critical(level, filename, msg);
+	ULOG_UNLOCK();
+}
+
 static void ulogTask(void *argument)
 {
   for (;;)
   {
-    char logMessage[MAX_LOG_MESSAGE_SIZE];
+    static char logMessage[MAX_LOG_MESSAGE_SIZE];
     BaseType_t result = xQueueReceive(xULogQueue, logMessage, portMAX_DELAY);
 
     if (result == pdTRUE)
diff --git a/KPI_Rover/Motors/PCA9685.c b/KPI_Rover/Motors/PCA9685.c
index f29ceeb..6c229f1 100644
--- a/KPI_Rover/Motors/PCA9685.c
+++ b/KPI_Rover/Motors/PCA9685.c
@@ -16,15 +16,23 @@ extern I2C_HandleTypeDef hi2c3;
 /**
  * Helper: Write a single byte to a register.
  */
-static void pca9685_write(uint8_t reg, uint8_t data)
+#define pca9685_write(reg, data) do \
+	{ \
+        if (pca9685_write_i2c(reg, data) == false) \
+        { \
+            return false; \
+        } \
+    } while (0)
+
+static bool pca9685_write_i2c(uint8_t reg, uint8_t data)
 {
-    HAL_I2C_Mem_Write(&hi2c3,
-                      PCA9685_ADDR,
-                      reg,
-                      I2C_MEMADD_SIZE_8BIT,
-                      &data,
-                      1,
-                      HAL_MAX_DELAY);
+    return HAL_I2C_Mem_Write(&hi2c3,
+                             PCA9685_ADDR,
+                             reg,
+                             I2C_MEMADD_SIZE_8BIT,
+                             &data,
+                             1,
+                             10) == HAL_OK;
 }
 
 /**
@@ -46,14 +54,14 @@ static void pca9685_write4(uint8_t reg, uint16_t on, uint16_t off)
                       I2C_MEMADD_SIZE_8BIT,
                       buf,
                       4,
-                      HAL_MAX_DELAY);
+                      10);
 }
 
 /**
  * Initialize the PCA9685 chip.
  * Setup frequency and enable auto-increment mode.
  */
-void PCA9685_Init(void)
+bool PCA9685_Init(void)
 {
     /* 1. Reset device (Wake up) */
     pca9685_write(MODE1, 0x00);
@@ -77,6 +85,8 @@ void PCA9685_Init(void)
      */
     pca9685_write(MODE1, 0xA1);
     HAL_Delay(5);
+
+    return true;
 }
 
 /**
diff --git a/KPI_Rover/Motors/PCA9685.h b/KPI_Rover/Motors/PCA9685.h
index ab71c78..01e4893 100644
--- a/KPI_Rover/Motors/PCA9685.h
+++ b/KPI_Rover/Motors/PCA9685.h
@@ -3,8 +3,9 @@
 
 #pragma once
 #include <stdint.h>
+#include <stdbool.h>
 
-void PCA9685_Init(void);
+bool PCA9685_Init(void);
 void PCA9685_SetPWM(uint8_t channel, uint16_t on, uint16_t off);
 void PCA9685_SetPin(uint8_t channel, uint8_t val);
 
diff --git a/KPI_Rover/Motors/ulMotorsController.c b/KPI_Rover/Motors/ulMotorsController.c
index b5b0389..a222325 100644
--- a/KPI_Rover/Motors/ulMotorsController.c
+++ b/KPI_Rover/Motors/ulMotorsController.c
@@ -5,6 +5,7 @@
 #include "Motors/PCA9685.h"
 #include <Database/ulDatabase.h>
 #include <stdio.h>
+#include <stdbool.h>
 #include <string.h>
 #include "usbd_cdc_if.h"
 
@@ -299,7 +300,10 @@ void ulMotorsController_Task(void* argument)
 
     ulMotorsController_Init(&g_motors_ctrl);
 
-    PCA9685_Init();
+    if (!PCA9685_Init()) {
+       ULOG_ERROR("Failed to initialize PCA9685");
+       osThreadExit();
+    }
 
     const osTimerAttr_t timer_attrs = {
    	   .name = "Motors_Timer"