From 99bfc320382ebfea13fe9208bbf49f33583c1f4e Mon Sep 17 00:00:00 2001
From: Eric Katzfey <eric.katzfey@modalai.com>
Date: Mon, 9 Dec 2024 11:29:20 -0800
Subject: [PATCH] AP_HAL_QURT: Add support for the ModalAI IO board to support
 PWM ESCs

---
 libraries/AP_HAL_QURT/RCOutput.cpp | 223 ++++++++++++++++++++++++-----
 libraries/AP_HAL_QURT/RCOutput.h   |  45 +++++-
 libraries/AP_HAL_QURT/interface.h  |   2 +-
 3 files changed, 233 insertions(+), 37 deletions(-)

diff --git a/libraries/AP_HAL_QURT/RCOutput.cpp b/libraries/AP_HAL_QURT/RCOutput.cpp
index eb509d0709..70b9ceee81 100644
--- a/libraries/AP_HAL_QURT/RCOutput.cpp
+++ b/libraries/AP_HAL_QURT/RCOutput.cpp
@@ -8,19 +8,27 @@ extern const AP_HAL::HAL& hal;
 
 using namespace QURT;
 
+// ESC specific definitions
 #define ESC_PACKET_TYPE_PWM_CMD 1
 #define ESC_PACKET_TYPE_FB_RESPONSE 128
 #define ESC_PACKET_TYPE_FB_POWER_STATUS 132
 
-#define ESC_PKT_HEADER 0xAF
+// IO board specific definitions
+#define IO_PACKET_TYPE_PWM_HIRES_CMD 6
+
+// Generic definitions
+#define PKT_HEADER 0xAF
+#define PACKET_TYPE_VERSION_EXT_REQUEST 24
+#define PACKET_TYPE_VERSION_EXT_RESPONSE 131
 
 void RCOutput::init()
 {
-    fd = sl_client_config_uart(QURT_UART_ESC, baudrate);
+    baudrate = ESC_BAUDRATE;
+    fd = sl_client_config_uart(QURT_UART_ESC_IO, baudrate);
     if (fd == -1) {
-        HAP_PRINTF("Failed to open ESC UART");
+        HAP_PRINTF("Failed to open ESC / IO UART");
     }
-    HAP_PRINTF("ESC UART: %d", fd);
+    HAP_PRINTF("ESC / IO UART: %d", fd);
 }
 
 void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
@@ -77,14 +85,14 @@ void RCOutput::read(uint16_t *period_us, uint8_t len)
 }
 
 /*
-  send a packet with CRC to the ESC
+  send a packet with CRC to the ESC or IO board
  */
-void RCOutput::send_esc_packet(uint8_t type, uint8_t *data, uint16_t size)
+void RCOutput::send_packet(uint8_t type, uint8_t *data, uint16_t size)
 {
     uint16_t packet_size = size + 5;
     uint8_t out[packet_size];
 
-    out[0] = ESC_PKT_HEADER;
+    out[0] = PKT_HEADER;
     out[1] = packet_size;
     out[2] = type;
 
@@ -106,35 +114,52 @@ static int16_t pwm_to_esc(uint16_t pwm)
     return int16_t(800*p);
 }
 
-/*
-  send current commands to ESCs
- */
-void RCOutput::send_receive(void)
+void RCOutput::scan_for_hardware(void)
 {
-    if (fd == -1) {
-        return;
-    }
+    // Alternate between sending a version request and looking for the
+    // version response
+    static bool request_version = true;
+    if (request_version) {
+        HAP_PRINTF("RCOUTPUT requesting version");
 
-    AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
-    uint32_t safety_mask = 0;
+        uint8_t data = 0;
+        send_packet(PACKET_TYPE_VERSION_EXT_REQUEST, &data, 1);
+        request_version = false;
+    } else {
+        HAP_PRINTF("RCOUTPUT checking response");
 
-    if (boardconfig != nullptr) {
-        // mask of channels to allow with safety on
-        safety_mask = boardconfig->get_safety_mask();
+        check_response();
+
+        // If we still haven't discovered what HW is out there then
+        // try a different baudrate
+        if (hw_type == HWType::UNKNOWN) {
+            if (baudrate == ESC_BAUDRATE) {
+                baudrate = IO_BAUDRATE;
+            } else {
+                baudrate = ESC_BAUDRATE;
+            }
+
+            sl_client_config_uart(QURT_UART_ESC_IO, baudrate);
+
+            request_version = true;
+        }
     }
-    
+}
+
+void RCOutput::send_esc_command(void)
+{
     int16_t data[5] {};
 
-    for (uint8_t i=0; i<4; i++) {
-        uint16_t v = period[i];
-        if (safety_on && (safety_mask & (1U<<i)) == 0) {
-            // when safety is on we send 0, which allows us to still
-            // get feedback telemetry data, including battery voltage
-            v = 0;
-        }
-        data[i] = pwm_to_esc(v);
+    // We don't set any LEDs
+    data[4] = 0;
+
+    for (uint8_t i=0; i<channel_count; i++) {
+        data[i] = pwm_to_esc(pwm_output[i]);
+        // Make sure feedback request bit is cleared for all ESCs
+        data[i] &= 0xFFFE;
     }
 
+    // TODO: Not used???
     need_write = false;
 
     const uint32_t now_ms = AP_HAL::millis();
@@ -145,11 +170,137 @@ void RCOutput::send_receive(void)
         data[last_fb_idx] |= 1;
     }
 
-    send_esc_packet(ESC_PACKET_TYPE_PWM_CMD, (uint8_t *)data, sizeof(data));
+    send_packet(ESC_PACKET_TYPE_PWM_CMD, (uint8_t *)data, sizeof(data));
 
     check_response();
 }
 
+void RCOutput::send_io_command(void)
+{
+    struct PACKED {
+        uint8_t  command_type;
+        uint16_t vals[8];
+    } hires_pwm_cmd;
+
+    hires_pwm_cmd.command_type = 0;
+
+    // Resolution of commands in the packet is 0.05us = 50ns
+    // Convert from standard 1us resolution to IO command resolution
+    for (uint32_t idx=0; idx<channel_count; idx++) {
+        hires_pwm_cmd.vals[idx] = pwm_output[idx] * 20;
+    }
+    // Channels 4 - 8 not supported right now
+    for (uint32_t idx=4; idx<8; idx++) {
+        hires_pwm_cmd.vals[idx] = 0;
+    }
+
+    send_packet(IO_PACKET_TYPE_PWM_HIRES_CMD, (uint8_t *) &hires_pwm_cmd, sizeof(hires_pwm_cmd));
+}
+
+void RCOutput::send_pwm_output(void)
+{
+    AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
+    uint32_t safety_mask = 0;
+
+    if (boardconfig != nullptr) {
+        // mask of channels to allow with safety on
+        safety_mask = boardconfig->get_safety_mask();
+    }
+
+    for (uint8_t i=0; i<channel_count; i++) {
+        uint16_t v = period[i];
+        if (safety_on && (safety_mask & (1U<<i)) == 0) {
+            // when safety is on we send 0, which allows us to still
+            // get feedback telemetry data from ESC, including battery voltage
+            v = 0;
+        }
+        pwm_output[i] = v;
+    }
+
+    switch (hw_type) {
+    case HWType::ESC:
+        send_esc_command();
+        break;
+    case HWType::IO:
+        send_io_command();
+        break;
+    case HWType::UNKNOWN:
+    default:
+        break;
+    }
+}
+
+/*
+  send current commands to ESC or IO board
+ */
+void RCOutput::send_receive(void)
+{
+    // No point proceeding if we were not able to open the UART
+    if (fd == -1) {
+        return;
+    }
+
+    switch (hw_type) {
+    case HWType::UNKNOWN:
+        scan_for_hardware();
+        break;
+    case HWType::ESC:
+    case HWType::IO:
+        send_pwm_output();
+        break;
+    default:
+        return;
+    }
+}
+
+std::string RCOutput::board_id_to_name(uint16_t board_id)
+{
+    switch (board_id) {
+    case 31: return std::string("ModalAi 4-in-1 ESC V2 RevB (M0049)");
+    case 32: return std::string("Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)");
+    case 33: return std::string("Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)");
+    case 34: return std::string("ModalAi 4-in-1 ESC (M0117-1)");
+    case 35: return std::string("ModalAi I/O Expander (M0065)");
+    case 36: return std::string("ModalAi 4-in-1 ESC (M0117-3)");
+    case 37: return std::string("ModalAi 4-in-1 ESC (M0134-1)");
+    case 38: return std::string("ModalAi 4-in-1 ESC (M0134-3)");
+    case 39: return std::string("ModalAi 4-in-1 ESC (M0129-1)");
+    case 40: return std::string("ModalAi 4-in-1 ESC (M0129-3)");
+    case 41: return std::string("ModalAi 4-in-1 ESC (M0134-6)");
+    case 42: return std::string("ModalAi 4-in-1 ESC (M0138-1)");
+    default: return std::string("Unknown Board");
+    }
+}
+
+void RCOutput::handle_version_feedback(const struct extended_version_info &pkt)
+{
+    uint16_t hw_ver = pkt.hw_version;
+
+    // Check to see if we have a recognized HW id
+    if (hw_ver == 35) {
+        // We found an IO board
+        hw_type = HWType::IO;
+    } else {
+        // Just assume an ESC and don't try to compare against a list of
+        // known ESCs since that list will likely expand over time.
+        // If the hardware responded with a valid version packet at the ESC
+        // baud rate then it is very likely an ESC.
+        hw_type = HWType::ESC;
+    }
+
+    // Dump all the version information
+    HAP_PRINTF("RCOUTPUT: Board ID: %i", pkt.id);
+    HAP_PRINTF("RCOUTPUT: Board Type : %i: %s", hw_ver, board_id_to_name(hw_ver).c_str());
+
+    HAP_PRINTF("RCOUTPUT: Unique ID  : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",
+               pkt.unique_id[11], pkt.unique_id[10], pkt.unique_id[9], pkt.unique_id[8],
+               pkt.unique_id[7], pkt.unique_id[6], pkt.unique_id[5], pkt.unique_id[4],
+               pkt.unique_id[3], pkt.unique_id[2], pkt.unique_id[1], pkt.unique_id[0]);
+
+    HAP_PRINTF("RCOUTPUT: Firmware   : version %4d, hash %.12s", pkt.sw_version, pkt.firmware_git_version);
+    HAP_PRINTF("RCOUTPUT: Bootloader : version %4d, hash %.12s", pkt.bootloader_version, pkt.bootloader_git_version);
+}
+
 /*
   handle a telem feedback packet
  */
@@ -185,27 +336,35 @@ void RCOutput::handle_power_status(const struct esc_power_status &pkt)
 void RCOutput::check_response(void)
 {
     uint8_t buf[256];
-    struct PACKED esc_packet {
+    struct PACKED data_packet {
         uint8_t header;
         uint8_t length;
         uint8_t type;
         union {
+            struct extended_version_info ver_info;
             struct esc_response_v2 resp_v2;
             struct esc_power_status power_status;
         } u;
     };
     auto n = sl_client_uart_read(fd, (char *)buf, sizeof(buf));
+    // TODO: Maintain a count of total received bytes over time
+    // HAP_PRINTF("RCOUTPUT response bytes: %d", n);
     while (n >= 3) {
-        const auto *pkt = (struct esc_packet *)buf;
-        if (pkt->header != ESC_PKT_HEADER || pkt->length > n) {
+        const auto *pkt = (struct data_packet *)buf;
+        if (pkt->header != PKT_HEADER || pkt->length > n) {
             return;
         }
         const uint16_t crc = calc_crc_modbus(&pkt->length, pkt->length-3);
         const uint16_t crc2 = buf[pkt->length-2] | buf[pkt->length-1]<<8;
         if (crc != crc2) {
+            // TODO: Maintain a count of failed CRCs over time
+            // HAP_PRINTF("RCOUTPUT CRC fail on input");
             return;
         }
         switch (pkt->type) {
+        case PACKET_TYPE_VERSION_EXT_RESPONSE:
+            handle_version_feedback(pkt->u.ver_info);
+            break;
         case ESC_PACKET_TYPE_FB_RESPONSE:
             handle_esc_feedback(pkt->u.resp_v2);
             break;
diff --git a/libraries/AP_HAL_QURT/RCOutput.h b/libraries/AP_HAL_QURT/RCOutput.h
index ee3f95aac2..c2be76ccb7 100644
--- a/libraries/AP_HAL_QURT/RCOutput.h
+++ b/libraries/AP_HAL_QURT/RCOutput.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <string>
 #include <AP_HAL/AP_HAL.h>
 #include "AP_HAL_QURT.h"
 #include <AP_ESC_Telem/AP_ESC_Telem.h>
@@ -32,19 +33,51 @@ public:
     /*
       force the safety switch on, disabling output from the ESCs/servos
      */
-    bool force_safety_on(void) override { safety_on = true; return true; }
+    bool force_safety_on(void) override
+    {
+        safety_on = true;
+        return true;
+    }
 
     /*
       force the safety switch off, enabling output from the ESCs/servos
      */
-    void force_safety_off(void) override { safety_on = false; }
+    void force_safety_off(void) override
+    {
+        safety_on = false;
+    }
 
 private:
-    const uint32_t baudrate = 2000000;
+    enum {
+        IO_BAUDRATE = 921600,
+        ESC_BAUDRATE = 2000000
+    } baudrate;
 
+    enum class HWType {
+        UNKNOWN = 0,            // Unknown board type
+        ESC = 1,                // ESC
+        IO = 2,                 // IO board
+    };
+    HWType hw_type = HWType::UNKNOWN;
+
+    void scan_for_hardware(void);
     void send_receive(void);
     void check_response(void);
-    void send_esc_packet(uint8_t type, uint8_t *data, uint16_t size);
+    void send_pwm_output(void);
+    void send_io_command(void);
+    void send_esc_command(void);
+    void send_packet(uint8_t type, uint8_t *data, uint16_t size);
+
+    struct PACKED extended_version_info {
+        uint8_t  id;
+        uint16_t sw_version;
+        uint16_t hw_version;
+        uint8_t  unique_id[12];
+        char     firmware_git_version[12];
+        char     bootloader_git_version[12];
+        uint16_t bootloader_version;
+        uint16_t crc;
+    };
 
     struct PACKED esc_response_v2 {
         uint8_t  id_state;     // bits 0:3 = state, bits 4:7 = ID
@@ -64,13 +97,17 @@ private:
         int16_t  current;  //Total Current (8mA resolution)
     };
 
+    void handle_version_feedback(const struct extended_version_info &pkt);
     void handle_esc_feedback(const struct esc_response_v2 &pkt);
     void handle_power_status(const struct esc_power_status &pkt);
 
+    std::string board_id_to_name(uint16_t board_id);
+
     int fd = -1;
     uint16_t enable_mask;
     static const uint8_t channel_count = 4;
     uint16_t period[channel_count];
+    uint16_t pwm_output[channel_count];
     volatile bool need_write;
     bool corked;
     HAL_Semaphore mutex;
diff --git a/libraries/AP_HAL_QURT/interface.h b/libraries/AP_HAL_QURT/interface.h
index fb3019f127..1c919da1c2 100644
--- a/libraries/AP_HAL_QURT/interface.h
+++ b/libraries/AP_HAL_QURT/interface.h
@@ -46,4 +46,4 @@ extern "C" {
 // IDs for serial ports
 #define QURT_UART_GPS 6
 #define QURT_UART_RCIN 7
-#define QURT_UART_ESC 2
+#define QURT_UART_ESC_IO 2 // UART for the ESC or IO board that bridges to ESC