diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp
index a3f57e40d6..d8ddb63730 100644
--- a/libraries/AP_BLHeli/AP_BLHeli.cpp
+++ b/libraries/AP_BLHeli/AP_BLHeli.cpp
@@ -1494,7 +1494,7 @@ void AP_BLHeli::log_bidir_telemetry(void)
     // ask the next ESC for telemetry
     uint8_t idx_pos = last_telem_esc;
     uint8_t idx = (idx_pos + 1) % num_motors;
-    for (; idx != idx_pos; idx = (idx_pos + 1) % num_motors) {
+    for (; idx != idx_pos; idx = (idx + 1) % num_motors) {
         if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) {
             break;
         }
@@ -1563,7 +1563,7 @@ void AP_BLHeli::update_telemetry(void)
         // ask the next ESC for telemetry
         uint8_t idx_pos = last_telem_esc;
         uint8_t idx = (idx_pos + 1) % num_motors;
-        for (; idx != idx_pos; idx = (idx_pos + 1) % num_motors) {
+        for (; idx != idx_pos; idx = (idx + 1) % num_motors) {
             if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) {
                 break;
             }