Ap_Perph:added XC-ESC Technology DroneCan telemtry support

Tools/AP_Periph/AP_Periph.cpp

@@ -35,6 +35,10 @@
 #define HAL_PERIPH_HWESC_SERIAL_PORT 3
 #endif
 
+#ifndef HAL_PERIPH_XCESC_SERIAL_PORT
+#define HAL_PERIPH_XCESC_SERIAL_PORT 3
+#endif
+
 // not only will the code not compile without features this enables,
 // but it forms part of a series of measures to give a robust recovery
 // mechanism on AP_Periph if a bad flash occurs.
@@ -273,6 +277,10 @@ void AP_Periph_FW::init()
     hwesc_telem.init(hal.serial(HAL_PERIPH_HWESC_SERIAL_PORT));
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_XCESC
+    hwesc_telem.init(hal.serial(HAL_PERIPH_XCESC_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];

Tools/AP_Periph/AP_Periph.h

@@ -23,6 +23,7 @@
 #include "../AP_Bootloader/app_comms.h"
 #include <AP_CheckFirmware/AP_CheckFirmware.h>
 #include "hwing_esc.h"
+#include "xckj_esc.h"
 #include <AP_CANManager/AP_CAN.h>
 #include <AP_CANManager/AP_SLCANIface.h>
 #include <AP_Scripting/AP_Scripting.h>

Tools/AP_Periph/Parameters.cpp

@@ -392,7 +392,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(hardpoint_rate, "HARDPOINT_RATE", 100),
 #endif
 
-#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD)
+#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD) ||defined(HAL_PERIPH_ENABLE_XCESC)
     // @Param: ESC_NUMBER
     // @DisplayName: ESC number
     // @Description: This is the ESC number to report as in UAVCAN ESC telemetry feedback packets.

Tools/AP_Periph/Parameters.h

@@ -149,7 +149,7 @@ class Parameters {
     AP_Int8 hardpoint_rate;
 #endif
 
-#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD)
+#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD) || defined(HAL_PERIPH_ENABLE_XCESC)
     #if defined ESC_NUMBERS
         #error "ESC_NUMBERS should not have been previously defined"
     #endif

Tools/AP_Periph/can.cpp

@@ -1913,6 +1913,9 @@ void AP_Periph_FW::can_update()
     #ifdef HAL_PERIPH_ENABLE_HWESC
         hwesc_telem_update();
     #endif
+#ifdef HAL_PERIPH_ENABLE_XCESC
+        xcesc_telem_update();
+#endif
 #ifdef HAL_PERIPH_ENABLE_ESC_APD
         apd_esc_telem_update();
 #endif

Tools/AP_Periph/xckj_esc.cpp

@@ -0,0 +1,130 @@
+/*
+  This protocol only allows for one ESC per UART RX line, so using a
+  CAN node per ESC works well.
+ */
+#include "AP_Periph.h"
+#include "hwing_esc.h"
+#include <AP_HAL/utility/sparse-endian.h>
+#include <dronecan_msgs.h>
+
+#ifdef HAL_PERIPH_ENABLE_XCESC
+
+#include <SITL/SITL.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define TELEM_HEADER 0x9B
+#define TELEM_LEN    0x16
+
+XCESC_Telem::XCESC_Telem(viod)
+{
+}
+
+void XCESC_Telem::init(AP_HAL::UARTDriver* uart) {
+    uart = _uart;
+    uart->begin(19200);
+    uart->set_options(AP_HAL::UARTDriver::OPTION_PULLDOWN_RX)
+}
+
+bool XCESC_Telem::updata() {
+    uint32_t sum = uart->available();
+    if (sum == 0) {
+        retrun false;
+    }
+
+    uint32_t now = AP_HAL::millis();
+    bool frame_gap = (now - last_read_ms) > 10;
+
+    last_read_ms = now;
+
+    if (sum > 500) {
+        sum = 500;
+    }
+    if (len == 0 && !frame_gap) {
+        uart->discard_input();
+        return false;
+    }
+
+    if (frame_gap) {
+        len = 0;
+    }
+
+    bool ret = false;
+
+    while (sum--){
+        uint8_t b = uart->read();
+        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;
+}
+
+bool XCESC_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) * 5.0 / 7.0; // scale from eRPM to RPM
+    decoded.voltage = be16toh(pkt.voltage) * 0.1;
+    decoded.phase_current = int16_t(be16toh(pkt.phase_current)) * 0.01;
+    decoded.current = int16_t(be16toh(pkt.current)) * 0.01;
+    decoded.mos_temperature = pkt.mos_temperature - 40;
+    decoded.cap_temperature = pkt.cap_temperature - 40;
+    decoded.status = be16toh(pkt.status);
+    if (decoded.status != 0) {
+        decoded.error_count++;
+    }
+
+    return true;
+}
+
+void AP_Periph_FW::xcesc_telem_update()
+{
+    if (!xcesc_telem.update()) {
+        return;
+    }
+    const XCESC_Telem::XCESC& t = xcesc_telem.get_telem();
+
+    uavcan_equipment_esc_Status pkt{};
+    pkt.esc_index = g.esc_number[0]; // only supports a single ESC
+    pkt.voltage = t.voltage;
+    pkt.current = t.current;
+    pkt.temperature = C_TO_KELVIN(MAX(t.mos_temperature, t.cap_temperature));
+    pkt.rpm = t.rpm;
+    pkt.power_rating_pct = t.phase_current;
+    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, !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_XCESC

Tools/AP_Periph/xckj_esc.h

@@ -0,0 +1,67 @@
+/*
+  ESC Telemetry for XC Technology DroneCan 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_XCESC
+
+class XCESC_Telem {
+public:
+    XCESC_Telem();
+
+    void init(AP_HAL::UARTDriver* uart);
+    bool updata();
+
+    struct XCESC {
+        uint32_t counter;
+        uint16_t throt_req;
+        uint16_t throt_val;
+        uint16_t rmp;
+        float voltage;
+        float phase_current;
+        float current;
+        uint8_t mos_temperature;
+        uint8_t cap_temperature;
+        uint16_t status;
+        uint32_t error_count;
+    };
+
+    const XCESC &get_telem(void) {
+        return decoded;
+    }
+
+private:
+    AP_HAL::UARTDriver* uart;
+
+    struct Packed {
+        uint8_t header;
+        uint8_t pkt_len;
+        uint32_t counter;
+        uint16_t throt_req;
+        uint16_t throt_req;
+        uint16_t rpm;
+        uint16_t voltage;
+        int16_t current;
+        int16_t phase_current;
+        uint8_t mos_temperature;
+        uint8_t cap_temperature;
+        uint16_t status;
+        uint16_t crc;
+    }pkt;
+
+    uint8_t len;
+    uint32_t last_read_ms;
+    uint32_t error_count;
+
+    struct XCESC decoded;
+
+    bool parse_packet(void);
+    uint8_t temperature_decode(uint8_t temp_raw) const;
+};
+
+#endif //HAL_PERIPH_ENABLE_XCESC

Tools/scripts/board_list.py

@@ -181,6 +181,7 @@ def find_ap_periph_boards(self):
             "f103-HWESC",
             "f103-Trigger",
             "G4-ESC",
+            "f103-XCESC",
         ]
         ret = []
         for x in self.boards:

libraries/AP_DroneCAN/AP_DroneCAN.cpp

@@ -1124,17 +1124,14 @@ void AP_DroneCAN::SRV_push_servos()
 
     if (_ESC_armed_mask != 0) {
         // push ESCs as fast as we can
-#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
         if (option_is_set(Options::USE_HOBBYWING_ESC)) {
             SRV_send_esc_hobbywing();
-        } else
-#endif
-#if AP_DRONECAN_XCKJ_ESC_SUPPORT
-        if (option_is_set(Options::USE_XCKJ_ESC))
+        }
+        else if (option_is_set(Options::USE_XCKJ_ESC))
         {
             SRV_send_esc_xckj();
-        } else
-#endif
+        } 
+        else
         {
             SRV_send_esc();
         }