Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_QURT: Add support for the ModalAI IO board to support PWM ESCs

This commit is contained in:
Eric Katzfey 2024-12-09 11:29:20 -08:00 committed by Andrew Tridgell
parent 0630b83478
commit 99bfc32038
3 changed files with 233 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

219
libraries/AP_HAL_QURT/RCOutput.cpp
View File

@ -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");
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;
}
AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
uint32_t safety_mask = 0;
sl_client_config_uart(QURT_UART_ESC_IO, baudrate);
if (boardconfig != nullptr) {
// mask of channels to allow with safety on
safety_mask = boardconfig->get_safety_mask();
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;

45
libraries/AP_HAL_QURT/RCOutput.h
View File

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

2
libraries/AP_HAL_QURT/interface.h
View File

@ -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