From 9be1a751da7bb5a574408efb91beb8961f400650 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 16 Nov 2024 11:12:45 +0000
Subject: [PATCH] AP_Volz_Protocol: send incomming servo telem data to new
 `AP_Servo_Telem` lib

---
 .../AP_Volz_Protocol/AP_Volz_Protocol.cpp     | 70 ++++++++-----------
 libraries/AP_Volz_Protocol/AP_Volz_Protocol.h |  6 +-
 2 files changed, 31 insertions(+), 45 deletions(-)

diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp
index 4dac4792b3..c5318ef3ce 100644
--- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp
+++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp
@@ -14,7 +14,7 @@
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
 
-#include <AP_Logger/AP_Logger.h>
+#include <AP_Servo_Telem/AP_Servo_Telem.h>
 
 // Extended Position Data Format defines -100 as 0x0080 decimal 128, we map this to a PWM of 1000 (if range is default)
 #define PWM_POSITION_MIN               1000
@@ -77,7 +77,7 @@ void AP_Volz_Protocol::init(void)
     }
 }
 
-#if HAL_LOGGING_ENABLED
+#if AP_SERVO_TELEM_ENABLED
 // Request telem data, cycling through each servo and telem item
 void AP_Volz_Protocol::request_telem()
 {
@@ -144,7 +144,7 @@ void AP_Volz_Protocol::loop()
             hal.scheduler->delay_microseconds(100);
         }
 
-#if HAL_LOGGING_ENABLED
+#if AP_SERVO_TELEM_ENABLED
         // Send a command for each servo, then one telem request
         const uint8_t num_servos = __builtin_popcount(bitmask.get());
         if (sent_count < num_servos) {
@@ -162,7 +162,7 @@ void AP_Volz_Protocol::loop()
             read_telem();
         }
 
-#else // No logging, send only
+#else // No telem, send only
         send_position_cmd();
 #endif
     }
@@ -208,7 +208,7 @@ void AP_Volz_Protocol::send_position_cmd()
 
         send_command(cmd);
 
-#if HAL_LOGGING_ENABLED
+#if AP_SERVO_TELEM_ENABLED
         {
             // Update the commanded angle
             WITH_SEMAPHORE(telem.sem);
@@ -221,7 +221,7 @@ void AP_Volz_Protocol::send_position_cmd()
     }
 }
 
-#if HAL_LOGGING_ENABLED
+#if AP_SERVO_TELEM_ENABLED
 void AP_Volz_Protocol::process_response(const CMD &cmd)
 {
     // Convert to 0 indexed
@@ -348,7 +348,7 @@ void AP_Volz_Protocol::read_telem()
     // Used up all attempts without running out of data.
     // Really should not end up here
 }
-#endif // HAL_LOGGING_ENABLED
+#endif // AP_SERVO_TELEM_ENABLED
 
 // Called each time the servo outputs are sent
 void AP_Volz_Protocol::update()
@@ -376,11 +376,11 @@ void AP_Volz_Protocol::update()
         servo_pwm[i] = (pwm == 0) ? c->get_trim() : pwm;
     }
 
-#if HAL_LOGGING_ENABLED
-    // take semaphore and log all channels at 5 Hz
-    const uint32_t now_ms = AP_HAL::millis();
-    if ((now_ms - telem.last_log_ms) > 200) {
-        telem.last_log_ms = now_ms;
+#if AP_SERVO_TELEM_ENABLED
+    // Report telem data
+    AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
+    if (servo_telem != nullptr) {
+        const uint32_t now_ms = AP_HAL::millis();
 
         WITH_SEMAPHORE(telem.sem);
         for (uint8_t i=0; i<ARRAY_SIZE(telem.data); i++) {
@@ -389,37 +389,25 @@ void AP_Volz_Protocol::update()
                 continue;
             }
 
-            // @LoggerMessage: VOLZ
-            // @Description: Volz servo data
-            // @Field: TimeUS: Time since system startup
-            // @Field: I: Instance
-            // @Field: Dang: desired angle
-            // @Field: ang: reported angle
-            // @Field: pc: primary supply current
-            // @Field: sc: secondary supply current
-            // @Field: pv: primary supply voltage
-            // @Field: sv: secondary supply voltage
-            // @Field: mt: motor temperature
-            // @Field: pt: pcb temperature
-            AP::logger().WriteStreaming("VOLZ",
-                "TimeUS,I,Dang,ang,pc,sc,pv,sv,mt,pt",
-                "s#ddAAvvOO",
-                "F000000000",
-                "QBffffffhh",
-                AP_HAL::micros64(),
-                i + 1, // convert to 1 indexed to match actuator IDs and SERVOx numbering
-                telem.data[i].desired_angle,
-                telem.data[i].angle,
-                telem.data[i].primary_current,
-                telem.data[i].secondary_current,
-                telem.data[i].primary_voltage,
-                telem.data[i].secondary_voltage,
-                telem.data[i].motor_temp_deg,
-                telem.data[i].pcb_temp_deg
-            );
+            const AP_Servo_Telem::TelemetryData telem_data {
+                .command_position = telem.data[i].desired_angle,
+                .measured_position = telem.data[i].angle,
+                .voltage = telem.data[i].primary_voltage,
+                .current = telem.data[i].primary_current,
+                .motor_temperature_cdeg = int16_t(telem.data[i].motor_temp_deg * 100),
+                .pcb_temperature_cdeg = int16_t(telem.data[i].pcb_temp_deg * 100),
+                .valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
+                               AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
+                               AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
+                               AP_Servo_Telem::TelemetryData::Types::CURRENT |
+                               AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
+                               AP_Servo_Telem::TelemetryData::Types::PCB_TEMP
+            };
+
+            servo_telem->update_telem_data(i, telem_data);
         }
     }
-#endif // HAL_LOGGING_ENABLED
+#endif // AP_SERVO_TELEM_ENABLED
 }
 
 // Return the crc for a given command packet
diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
index f14df30230..d17fa76570 100644
--- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
+++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
@@ -44,8 +44,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Param/AP_Param.h>
 #include <SRV_Channel/SRV_Channel_config.h>
-#include <AP_Logger/AP_Logger_config.h>
-
+#include <AP_Servo_Telem/AP_Servo_Telem_config.h>
 
 class AP_Volz_Protocol {
 public:
@@ -110,7 +109,7 @@ private:
     AP_Int16 range;
     bool initialised;
 
-#if HAL_LOGGING_ENABLED
+#if AP_SERVO_TELEM_ENABLED
     // Request telem data, cycling through each servo and telem item
     void request_telem();
 
@@ -143,7 +142,6 @@ private:
             int16_t motor_temp_deg;
             int16_t pcb_temp_deg;
         } data[NUM_SERVO_CHANNELS];
-        uint32_t last_log_ms;
     } telem;
 #endif