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

223
libraries/AP_HAL_QURT/RCOutput.cpp
View File

@ -8,19 +8,27 @@ extern const AP_HAL::HAL& hal;
using namespace QURT; using namespace QURT;
// ESC specific definitions
#define ESC_PACKET_TYPE_PWM_CMD 1 #define ESC_PACKET_TYPE_PWM_CMD 1
#define ESC_PACKET_TYPE_FB_RESPONSE 128 #define ESC_PACKET_TYPE_FB_RESPONSE 128
#define ESC_PACKET_TYPE_FB_POWER_STATUS 132 #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() 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) { 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) 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; uint16_t packet_size = size + 5;
uint8_t out[packet_size]; uint8_t out[packet_size];
out[0] = ESC_PKT_HEADER; out[0] = PKT_HEADER;
out[1] = packet_size; out[1] = packet_size;
out[2] = type; out[2] = type;
@ -106,35 +114,52 @@ static int16_t pwm_to_esc(uint16_t pwm)
return int16_t(800*p); return int16_t(800*p);
} }
/* void RCOutput::scan_for_hardware(void)
send current commands to ESCs
*/
void RCOutput::send_receive(void)
{ {
if (fd == -1) { // Alternate between sending a version request and looking for the
return; // version response
} static bool request_version = true;
if (request_version) {
HAP_PRINTF("RCOUTPUT requesting version");
AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton(); uint8_t data = 0;
uint32_t safety_mask = 0; send_packet(PACKET_TYPE_VERSION_EXT_REQUEST, &data, 1);
request_version = false;
} else {
HAP_PRINTF("RCOUTPUT checking response");
if (boardconfig != nullptr) { check_response();
// mask of channels to allow with safety on
safety_mask = boardconfig->get_safety_mask(); // 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] {}; int16_t data[5] {};
for (uint8_t i=0; i<4; i++) { // We don't set any LEDs
uint16_t v = period[i]; data[4] = 0;
if (safety_on && (safety_mask & (1U<<i)) == 0) {
// when safety is on we send 0, which allows us to still for (uint8_t i=0; i<channel_count; i++) {
// get feedback telemetry data, including battery voltage data[i] = pwm_to_esc(pwm_output[i]);
v = 0; // Make sure feedback request bit is cleared for all ESCs
} data[i] &= 0xFFFE;
data[i] = pwm_to_esc(v);
} }
// TODO: Not used???
need_write = false; need_write = false;
const uint32_t now_ms = AP_HAL::millis(); const uint32_t now_ms = AP_HAL::millis();
@ -145,11 +170,137 @@ void RCOutput::send_receive(void)
data[last_fb_idx] |= 1; 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(); 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 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) void RCOutput::check_response(void)
{ {
uint8_t buf[256]; uint8_t buf[256];
struct PACKED esc_packet { struct PACKED data_packet {
uint8_t header; uint8_t header;
uint8_t length; uint8_t length;
uint8_t type; uint8_t type;
union { union {
struct extended_version_info ver_info;
struct esc_response_v2 resp_v2; struct esc_response_v2 resp_v2;
struct esc_power_status power_status; struct esc_power_status power_status;
} u; } u;
}; };
auto n = sl_client_uart_read(fd, (char *)buf, sizeof(buf)); 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) { while (n >= 3) {
const auto *pkt = (struct esc_packet *)buf; const auto *pkt = (struct data_packet *)buf;
if (pkt->header != ESC_PKT_HEADER || pkt->length > n) { if (pkt->header != PKT_HEADER || pkt->length > n) {
return; return;
} }
const uint16_t crc = calc_crc_modbus(&pkt->length, pkt->length-3); 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; const uint16_t crc2 = buf[pkt->length-2] | buf[pkt->length-1]<<8;
if (crc != crc2) { if (crc != crc2) {
// TODO: Maintain a count of failed CRCs over time
// HAP_PRINTF("RCOUTPUT CRC fail on input");
return; return;
} }
switch (pkt->type) { switch (pkt->type) {
case PACKET_TYPE_VERSION_EXT_RESPONSE:
handle_version_feedback(pkt->u.ver_info);
break;
case ESC_PACKET_TYPE_FB_RESPONSE: case ESC_PACKET_TYPE_FB_RESPONSE:
handle_esc_feedback(pkt->u.resp_v2); handle_esc_feedback(pkt->u.resp_v2);
break; break;

45
libraries/AP_HAL_QURT/RCOutput.h
View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include <string>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT.h" #include "AP_HAL_QURT.h"
#include <AP_ESC_Telem/AP_ESC_Telem.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 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 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: 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 send_receive(void);
void check_response(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 { struct PACKED esc_response_v2 {
uint8_t id_state; // bits 0:3 = state, bits 4:7 = ID uint8_t id_state; // bits 0:3 = state, bits 4:7 = ID
@ -64,13 +97,17 @@ private:
int16_t current; //Total Current (8mA resolution) 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_esc_feedback(const struct esc_response_v2 &pkt);
void handle_power_status(const struct esc_power_status &pkt); void handle_power_status(const struct esc_power_status &pkt);
std::string board_id_to_name(uint16_t board_id);
int fd = -1; int fd = -1;
uint16_t enable_mask; uint16_t enable_mask;
static const uint8_t channel_count = 4; static const uint8_t channel_count = 4;
uint16_t period[channel_count]; uint16_t period[channel_count];
uint16_t pwm_output[channel_count];
volatile bool need_write; volatile bool need_write;
bool corked; bool corked;
HAL_Semaphore mutex; HAL_Semaphore mutex;

2
libraries/AP_HAL_QURT/interface.h
View File

@ -46,4 +46,4 @@ extern "C" {
// IDs for serial ports // IDs for serial ports
#define QURT_UART_GPS 6 #define QURT_UART_GPS 6
#define QURT_UART_RCIN 7 #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