diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp b/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp
index 923b5bd261..3b957b11f2 100644
--- a/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp
+++ b/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp
@@ -61,6 +61,7 @@ void AP_Servo_Telem::update_telem_data(const uint8_t servo_index, const Telemetr
     if (servo_index >= ARRAY_SIZE(_telem_data) || (new_data.valid_types == 0)) {
         return;
     }
+    active_mask |= 1U << servo_index;
 
     volatile TelemetryData &telemdata = _telem_data[servo_index];
 
@@ -112,7 +113,7 @@ void AP_Servo_Telem::write_log()
     AP_Logger *logger = AP_Logger::get_singleton();
 
     // Check logging is available and enabled
-    if ((logger == nullptr) || !logger->logging_enabled()) {
+    if ((logger == nullptr) || !logger->logging_enabled() || active_mask == 0) {
         return;
     }
 
@@ -134,15 +135,15 @@ void AP_Servo_Telem::write_log()
             LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG),
             time_us     : now_us,
             id          : i,
-            position    : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.measured_position             : AP::logger().quiet_nanf(),
-            force       : telemdata.present(TelemetryData::Types::FORCE)             ? telemdata.force                         : AP::logger().quiet_nanf(),
-            speed       : telemdata.present(TelemetryData::Types::SPEED)             ? telemdata.speed                         : AP::logger().quiet_nanf(),
+            position    : telemdata.present(TelemetryData::Types::MEASURED_POSITION)  ? telemdata.measured_position             : AP::logger().quiet_nanf(),
+            force       : telemdata.present(TelemetryData::Types::FORCE)              ? telemdata.force                         : AP::logger().quiet_nanf(),
+            speed       : telemdata.present(TelemetryData::Types::SPEED)              ? telemdata.speed                         : AP::logger().quiet_nanf(),
             power_pct   : telemdata.duty_cycle,
-            pos_cmd     : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.command_position              : AP::logger().quiet_nanf(),
-            voltage     : telemdata.present(TelemetryData::Types::VOLTAGE)           ? telemdata.voltage                       : AP::logger().quiet_nanf(),
-            current     : telemdata.present(TelemetryData::Types::CURRENT)           ? telemdata.current                       : AP::logger().quiet_nanf(),
-            mot_temp    : telemdata.present(TelemetryData::Types::MOTOR_TEMP)        ? telemdata.motor_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(),
-            pcb_temp    : telemdata.present(TelemetryData::Types::PCB_TEMP)          ? telemdata.pcb_temperature_cdeg * 0.01   : AP::logger().quiet_nanf(),
+            pos_cmd     : telemdata.present(TelemetryData::Types::COMMANDED_POSITION) ? telemdata.command_position              : AP::logger().quiet_nanf(),
+            voltage     : telemdata.present(TelemetryData::Types::VOLTAGE)            ? telemdata.voltage                       : AP::logger().quiet_nanf(),
+            current     : telemdata.present(TelemetryData::Types::CURRENT)            ? telemdata.current                       : AP::logger().quiet_nanf(),
+            mot_temp    : telemdata.present(TelemetryData::Types::MOTOR_TEMP)         ? telemdata.motor_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(),
+            pcb_temp    : telemdata.present(TelemetryData::Types::PCB_TEMP)           ? telemdata.pcb_temperature_cdeg * 0.01   : AP::logger().quiet_nanf(),
             error       : telemdata.status_flags,
         };
         AP::logger().WriteBlock(&pkt, sizeof(pkt));
diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem.h b/libraries/AP_Servo_Telem/AP_Servo_Telem.h
index bff0aece82..ad3e97cab6 100644
--- a/libraries/AP_Servo_Telem/AP_Servo_Telem.h
+++ b/libraries/AP_Servo_Telem/AP_Servo_Telem.h
@@ -80,5 +80,7 @@ private:
     uint32_t _last_telem_log_ms[SERVO_TELEM_MAX_SERVOS];
 
     static AP_Servo_Telem *_singleton;
+
+    uint32_t active_mask;
 };
 #endif // AP_SERVO_TELEM_ENABLED