AP_Periph: added Hobbywing ESC telemetry support

useful to add UAVCAN ESC status for UART based telemetry for Hobbywing
ESCs

Tools/AP_Periph/AP_Periph.cpp

@@ -136,6 +136,10 @@ void AP_Periph_FW::init()
 #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
     pwm_hardpoint_init();
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_HWESC
+    hwesc_telem.init(hal.uartB);
+#endif
     
     start_ms = AP_HAL::millis();
 }

Tools/AP_Periph/AP_Periph.h

@@ -6,6 +6,7 @@
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include "../AP_Bootloader/app_comms.h"
+#include "hwing_esc.h"
 
 #if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_NCP5623_LED)
 #define AP_PERIPH_HAVE_LED
@@ -81,6 +82,11 @@ public:
     } pwm_hardpoint;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_HWESC
+    HWESC_Telem hwesc_telem;
+    void hwesc_telem_update();
+#endif
+    
     // setup the var_info table
     AP_Param param_loader{var_info};
 

Tools/AP_Periph/Parameters.cpp

@@ -94,6 +94,10 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(hardpoint_id, "HARDPOINT_ID", HAL_PWM_HARDPOINT_ID_DEFAULT),
     GSCALAR(hardpoint_rate, "HARDPOINT_RATE", 100),
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_HWESC
+    GSCALAR(esc_number, "ESC_NUMBER", 0),
+#endif
     
     AP_VAREND
 };

Tools/AP_Periph/Parameters.h

@@ -27,6 +27,7 @@ public:
         k_param_hardpoint_id,
         k_param_hardpoint_rate,
         k_param_baro_enable,
+        k_param_esc_number,
     };
 
     AP_Int16 format_version;
@@ -57,6 +58,10 @@ public:
     AP_Int16 hardpoint_id;
     AP_Int8 hardpoint_rate;
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_HWESC
+    AP_Int8 esc_number;
+#endif
     
     Parameters() {}
 };

Tools/AP_Periph/can.cpp

@@ -38,6 +38,7 @@
 #include <uavcan/equipment/indication/LightsCommand.h>
 #include <uavcan/equipment/range_sensor/Measurement.h>
 #include <uavcan/equipment/hardpoint/Command.h>
+#include <uavcan/equipment/esc/Status.h>
 #include <ardupilot/indication/SafetyState.h>
 #include <ardupilot/indication/Button.h>
 #include <ardupilot/equipment/trafficmonitor/TrafficReport.h>
@@ -1104,6 +1105,38 @@ void AP_Periph_FW::pwm_hardpoint_update()
 }
 #endif // HAL_PERIPH_ENABLE_PWM_HARDPOINT
 
+#ifdef HAL_PERIPH_ENABLE_HWESC
+void AP_Periph_FW::hwesc_telem_update()
+{
+    if (!hwesc_telem.update()) {
+        return;
+    }
+    const HWESC_Telem::HWESC &t = hwesc_telem.get_telem();
+
+    uavcan_equipment_esc_Status pkt {};
+    pkt.esc_index = g.esc_number;
+    pkt.voltage = t.voltage;
+    pkt.current = t.current;
+    pkt.temperature = t.temperature;
+    pkt.rpm = t.rpm;
+    pkt.power_rating_pct = (t.load & 0x7F);
+
+    fix_float16(pkt.voltage);
+    fix_float16(pkt.current);
+    fix_float16(pkt.temperature);
+
+    uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE];
+    uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer);
+    canardBroadcast(&canard,
+                    UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
+                    UAVCAN_EQUIPMENT_ESC_STATUS_ID,
+                    &transfer_id,
+                    CANARD_TRANSFER_PRIORITY_LOW,
+                    &buffer[0],
+                    total_size);
+}
+#endif // HAL_PERIPH_ENABLE_HWESC
+
 
 void AP_Periph_FW::can_update()
 {
@@ -1130,6 +1163,10 @@ void AP_Periph_FW::can_update()
 #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
     pwm_hardpoint_update();
 #endif
+#ifdef HAL_PERIPH_ENABLE_HWESC
+    hwesc_telem_update();
+#endif
+
     processTx();
     processRx();
 }

Tools/AP_Periph/hwing_esc.cpp

@@ -0,0 +1,117 @@
+/*
+  ESC Telemetry for Hobbywing Pro 80A HV ESC. This will be
+  incorporated into a broader ESC telemetry library in ArduPilot
+  master in the future
+
+  This protocol only allows for one ESC per UART RX line, so using a
+  CAN node per ESC works well.
+ */
+#include "hwing_esc.h"
+#include <AP_HAL/utility/sparse-endian.h>
+
+#ifdef HAL_PERIPH_ENABLE_HWESC
+
+extern const AP_HAL::HAL& hal;
+
+#define TELEM_HEADER 0x9B
+#define TELEM_LEN    0x16
+
+// constructor
+HWESC_Telem::HWESC_Telem(void)
+{
+}
+
+void HWESC_Telem::init(AP_HAL::UARTDriver *_uart)
+{
+    uart = _uart;
+    uart->begin(19200);
+    uart->set_options(AP_HAL::UARTDriver::OPTION_PULLDOWN_RX);
+}
+
+/*
+  update ESC telemetry
+ */
+bool HWESC_Telem::update()
+{
+    uint32_t n = uart->available();
+    if (n == 0) {
+        return false;
+    }
+
+    // we expect at least 50ms idle between frames
+    uint32_t now = AP_HAL::millis();
+    bool frame_gap = (now - last_read_ms) > 10;
+
+    last_read_ms = now;
+
+    // don't read too much in one loop to prevent too high CPU load
+    if (n > 500) {
+        n = 500;
+    }
+    if (len == 0 && !frame_gap) {
+        // discard
+        while (n--) {
+            uart->read();
+        }
+        return false;
+    }
+
+    if (frame_gap) {
+        len = 0;
+    }
+
+    bool ret = false;
+
+    while (n--) {
+        uint8_t b = uart->read();
+        //hal.console->printf("t=%u 0x%02x\n", now, b);
+        if (len == 0 && b != TELEM_HEADER) {
+            continue;
+        }
+        if (len == 1 && b != TELEM_LEN) {
+            continue;
+        }
+        uint8_t *buf = (uint8_t *)&pkt;
+        buf[len++] = b;
+        if (len == sizeof(pkt)) {
+            ret = parse_packet();
+            len = 0;
+        }
+    }
+    return ret;
+}
+
+static uint16_t calc_crc(const uint8_t *buf, uint8_t len)
+{
+    uint16_t crc = 0;
+    while (len--) {
+        crc += *buf++;
+    }
+    return crc;
+}
+
+/*
+  parse packet
+ */
+bool HWESC_Telem::parse_packet(void)
+{
+    uint16_t crc = calc_crc((uint8_t *)&pkt, sizeof(pkt)-2);
+    if (crc != pkt.crc) {
+        return false;
+    }
+
+    decoded.counter = be32toh(pkt.counter);
+    decoded.throttle_req = be16toh(pkt.throttle_req);
+    decoded.throttle = be16toh(pkt.throttle);
+    decoded.rpm = be16toh(pkt.rpm);
+    decoded.voltage = be16toh(pkt.voltage) * 0.1;
+    decoded.load = be16toh(pkt.load);
+    decoded.current = be16toh(pkt.current);
+    decoded.temperature = be16toh(pkt.temperature);
+    decoded.unknown = be16toh(pkt.unknown);
+
+    return true;
+}
+
+#endif // HAL_PERIPH_ENABLE_HWESC
+

Tools/AP_Periph/hwing_esc.h

@@ -0,0 +1,62 @@
+/*
+  ESC Telemetry for Hobbywing Pro 80A HV ESC. This will be
+  incorporated into a broader ESC telemetry library in ArduPilot
+  master in the future
+ */
+
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+
+#ifdef HAL_PERIPH_ENABLE_HWESC
+
+class HWESC_Telem {
+public:
+    HWESC_Telem();
+
+    void init(AP_HAL::UARTDriver *uart);
+    bool update();
+
+    struct HWESC {
+        uint32_t counter;
+        uint16_t throttle_req;
+        uint16_t throttle;
+        uint16_t rpm;
+        float voltage;
+        uint16_t load;
+        float current;
+        uint16_t temperature;
+        uint16_t unknown;
+    };
+
+    const HWESC &get_telem(void) {
+        return decoded;
+    }
+
+private:
+    AP_HAL::UARTDriver *uart;
+
+    struct PACKED {
+        uint8_t header; // 0x9B
+        uint8_t pkt_len; // 0x16
+        uint32_t counter;
+        uint16_t throttle_req;
+        uint16_t throttle;
+        uint16_t rpm;
+        uint16_t voltage;
+        uint16_t load;
+        uint16_t current;
+        uint16_t temperature;
+        uint16_t unknown;
+        uint16_t crc;
+    } pkt;
+
+    uint8_t len;
+    uint32_t last_read_ms;
+
+    struct HWESC decoded;
+
+    bool parse_packet(void);
+};
+
+#endif // HAL_PERIPH_ENABLE_HWESC