Skip to content

Commit

Permalink
AP_HAL_QURT: Add support for the ModalAI IO board to support PWM ESCs
Browse files Browse the repository at this point in the history
  • Loading branch information
@katzfey
katzfey committed Dec 9, 2024
1 parent 4d31a73 commit 35d6782
Show file tree
Hide file tree
Showing 3 changed files with 237 additions and 35 deletions.
225 changes: 195 additions & 30 deletions libraries/AP_HAL_QURT/RCOutput.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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;

Expand All @@ -106,48 +114,197 @@ 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");

uint8_t data = 0;
send_packet(PACKET_TYPE_VERSION_EXT_REQUEST, &data, 1);
request_version = false;
} else {
HAP_PRINTF("RCOUTPUT checking response");

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] {};

// 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();
if (now_ms - last_fb_req_ms > 5) {
last_fb_req_ms = now_ms;
// request feedback from one ESC
last_fb_idx = (last_fb_idx+1) % 4;
data[last_fb_idx] |= 1;
}

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;
}

HAP_PRINTF("RCOUTPUT IO pwm output: %d, %d, %d, %d",
(uint32_t) hires_pwm_cmd.vals[0],
(uint32_t) hires_pwm_cmd.vals[1],
(uint32_t) hires_pwm_cmd.vals[2],
(uint32_t) hires_pwm_cmd.vals[3]);

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();
}

int16_t data[5] {};

for (uint8_t i=0; i<4; i++) {
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, including battery voltage
// get feedback telemetry data from ESC, including battery voltage
v = 0;
}
data[i] = pwm_to_esc(v);
pwm_output[i] = v;
}

need_write = false;
switch (hw_type) {
case HWType::ESC:
send_esc_command();
break;
case HWType::IO:
send_io_command();
break;
case HWType::UNKNOWN:
default:
break;
}
}

const uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_fb_req_ms > 5) {
last_fb_req_ms = now_ms;
// request feedback from one ESC
last_fb_idx = (last_fb_idx+1) % 4;
data[last_fb_idx] |= 1;
/*
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;
}

send_esc_packet(ESC_PACKET_TYPE_PWM_CMD, (uint8_t *)data, sizeof(data));
switch (hw_type) {
case HWType::UNKNOWN:
scan_for_hardware();
break;
case HWType::ESC:
case HWType::IO:
send_pwm_output();
break;
default:
return;
}
}

check_response();
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);
}

/*
Expand Down Expand Up @@ -185,27 +342,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;
Expand Down
45 changes: 41 additions & 4 deletions libraries/AP_HAL_QURT/RCOutput.h
View file
Original file line number Diff line number Diff line change
@@ -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>
Expand Down Expand Up @@ -32,19 +33,51 @@ class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend
/*
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
Expand All @@ -64,13 +97,17 @@ class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend
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;
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_HAL_QURT/interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

0 comments on commit 35d6782

Please sign in to comment.