AP_Periph: Support APD ESC telemetry

Tools/AP_Periph/AP_Periph.cpp

@@ -242,6 +242,15 @@ void AP_Periph_FW::init()
     hwesc_telem.init(hal.serial(HAL_PERIPH_HWESC_SERIAL_PORT));
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_ESC_APD
+    for (uint8_t i = 0; i < ESC_NUMBERS; i++) {
+        const uint8_t port = g.esc_serial_port[i];
+        if (port < SERIALMANAGER_NUM_PORTS) { // skip bad ports
+            apd_esc_telem[i] = new ESC_APD_Telem (hal.serial(port), g.pole_count[i]);
+        }
+    }
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_MSP
     if (g.msp_port >= 0) {
         msp_init(hal.serial(g.msp_port));

Tools/AP_Periph/AP_Periph.h

@@ -41,6 +41,8 @@
 #include "GCS_MAVLink.h"
 #endif
 
+#include "esc_apd_telem.h"
+
 #if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY)
 #define AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY
 #endif
@@ -238,6 +240,11 @@ public:
     AP_KDECAN kdecan;
 #endif
     
+#ifdef HAL_PERIPH_ENABLE_ESC_APD
+    ESC_APD_Telem *apd_esc_telem[APD_ESC_INSTANCES];
+    void apd_esc_telem_update();
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
 #if HAL_WITH_ESC_TELEM
     AP_ESC_Telem esc_telem;
@@ -329,5 +336,3 @@ namespace AP
 }
 
 extern AP_Periph_FW periph;
-
-

Tools/AP_Periph/Parameters.cpp

@@ -57,6 +57,14 @@ extern const AP_HAL::HAL &hal;
 #define MAV_SYSTEM_ID HAL_DEFAULT_MAV_SYSTEM_ID
 #endif
 
+#ifndef APD_ESC_SERIAL_0
+  #define APD_ESC_SERIAL_0 -1
+#endif
+
+#ifndef APD_ESC_SERIAL_1
+  #define APD_ESC_SERIAL_1 -1
+#endif
+
 /*
  *  AP_Periph parameter definitions
  *
@@ -327,13 +335,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(hardpoint_rate, "HARDPOINT_RATE", 100),
 #endif
 
-#ifdef HAL_PERIPH_ENABLE_HWESC
+#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD)
     // @Param: ESC_NUMBER
     // @DisplayName: ESC number
     // @Description: This is the ESC number to report as in UAVCAN ESC telemetry feedback packets.
     // @Increment: 1
     // @User: Advanced
-    GSCALAR(esc_number, "ESC_NUMBER", 0),
+    GARRAY(esc_number, 0, "ESC_NUMBER", 0),
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
@@ -513,6 +521,36 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(kdecan, "KDE_",   AP_KDECAN),
 #endif
 
+#if defined(HAL_PERIPH_ENABLE_ESC_APD)
+    GARRAY(pole_count, 0, "ESC_NUM_POLES", 22),
+#endif
+
+#if defined(HAL_PERIPH_ENABLE_ESC_APD)
+    // @Param: ESC_APD_SERIAL_1
+    // @DisplayName: ESC APD Serial 1
+    // @Description: Which serial port to use for APD ESC data
+    // @Range: 0 6
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(esc_serial_port, 0, "ESC_APD_SERIAL_1", APD_ESC_SERIAL_0),
+
+  #if APD_ESC_INSTANCES > 1
+    GARRAY(esc_number, 1, "ESC_NUMBER2", 1),
+
+    GARRAY(pole_count, 1, "ESC_NUM_POLES2", 22),
+
+    // @Param: ESC_APD_SERIAL_2
+    // @DisplayName: ESC APD Serial 2
+    // @Description: Which serial port to use for APD ESC data
+    // @Range: 0 6
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(esc_serial_port, 1, "ESC_APD_SERIAL_2", APD_ESC_SERIAL_1),
+  #endif
+#endif
+
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -30,7 +30,7 @@ public:
         k_param_hardpoint_id,
         k_param_hardpoint_rate,
         k_param_baro_enable,
-        k_param_esc_number,
+        k_param_esc_number0,
         k_param_battery,
         k_param_debug,
         k_param_serial_number,
@@ -71,6 +71,11 @@ public:
         k_param_proximity_max_rate,
         k_param_nmea,
         k_param_kdecan,
+        k_param_pole_count0,
+        k_param_esc_serial_port0,
+        k_param_esc_number1,
+        k_param_pole_count1,
+        k_param_esc_serial_port1,
     };
 
     AP_Int16 format_version;
@@ -121,8 +126,21 @@ public:
     AP_Int8 hardpoint_rate;
 #endif
 
-#ifdef HAL_PERIPH_ENABLE_HWESC
+#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD)
-    AP_Int8 esc_number;
+    #if defined ESC_NUMBERS
+        #error "ESC_NUMBERS should not have been previously defined"
+    #endif
+    #if defined(APD_ESC_INSTANCES)
+        #define ESC_NUMBERS APD_ESC_INSTANCES
+    #else
+        #define ESC_NUMBERS 2
+    #endif // defined(APD_ESC_INSTANCES)
+    AP_Int8 esc_number[ESC_NUMBERS];
+    AP_Int8 esc_serial_port[ESC_NUMBERS];
+#endif
+
+#if defined(ESC_NUMBERS)
+    AP_Int8 pole_count[ESC_NUMBERS];
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_GPS

Tools/AP_Periph/can.cpp

@@ -1671,8 +1671,42 @@ void AP_Periph_FW::esc_telem_update()
     }
 }
 #endif // HAL_WITH_ESC_TELEM
-#endif // HAL_PERIPH_ENABLE_RC_OUT
 
+#ifdef HAL_PERIPH_ENABLE_ESC_APD
+void AP_Periph_FW::apd_esc_telem_update()
+{
+    for(uint8_t i = 0; i < ARRAY_SIZE(apd_esc_telem); i++) {
+        if (apd_esc_telem[i] == nullptr) {
+            continue;
+        }
+        ESC_APD_Telem &esc = *apd_esc_telem[i];
+
+        if (esc.update()) {
+            const ESC_APD_Telem::telem &t = esc.get_telem();
+
+            uavcan_equipment_esc_Status pkt {};
+            static_assert(APD_ESC_INSTANCES <= ARRAY_SIZE(g.esc_number), "There must be an ESC instance number for each APD ESC");
+            pkt.esc_index = g.esc_number[i];
+            pkt.voltage = t.voltage;
+            pkt.current = t.current;
+            pkt.temperature = t.temperature;
+            pkt.rpm = t.rpm;
+            pkt.power_rating_pct = t.power_rating_pct;
+            pkt.error_count = t.error_count;
+
+            uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {};
+            uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout());
+            canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
+                            UAVCAN_EQUIPMENT_ESC_STATUS_ID,
+                            CANARD_TRANSFER_PRIORITY_LOW,
+                            &buffer[0],
+                            total_size);
+                }
+    }
+
+}
+#endif // HAL_PERIPH_ENABLE_ESC_APD
+#endif // HAL_PERIPH_ENABLE_RC_OUT
 
 void AP_Periph_FW::can_update()
 {
@@ -1733,6 +1767,9 @@ void AP_Periph_FW::can_update()
     #ifdef HAL_PERIPH_ENABLE_HWESC
         hwesc_telem_update();
     #endif
+#ifdef HAL_PERIPH_ENABLE_ESC_APD
+        apd_esc_telem_update();
+#endif
     #ifdef HAL_PERIPH_ENABLE_MSP
         msp_sensor_update();
     #endif

Tools/AP_Periph/esc_apd_telem.cpp

@@ -0,0 +1,97 @@
+/*
+  ESC Telemetry for the APD HV Pro ESC
+
+  Protocol is here: https://docs.powerdrives.net/products/hv_pro/uart-telemetry-output
+ */
+#include "esc_apd_telem.h"
+#include <AP_HAL/utility/sparse-endian.h>
+#include <AP_Math/crc.h>
+#include <AP_Math/definitions.h>
+#include <string.h>
+
+#ifdef HAL_PERIPH_ENABLE_ESC_APD
+
+extern const AP_HAL::HAL& hal;
+
+#define TELEM_HEADER 0x9B
+#define TELEM_LEN    0x16
+
+ESC_APD_Telem::ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles) :
+    pole_count(num_poles),
+    uart(_uart) {
+    uart->begin(115200);
+}
+
+bool ESC_APD_Telem::update() {
+    uint32_t n = uart->available();
+    if (n == 0) {
+        return false;
+    }
+
+    // don't read too much in one loop to prevent too high CPU load
+    if (n > 50) {
+        n = 50;
+    }
+
+    bool ret = false;
+
+    while (n--) {
+        uint8_t b = uart->read();
+        received.bytes[len++] = b;
+
+        // check the packet size first
+        if ((size_t)len >= sizeof(received.packet)) {
+            // we have a full packet, check the stop byte
+            if (received.packet.stop == 65535) {
+                // valid stop byte, check the CRC
+                if (crc_fletcher16(received.bytes, 18) == received.packet.checksum) {
+                    // valid packet, copy the data we need and reset length
+                    decoded.voltage = le16toh(received.packet.voltage) * 1e-2f;
+                    decoded.temperature = convert_temperature(le16toh(received.packet.temperature));
+                    decoded.current = le16toh(received.packet.bus_current) * (1 / 12.5f);
+                    decoded.rpm = le32toh(received.packet.erpm) / pole_count;
+                    decoded.power_rating_pct = le16toh(received.packet.motor_duty) * 1e-2f;
+                    ret = true;
+                    len = 0;
+                } else {
+                    // we have an invalid packet, shift it back a byte
+                    shift_buffer();
+                }
+            } else {
+                // invalid stop byte, we've lost sync, shift the packet by 1 byte
+                shift_buffer();
+            }
+            
+        }
+    }
+    return ret;
+}
+
+// shift the decode buffer left by 1 byte, and rewind the progress
+void ESC_APD_Telem::shift_buffer(void) {
+    memmove(received.bytes, received.bytes + 1, sizeof(received.bytes) - 1);
+    len--;
+}
+
+// convert the raw ESC temperature to a useful value (in Kelvin)
+// based on the 1.1 example C code found here https://docs.powerdrives.net/products/hv_pro/uart-telemetry-output
+float ESC_APD_Telem::convert_temperature(uint16_t raw) const {
+    const float series_resistor     = 10000;
+    const float nominal_resistance  = 10000;
+    const float nominal_temperature = 25;
+    const float b_coefficent        = 3455;
+
+
+    const float Rntc = series_resistor / ((4096 / float(raw)) - 1);
+
+    float temperature = Rntc / nominal_resistance;          // (R/Ro)
+    temperature = logf(temperature);                        // ln(R/Ro)
+    temperature /= b_coefficent;                            // 1/B * ln(R/Ro)
+    temperature += 1 / C_TO_KELVIN(nominal_temperature); // + (1/To)
+    temperature = 1 / temperature;                          // invert
+
+    // the example code rejected anything below 0C, or above 200C, the 200C clamp makes some sense, the below 0C is harder to accept
+    return temperature;
+}
+
+#endif // HAL_PERIPH_ENABLE_ESC_APD

Tools/AP_Periph/esc_apd_telem.h

@@ -0,0 +1,61 @@
+/*
+  ESC Telemetry for APD ESC.
+ */
+
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+
+#ifdef HAL_PERIPH_ENABLE_ESC_APD
+
+class ESC_APD_Telem {
+public:
+    ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles);
+    bool update();
+
+    CLASS_NO_COPY(ESC_APD_Telem);
+
+    struct telem {
+        uint32_t error_count;
+        float voltage;
+        float current;
+        float temperature; // kelvin
+        int32_t rpm;
+        uint8_t power_rating_pct;
+    };
+
+    const telem &get_telem(void) {
+        return decoded;
+    }
+
+private:
+    AP_HAL::UARTDriver *uart;
+
+    union {
+        struct PACKED {
+            uint16_t voltage;
+            uint16_t temperature;
+            int16_t bus_current;
+            uint16_t reserved0;
+            uint32_t erpm;
+            uint16_t input_duty;
+            uint16_t motor_duty;
+            uint16_t reserved1;
+            uint16_t checksum; // 16 bit fletcher checksum
+            uint16_t stop; // should always be 65535 on a valid packet
+        } packet;
+        uint8_t bytes[22];
+    } received;
+    static_assert(sizeof(received.packet) == sizeof(received.bytes), "The packet must be the same size as the raw buffer");
+
+    uint8_t len;
+
+    struct telem decoded;
+
+    float pole_count;
+
+    float convert_temperature(uint16_t raw) const;
+    void shift_buffer(void);
+};
+
+#endif // HAL_PERIPH_ENABLE_ESC_APD