AP_Periph: add support for sending servo telemety

Tools/AP_Periph/AP_Periph.h

@@ -114,6 +114,13 @@
 #undef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT
 #endif
 
+#if AP_SERVO_TELEM_ENABLED
+    #include <AP_Servo_Telem/AP_Servo_Telem.h>
+    #if !AP_PERIPH_RC_OUT_ENABLED
+      #error"'AP_SERVO_TELEM_ENABLED' requires `AP_PERIPH_RC_OUT_ENABLED`"
+    #endif
+#endif
+
 #include "Parameters.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -377,6 +384,16 @@ class AP_Periph_FW {
     void rcout_handle_safety_state(uint8_t safety_state);
 #endif
 
+#if AP_SERVO_TELEM_ENABLED
+    void servo_telem_update();
+    struct {
+        AP_Servo_Telem lib;
+        uint32_t last_update_ms;
+        uint32_t last_send_ms;
+        uint8_t last_send_index;
+    } servo_telem;
+#endif
+
 #if AP_PERIPH_RCIN_ENABLED
     void rcin_init();
     void rcin_update();

Tools/AP_Periph/Parameters.cpp

@@ -745,6 +745,17 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(servo_command_timeout_ms, "SRV_CMD_TIME_OUT", 200),
 #endif
 
+#if AP_SERVO_TELEM_ENABLED
+    // @Param: SRV_TLM_MSG_RATE
+    // @DisplayName: Servo telemetry message rate
+    // @Description: This is the rate servo telem data is sent in Hz. Zero means no send. Each servo is sent in turn.
+    // @Units: Hz
+    // @Range: 0 200
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(servo_telem_msg_rate, "SRV_TLM_MSG_RATE", 20),
+#endif
+
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -103,6 +103,7 @@ class Parameters {
         k_param__gcs,
         k_param_battery_tag,
         k_param_servo_command_timeout_ms,
+        k_param_servo_telem_msg_rate,
     };
 
     AP_Int16 format_version;
@@ -196,6 +197,9 @@ class Parameters {
 #endif
     AP_Int16 servo_command_timeout_ms;
 #endif
+#if AP_SERVO_TELEM_ENABLED
+    AP_Int16 servo_telem_msg_rate;
+#endif
 
     AP_Int8 debug;
 

Tools/AP_Periph/Servo_telem.cpp

@@ -0,0 +1,80 @@
+
+#include "AP_Periph.h"
+
+#if AP_SERVO_TELEM_ENABLED
+
+#include <dronecan_msgs.h>
+
+// Send servo telem message occasionally
+void AP_Periph_FW::servo_telem_update()
+{
+    const uint32_t now_ms = AP_HAL::millis();
+
+    // Run update function at 50hz mirroring vehicle update rate
+    if (now_ms - servo_telem.last_update_ms > 20) {
+        servo_telem.lib.update();
+        servo_telem.last_update_ms = now_ms;
+    }
+
+    // Reporting is disabled
+    if (g.servo_telem_msg_rate <= 0) {
+        return;
+    }
+
+    // Check if its time to send the next instance
+    if (now_ms - servo_telem.last_send_ms < (1000U / g.servo_telem_msg_rate)) {
+        return;
+    }
+    servo_telem.last_send_ms = now_ms;
+
+    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
+        // Send each servo in turn
+        const uint8_t index = (servo_telem.last_send_index + 1 + i) % NUM_SERVO_CHANNELS;
+
+        // Move on to next instance if no data is available
+        AP_Servo_Telem::TelemetryData data;
+        if (!servo_telem.lib.get_telem(index, data)) {
+            continue;
+        }
+
+        // Blank packet
+        const float nan = nanf("");
+        uavcan_equipment_actuator_Status pkt {
+            actuator_id: i,
+            position: nan,
+            force: nan,
+            speed: nan,
+            power_rating_pct: UAVCAN_EQUIPMENT_ACTUATOR_STATUS_POWER_RATING_PCT_UNKNOWN
+        };
+
+        // Fill in present data
+        if (data.present(AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION)) {
+            pkt.position = radians(data.measured_position);
+        }
+        if (data.present(AP_Servo_Telem::TelemetryData::Types::FORCE)) {
+            pkt.force = data.force;
+        }
+        if (data.present(AP_Servo_Telem::TelemetryData::Types::SPEED)) {
+            pkt.speed = data.speed;
+        }
+        if (data.present(AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE)) {
+            pkt.power_rating_pct = data.duty_cycle;
+        }
+
+        // Encode message into buffer
+        uint8_t buffer[UAVCAN_EQUIPMENT_ACTUATOR_STATUS_MAX_SIZE];
+        const uint16_t total_size = uavcan_equipment_actuator_Status_encode(&pkt, buffer, !canfdout());
+
+        // Send message
+        canard_broadcast(UAVCAN_EQUIPMENT_ACTUATOR_STATUS_SIGNATURE,
+                         UAVCAN_EQUIPMENT_ACTUATOR_STATUS_ID,
+                         CANARD_TRANSFER_PRIORITY_LOW,
+                         &buffer[0],
+                         total_size);
+
+        servo_telem.last_send_index = index;
+        break;
+    }
+}
+
+#endif // AP_SERVO_TELEM_ENABLED

Tools/AP_Periph/wscript

@@ -86,7 +86,8 @@ def build(bld):
                      'GCS_MAVLink',
                      'AP_Relay',
                      'AP_MultiHeap',
-                     'AP_DAC'
+                     'AP_DAC',
+                     'AP_Servo_Telem',
                      ]
 
     bld.ap_stlib(

libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h

@@ -739,3 +739,7 @@
 #ifndef HAL_USE_LOAD_MEASURE
 #define HAL_USE_LOAD_MEASURE 0
 #endif
+
+#ifndef AP_SERVO_TELEM_ENABLED
+#define AP_SERVO_TELEM_ENABLED 0
+#endif