AP_Periph: add support for extended ESC DroneCAN message.

Tools/AP_Periph/AP_Periph.h

@@ -334,9 +334,15 @@ class AP_Periph_FW {
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
 #if HAL_WITH_ESC_TELEM
     AP_ESC_Telem esc_telem;
+    uint8_t get_motor_number(const uint8_t esc_number) const;
     uint32_t last_esc_telem_update_ms;
     void esc_telem_update();
     uint32_t esc_telem_update_period_ms;
+#if AP_EXTENDED_ESC_TELEM_ENABLED
+    void esc_telem_extended_update(const uint32_t &now_ms);
+    uint32_t last_esc_telem_extended_update;
+    uint8_t last_esc_telem_extended_sent_id;
+#endif
 #endif
 
     SRV_Channels servo_channels;

Tools/AP_Periph/Parameters.cpp

@@ -712,6 +712,17 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(rpm_msg_rate, "RPM_MSG_RATE", 0),
 #endif
 
+#if AP_EXTENDED_ESC_TELEM_ENABLED && HAL_WITH_ESC_TELEM
+    // @Param: ESC_EXT_TLM_RATE
+    // @DisplayName: ESC Extended telemetry message rate
+    // @Description: This is the rate at which extended ESC Telemetry will be sent across the CAN bus for each ESC
+    // @Units: Hz
+    // @Range: 0 50
+    // @Increment: 1
+    // @User: Advanced
+    GSCALAR(esc_extended_telem_rate, "ESC_EXT_TLM_RATE", AP_PERIPH_ESC_TELEM_RATE_DEFAULT / 10),
+#endif
+
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -96,6 +96,7 @@ class Parameters {
         k_param_options,
         k_param_rpm_msg_rate,
         k_param_esc_rate,
+        k_param_esc_extended_telem_rate,
     };
 
     AP_Int16 format_version;
@@ -183,6 +184,9 @@ class Parameters {
 #endif
 #if HAL_WITH_ESC_TELEM
     AP_Int32 esc_telem_rate;
+#if AP_EXTENDED_ESC_TELEM_ENABLED
+    AP_Int16 esc_extended_telem_rate;
+#endif
 #endif
 #endif
 

Tools/AP_Periph/can.cpp

@@ -1670,6 +1670,20 @@ void AP_Periph_FW::can_start()
 
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
 #if HAL_WITH_ESC_TELEM
+// try to map the ESC number to a motor number. This is needed
+// for when we have multiple CAN nodes, one for each ESC
+uint8_t AP_Periph_FW::get_motor_number(const uint8_t esc_number) const
+{
+    const auto *channel = SRV_Channels::srv_channel(esc_number);
+    // try to map the ESC number to a motor number. This is needed
+    // for when we have multiple CAN nodes, one for each ESC
+    if (channel == nullptr) {
+        return esc_number;
+    }
+    const int8_t motor_num = channel->get_motor_num();
+    return (motor_num == -1) ? esc_number : motor_num;
+}
+
 /*
   send ESC status packets based on AP_ESC_Telem
  */
@@ -1681,15 +1695,8 @@ void AP_Periph_FW::esc_telem_update()
         mask &= ~(1U<<i);
         const float nan = nanf("");
         uavcan_equipment_esc_Status pkt {};
-        const auto *channel = SRV_Channels::srv_channel(i);
-        // try to map the ESC number to a motor number. This is needed
-        // for when we have multiple CAN nodes, one for each ESC
-        if (channel == nullptr) {
-            pkt.esc_index = i;
-        } else {
-            const int8_t motor_num = channel->get_motor_num();
-            pkt.esc_index = motor_num==-1? i:motor_num;
-        }
+        pkt.esc_index = get_motor_number(i);
+
         if (!esc_telem.get_voltage(i, pkt.voltage)) {
             pkt.voltage = nan;
         }
@@ -1708,7 +1715,14 @@ void AP_Periph_FW::esc_telem_update()
         if (esc_telem.get_raw_rpm(i, rpm)) {
             pkt.rpm = rpm;
         }
-        pkt.power_rating_pct = 0;
+
+#if AP_EXTENDED_ESC_TELEM_ENABLED
+        uint8_t power_rating_pct;
+        if (esc_telem.get_power_percentage(i, power_rating_pct)) {
+            pkt.power_rating_pct = power_rating_pct;
+        }
+#endif
+
         pkt.error_count = 0;
 
         uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE];
@@ -1722,6 +1736,73 @@ void AP_Periph_FW::esc_telem_update()
 }
 #endif // HAL_WITH_ESC_TELEM
 
+#if AP_EXTENDED_ESC_TELEM_ENABLED
+void AP_Periph_FW::esc_telem_extended_update(const uint32_t &now_ms)
+{
+    if (g.esc_extended_telem_rate <= 0) {
+        // Not configured to send
+        return;
+    }
+
+    uint32_t mask = esc_telem.get_active_esc_mask();
+    if (mask == 0) {
+        // No ESCs to report
+        return;
+    }
+
+    // ESCs are sent in turn to minimise used bandwidth, to make the rate param match the status message we multiply
+    // the period such that the param gives the per-esc rate
+    const uint32_t update_period_ms = 1000 / constrain_int32(g.esc_extended_telem_rate.get() * __builtin_popcount(mask), 1, 1000);
+    if (now_ms - last_esc_telem_extended_update < update_period_ms) {
+        // Too soon!
+        return;
+    }
+    last_esc_telem_extended_update = now_ms;
+
+    for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
+        // Send each ESC in turn
+        const uint8_t index = (last_esc_telem_extended_sent_id + 1 + i) % ESC_TELEM_MAX_ESCS;
+
+        if ((mask & (1U << index)) == 0) {
+            // Not enabled
+            continue;
+        }
+
+        uavcan_equipment_esc_StatusExtended pkt {};
+
+        // Only send if we have data
+        bool have_data = false;
+
+        int16_t motor_temp_cdeg;
+        if (esc_telem.get_motor_temperature(index, motor_temp_cdeg)) {
+            // Convert from centi-degrees to degrees
+            pkt.motor_temperature_degC = motor_temp_cdeg * 0.01;
+            have_data = true;
+        }
+
+        have_data |= esc_telem.get_input_duty(index, pkt.input_pct);
+        have_data |= esc_telem.get_output_duty(index, pkt.output_pct);
+        have_data |= esc_telem.get_flags(index, pkt.status_flags);
+
+        if (have_data) {
+            pkt.esc_index = get_motor_number(index);
+
+            uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_MAX_SIZE];
+            const uint16_t total_size = uavcan_equipment_esc_StatusExtended_encode(&pkt, buffer, !canfdout());
+
+            canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_SIGNATURE,
+                                UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_ID,
+                                CANARD_TRANSFER_PRIORITY_LOW,
+                                &buffer[0],
+                                total_size);
+        }
+
+        last_esc_telem_extended_sent_id = index;
+        break;
+    }
+}
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_ESC_APD
 void AP_Periph_FW::apd_esc_telem_update()
 {

Tools/AP_Periph/rc_out.cpp

@@ -165,6 +165,9 @@ void AP_Periph_FW::rcout_update()
         last_esc_telem_update_ms = now_ms;
         esc_telem_update();
     }
+#if AP_EXTENDED_ESC_TELEM_ENABLED
+    esc_telem_extended_update(now_ms);
+#endif
 #endif
 }
 

Tools/ardupilotwaf/boards.py

@@ -941,6 +941,7 @@ def configure_env(self, cfg, env):
             AP_BATTERY_ESC_ENABLED = 1,
             HAL_PWM_COUNT = 32,
             HAL_WITH_ESC_TELEM = 1,
+            AP_EXTENDED_ESC_TELEM_ENABLED = 1,
             AP_TERRAIN_AVAILABLE = 1,
         )
 

libraries/AP_ESC_Telem/AP_ESC_Telem.cpp

@@ -238,10 +238,12 @@ bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const
 // get an individual ESC's temperature in centi-degrees if available, returns true on success
 bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const
 {
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return false;
+    }
+
     const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
-    if (esc_index >= ESC_TELEM_MAX_ESCS
-        || telemdata.stale()
-        || !(telemdata.types & (AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) {
+    if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)) {
         return false;
     }
     temp = telemdata.temperature_cdeg;
@@ -251,10 +253,12 @@ bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const
 // get an individual motor's temperature in centi-degrees if available, returns true on success
 bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const
 {
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return false;
+    }
+
     const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
-    if (esc_index >= ESC_TELEM_MAX_ESCS
-        || telemdata.stale()
-        || !(telemdata.types & (AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) {
+    if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)) {
         return false;
     }
     temp = telemdata.motor_temp_cdeg;
@@ -280,10 +284,12 @@ bool AP_ESC_Telem::get_highest_temperature(int16_t& temp) const
 // get an individual ESC's current in Ampere if available, returns true on success
 bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const
 {
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return false;
+    }
+
     const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
-    if (esc_index >= ESC_TELEM_MAX_ESCS
-        || telemdata.stale()
-        || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) {
+    if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::CURRENT)) {
         return false;
     }
     amps = telemdata.current;
@@ -293,10 +299,12 @@ bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const
 // get an individual ESC's voltage in Volt if available, returns true on success
 bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const
 {
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return false;
+    }
+
     const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
-    if (esc_index >= ESC_TELEM_MAX_ESCS
-        || telemdata.stale()
-        || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) {
+    if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) {
         return false;
     }
     volts = telemdata.voltage;
@@ -306,10 +314,12 @@ bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const
 // get an individual ESC's energy consumption in milli-Ampere.hour if available, returns true on success
 bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah) const
 {
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return false;
+    }
+
     const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
-    if (esc_index >= ESC_TELEM_MAX_ESCS
-        || telemdata.stale()
-        || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) {
+    if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) {
         return false;
     }
     consumption_mah = telemdata.consumption_mah;
@@ -319,16 +329,80 @@ bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah
 // get an individual ESC's usage time in seconds if available, returns true on success
 bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const
 {
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return false;
+    }
+
     const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
-    if (esc_index >= ESC_TELEM_MAX_ESCS
-        || telemdata.stale()
-        || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) {
+    if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::USAGE)) {
         return false;
     }
     usage_s = telemdata.usage_s;
     return true;
 }
 
+#if AP_EXTENDED_ESC_TELEM_ENABLED
+// get an individual ESC's input duty cycle if available, returns true on success
+bool AP_ESC_Telem::get_input_duty(uint8_t esc_index, uint8_t& input_duty) const
+{
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return false;
+    }
+
+    const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
+    if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) {
+        return false;
+    }
+    input_duty = telemdata.input_duty;
+    return true;
+}
+
+// get an individual ESC's output duty cycle if available, returns true on success
+bool AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) const
+{
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return false;
+    }
+
+    const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
+    if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) {
+        return false;
+    }
+    output_duty = telemdata.output_duty;
+    return true;
+}
+
+// get an individual ESC's status flags if available, returns true on success
+bool AP_ESC_Telem::get_flags(uint8_t esc_index, uint32_t& flags) const
+{
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return false;
+    }
+
+    const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
+    if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::FLAGS)) {
+        return false;
+    }
+    flags = telemdata.flags;
+    return true;
+}
+
+// get an individual ESC's percentage of output power if available, returns true on success
+bool AP_ESC_Telem::get_power_percentage(uint8_t esc_index, uint8_t& power_percentage) const
+{
+    if (esc_index >= ESC_TELEM_MAX_ESCS) {
+        return false;
+    }
+
+    const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
+    if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE)) {
+        return false;
+    }
+    power_percentage = telemdata.power_percentage;
+    return true;
+}
+#endif // AP_EXTENDED_ESC_TELEM_ENABLED
+
 // send ESC telemetry messages over MAVLink
 void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
 {

libraries/AP_ESC_Telem/AP_ESC_Telem.h

@@ -69,6 +69,20 @@ class AP_ESC_Telem {
     // get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success
     bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const;
 
+#if AP_EXTENDED_ESC_TELEM_ENABLED
+    // get an individual ESC's input duty cycle if available, returns true on success
+    bool get_input_duty(uint8_t esc_index, uint8_t& input_duty) const;
+
+    // get an individual ESC's output duty cycle if available, returns true on success
+    bool get_output_duty(uint8_t esc_index, uint8_t& output_duty) const;
+
+    // get an individual ESC's status flags if available, returns true on success
+    bool get_flags(uint8_t esc_index, uint32_t& flags) const;
+
+    // get an individual ESC's percentage of output power if available, returns true on success
+    bool get_power_percentage(uint8_t esc_index, uint8_t& power_percentage) const;
+#endif
+
     // return the average motor frequency in Hz for dynamic filtering
     float get_average_motor_frequency_hz(uint32_t servo_channel_mask) const { return get_average_motor_rpm(servo_channel_mask) * (1.0f / 60.0f); };