AP_Periph: add support for sending servo telemety

Tools/AP_Periph/AP_Periph.cpp

@@ -513,10 +513,6 @@ void AP_Periph_FW::update()
     rcin_update();
 #endif
 
-#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
-    actuator_telem.update();
-#endif
-
 #if AP_PERIPH_BATTERY_BALANCE_ENABLED
     batt_balance_update();
 #endif

Tools/AP_Periph/AP_Periph.h

@@ -116,6 +116,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
@@ -379,6 +386,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

@@ -751,6 +751,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

@@ -104,6 +104,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;
@@ -197,6 +198,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,86 @@
+
+#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) {
+        // Update periph only actuator telem
+#if AP_PERIPH_ACTUATOR_TELEM_ENABLED
+        actuator_telem.update();
+#endif
+
+        // Update main servo telem lib
+        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: (uint8_t)(index + 1), // One based indexing for DroneCAN transport
+            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/actuator_telem.cpp

@@ -45,14 +45,11 @@ extern const AP_HAL::HAL &hal;
 #define AP_PERIPH_ACTUATOR_TELEM_CURR_MAX_DEFAULT 2.5f
 #endif
 
+#if AP_SERVO_TELEM_ENABLED == 0
+    #error "AP_PERIPH_ACTUATOR_TELEM_ENABLED requires AP_SERVO_TELEM_ENABLED"
+#endif
+
 const AP_Param::GroupInfo ActuatorTelem::var_info[] = {
-    // @Param: _TELEM_RATE
-    // @DisplayName: Actuator Telemetry rate
-    // @Description: Actuator Telemetry update rate in Hz. Set to 0 to disable.
-    // @Units: Hz
-    // @Range: 0 100
-    // @User: Standard
-    AP_GROUPINFO("_TELEM_RATE", 1, ActuatorTelem, rate, AP_PERIPH_ACTUATOR_TELEM_RATE_DEFAULT),
 
     // @Param: _NUM_CHANS
     // @DisplayName: Number of actuator channels
@@ -129,71 +126,48 @@ void ActuatorTelem::send_telemetry(uint8_t channel_index, uint8_t actuator_id)
         return;
     }
     // Read current from analog input
-    float current_amp = 0;
-    float adc_voltage = source->voltage_average();
-    current_amp = (adc_voltage - curr_amp_offset) * curr_amp_per_volt;
-
-    uavcan_equipment_actuator_Status pkt {}; 
+    const float adc_voltage = source->voltage_average();
+    const float current_amp = (adc_voltage - curr_amp_offset) * curr_amp_per_volt;
 
-    pkt.actuator_id = actuator_id;
-    pkt.position = nanf(""); // Not available
-    pkt.force = nanf("");    // Not available
-    pkt.speed = nanf("");    // Not available
+    AP_Servo_Telem::TelemetryData telem_data {
+        .current = current_amp,
+        .present_types = AP_Servo_Telem::TelemetryData::Types::CURRENT
+    };
 
     // Calculate power rating percentage from current
     const float max_current = curr_max.get();
-    if (max_current > 0 && current_amp >= 0) {
-        pkt.power_rating_pct = constrain_int16(current_amp / max_current * 100.0f, 0, 100);
-    } else {
-        pkt.power_rating_pct = UAVCAN_EQUIPMENT_ACTUATOR_STATUS_POWER_RATING_PCT_UNKNOWN;
+    if ((max_current > 0) && (current_amp >= 0)) {
+        telem_data.duty_cycle = constrain_float(current_amp / max_current * 100.0f, 0, 100);
+        telem_data.present_types |= AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE;
     }
 
-    // encode and broadcast
-    uint8_t buffer[UAVCAN_EQUIPMENT_ACTUATOR_STATUS_MAX_SIZE];
-    uint16_t total_size = uavcan_equipment_actuator_Status_encode(&pkt, buffer, !periph.canfdout());
+    periph.servo_telem.lib.update_telem_data(actuator_id, telem_data);
 
-    periph.canard_broadcast(UAVCAN_EQUIPMENT_ACTUATOR_STATUS_SIGNATURE,
-                            UAVCAN_EQUIPMENT_ACTUATOR_STATUS_ID,
-                            CANARD_TRANSFER_PRIORITY_LOW,
-                            &buffer[0],
-                            total_size);
 }
 
 void ActuatorTelem::update()
 {
-    // Check global telemetry rate
-    const float telem_rate = rate.get();
-    if (telem_rate <= 0) {
-        return;  // telemetry disabled
-    }
-
-    const uint32_t now = AP_HAL::millis();
-    if (now - last_telem_update_ms < 1000.0f / telem_rate) {
+    const int8_t num_chans_val = num_chans.get();
+    if (num_chans_val <= 0) {
         return;
     }
-    last_telem_update_ms = now;
 
-#if HAL_PWM_COUNT > 0
-    const int8_t num_chans_val = num_chans.get();
-    if (num_chans_val > 0) {
-        for (uint8_t i = 0; i < MIN(HAL_PWM_COUNT, num_chans_val); i++) {
-            const auto *srv_channel = periph.servo_channels.srv_channel(i);
-            if (srv_channel == nullptr) {
-                continue;
-            }
+    for (uint8_t i = 0; i < MIN(HAL_PWM_COUNT, num_chans_val); i++) {
+        const auto *srv_channel = periph.servo_channels.srv_channel(i);
+        if (srv_channel == nullptr) {
+            continue;
+        }
 
-            const SRV_Channel::Function function = srv_channel->get_function();
-            // Only send for configured actuator functions
-            if (function < SRV_Channel::k_rcin1 || function > SRV_Channel::k_rcin16) {
-                continue;
-            }
+        const SRV_Channel::Function function = srv_channel->get_function();
+        // Only send for configured actuator functions
+        if (function < SRV_Channel::k_rcin1 || function > SRV_Channel::k_rcin16) {
+            continue;
+        }
 
-            const uint8_t actuator_id = function - SRV_Channel::k_rcin1 + 1;
+        const uint8_t actuator_id = function - SRV_Channel::k_rcin1;
 
-            send_telemetry(i, actuator_id);
-        }
+        send_telemetry(i, actuator_id);
     }
-#endif
 }
 
 #endif  // AP_PERIPH_ACTUATOR_TELEM_ENABLED

Tools/AP_Periph/actuator_telem.h

@@ -9,7 +9,6 @@
 // Manager for all actuator channels
 class ActuatorTelem {
 public:
-    friend class AP_Periph_FW;
     ActuatorTelem(void);
 
     void init(void);
@@ -19,13 +18,11 @@ class ActuatorTelem {
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
-    AP_Int16 rate;
     AP_Int8 num_chans;
     AP_Int8 curr_pin1;
     AP_Float curr_amp_offset;
     AP_Float curr_amp_per_volt;
     AP_Float curr_max;
-    uint32_t last_telem_update_ms;
     AP_HAL::AnalogSource *analog_sources[HAL_ACTUATOR_TELEM_CURR_MAX_CHANNELS];
 };
 

Tools/AP_Periph/can.cpp

@@ -2001,6 +2001,9 @@ void AP_Periph_FW::can_update()
 #endif
 #if AP_PERIPH_RPM_STREAM_ENABLED
         rpm_sensor_send();
+#endif
+#if AP_SERVO_TELEM_ENABLED
+        servo_telem_update();
 #endif
     }
     const uint32_t now_us = AP_HAL::micros();

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_DroneCAN/AP_DroneCAN.cpp

@@ -1408,7 +1408,7 @@ void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const
                          AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE
     };
 
-    servo_telem->update_telem_data(msg.actuator_id, telem_data);
+    servo_telem->update_telem_data(msg.actuator_id - 1, telem_data);
 }
 #endif
 
@@ -1440,7 +1440,7 @@ void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, cons
                          AP_Servo_Telem::TelemetryData::Types::STATUS
     };
 
-    servo_telem->update_telem_data(msg.servo_id, telem_data);
+    servo_telem->update_telem_data(msg.servo_id - 1, telem_data);
 }
 #endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT
 
@@ -1465,7 +1465,7 @@ void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer,
                          AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP
     };
 
-    servo_telem->update_telem_data(msg.actuator_id, telem_data);
+    servo_telem->update_telem_data(msg.actuator_id - 1, telem_data);
 }
 #endif
 

libraries/AP_HAL_ChibiOS/hwdef/VM-L431-SRV-Hub-4CHP/defaults.parm

@@ -1,4 +1,4 @@
-ACT_TELEM_RATE 10
+SRV_TLM_MSG_RATE 40
 ACT_NUM_CHANS 4
 ACT_CURR_PIN1 9
 ACT_AMP_OFFSET 0

libraries/AP_HAL_ChibiOS/hwdef/VM-L431-SRV-Hub-4CHP/hwdef.dat

@@ -12,6 +12,7 @@ define HAL_USE_ADC TRUE
 
 #Servo VCC Current Sampling
 define AP_PERIPH_ACTUATOR_TELEM_ENABLED 1
+define AP_SERVO_TELEM_ENABLED 1
 define STM32_ADC_USE_ADC1 TRUE
 PA4 ACT1_CURRENT_SENS ADC1 SCALE(1) # Current Sampling on PWM(1)
 PA5 ACT2_CURRENT_SENS ADC1 SCALE(1) # Current Sampling on PWM(2)

libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h

@@ -745,3 +745,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

libraries/AP_Servo_Telem/AP_Servo_Telem.h

@@ -9,7 +9,16 @@
 
 
 #ifndef SERVO_TELEM_MAX_SERVOS
-    #define SERVO_TELEM_MAX_SERVOS NUM_SERVO_CHANNELS
+    #ifndef HAL_BUILD_AP_PERIPH
+        // On a vehicle board we only expect responses from those servos we directly control
+        #define SERVO_TELEM_MAX_SERVOS NUM_SERVO_CHANNELS
+    #else
+        // On periph we handle a sub-set of outputs from a vehicle, because we don't know
+        // which indexes those will be we have to support more telem channels than outputs
+        // This allows output ID 10 to be output on the first servo channel.
+        // This should be reported to the vehicle as ID 10 not ID 1.
+        #define SERVO_TELEM_MAX_SERVOS 32
+    #endif
 #endif
 static_assert(SERVO_TELEM_MAX_SERVOS > 0, "Cannot have 0 Servo telem instances");