diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
index 8fae9b02d1..8589338bd7 100644
--- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
+++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
@@ -82,7 +82,10 @@ uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) con
     for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS && i < nfreqs; i++) {
         float rpm;
         if (get_rpm(i, rpm)) {
-            freqs[valid_escs++] = rpm * (1.0f / 60.0f);
+            freqs[i] = rpm * (1.0f / 60.0f);
+            valid_escs++;
+        } else {
+            freqs[i] = 0.0f;
         }
     }