AP_Servo_Telem: add getter for telem struct

libraries/AP_Servo_Telem/AP_Servo_Telem.cpp

@@ -152,6 +152,23 @@ void AP_Servo_Telem::write_log()
 }
 #endif  // HAL_LOGGING_ENABLED
 
+// Fill in telem structure if telem is available, return false if not
+bool AP_Servo_Telem::get_telem(const uint8_t servo_index, TelemetryData& telem) const volatile
+{
+    // Check for valid index
+    if (servo_index >= ARRAY_SIZE(_telem_data)) {
+        return false;
+    }
+
+    // Check if data has ever been received for the servo index provided
+    if ((active_mask & (1U << servo_index)) == 0) {
+        return false;
+    }
+
+    telem = *const_cast<TelemetryData*>(&_telem_data[servo_index]);
+    return true;
+}
+
 // Get the AP_Servo_Telem singleton
 AP_Servo_Telem *AP_Servo_Telem::get_singleton()
 {

libraries/AP_Servo_Telem/AP_Servo_Telem.h

@@ -70,6 +70,9 @@ public:
     // callback to update the data in the frontend, should be called by the driver when new data is available
     void update_telem_data(const uint8_t servo_index, const TelemetryData& new_data);
 
+    // Fill in telem structure if telem is available, return false if not
+    bool get_telem(const uint8_t servo_index, TelemetryData& telem) const volatile;
+
 private:
 
     // Log telem of each servo