diff --git a/src/shared/scheduler/TaskScheduler.cpp b/src/shared/scheduler/TaskScheduler.cpp
index d22747bd980a16afb360e03a80db97a19cc04e45..3b4ae20d36b87fe523365abb429fcf780bf18fa0 100644
--- a/src/shared/scheduler/TaskScheduler.cpp
+++ b/src/shared/scheduler/TaskScheduler.cpp
@@ -73,7 +73,7 @@ uint8_t TaskScheduler::addTask(function_t function, uint32_t period,
     uint8_t id = 1;
 
     // Find a suitable id for the new task
-    for (; tasks[id] != nullptr && id < 255; ++id)
+    for (; id < 255 && tasks[id] != nullptr; ++id)
         ;
 
     return addTask(function, period, id, policy, startTick);
diff --git a/src/tests/actuators/test-servo.cpp b/src/tests/actuators/test-servo.cpp
index 893321c20ba25647a9ba402542831d014053874c..76736c054d69921b1112c424f91f962bdf25eba0 100644
--- a/src/tests/actuators/test-servo.cpp
+++ b/src/tests/actuators/test-servo.cpp
@@ -80,7 +80,7 @@ int main()
 
     // Start a periodic task to move the first three servos
     TaskScheduler scheduler;
-    uint8_t id = scheduler.addTask(&moveServo, 2 * 1000);
+    scheduler.addTask(&moveServo, 2 * 1000);
     scheduler.start();
 
     // Control the fourth servo manually