diff --git a/src/shared/scheduler/TaskScheduler.cpp b/src/shared/scheduler/TaskScheduler.cpp
index 1f8adf607950c437cac48ff11f9920af5912ea8c..498e7a8dc8dc7810e6533111c3db66a12e2b3ef9 100644
--- a/src/shared/scheduler/TaskScheduler.cpp
+++ b/src/shared/scheduler/TaskScheduler.cpp
@@ -38,7 +38,7 @@ TaskScheduler::TaskScheduler(miosix::Priority priority)
     // Create dynamically the tasks vector because of too much space
     tasks = new std::array<Task, TASKS_SIZE>();
 
-    // Initialize the vector
+    // Initialize the vector elements
     for (size_t i = 1; i < TASKS_SIZE; i++)
     {
         function_t function;
@@ -66,6 +66,9 @@ size_t TaskScheduler::addTask(function_t function, uint32_t period,
     // Check if in the corresponding id there's already a task
     if ((*tasks)[id].valid)
     {
+        // Unlock the mutex for expensive operation
+        Unlock<FastMutex> unlock(mutex);
+
         LOG_ERR(logger, "Full task scheduler, id = {:zu}", id);
         return 0;
     }
@@ -93,6 +96,9 @@ bool TaskScheduler::removeTask(uint8_t id)
     // Check if the task is actually present
     if ((*tasks)[id].valid == false)
     {
+        // Unlock the mutex for expensive operation
+        Unlock<FastMutex> unlock(mutex);
+
         LOG_ERR(logger, "Attempting to remove a task not registered");
         return false;
     }
@@ -198,29 +204,27 @@ void TaskScheduler::run()
             // Execute the task function
             if (nextEvent.task->valid)
             {
-                Unlock<FastMutex> unlock(lock);
-
-                try
                 {
-                    nextEvent.task->function();
+                    Unlock<FastMutex> unlock(lock);
+
+                    try
+                    {
+                        nextEvent.task->function();
+                    }
+                    catch (...)
+                    {
+                        // Update the failed statistic
+                        nextEvent.task->failedEvents++;
+                    }
                 }
-                catch (...)
-                {
-                    // Update the failed statistic
-                    nextEvent.task->failedEvents++;
-                }
-            }
 
-            updateStats(nextEvent, startTick, getTick());
-            enqueue(nextEvent, startTick);
+                // Enqueue only on a valid task
+                updateStats(nextEvent, startTick, getTick());
+                enqueue(nextEvent, startTick);
+            }
         }
         else
         {
-            if (!nextEvent.task->valid)
-            {
-                agenda.pop();
-            }
-
             Unlock<FastMutex> unlock(lock);
 
             Thread::sleepUntil(nextEvent.nextTick);
@@ -250,7 +254,7 @@ void TaskScheduler::updateStats(const Event& event, int64_t startTick,
     event.task->workloadStats.add(endTick - startTick);
 }
 
-void TaskScheduler::enqueue(Event& event, int64_t startTick)
+void TaskScheduler::enqueue(Event event, int64_t startTick)
 {
     constexpr float msToTick = TICK_FREQ / 1000.f;
 
@@ -260,7 +264,6 @@ void TaskScheduler::enqueue(Event& event, int64_t startTick)
             // If the task is one shot we won't push it to the agenda and we'll
             // remove it from the tasks map.
             (*tasks)[event.task->id].valid = false;
-
             return;
         case Policy::SKIP:
             // Updated the missed events count
diff --git a/src/shared/scheduler/TaskScheduler.h b/src/shared/scheduler/TaskScheduler.h
index ff01c6692fde0b99151453ed733a8856a42729e9..bf82b2b7a3c2aa316c89c47c62af18ab566ae412 100644
--- a/src/shared/scheduler/TaskScheduler.h
+++ b/src/shared/scheduler/TaskScheduler.h
@@ -191,7 +191,7 @@ private:
      * \param startTick Activation tick, needed to update the nextTick value of
      * the event.
      */
-    void enqueue(Event& event, int64_t startTick);
+    void enqueue(Event event, int64_t startTick);
 
     /**
      * @brief Creates a task with the passed values