mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_QURT: Add support for the ModalAI IO board to support PWM ESCs
This commit is contained in:
parent
0630b83478
commit
99bfc32038
|
@ -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");
|
||||||
|
|
||||||
|
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();
|
sl_client_config_uart(QURT_UART_ESC_IO, baudrate);
|
||||||
uint32_t safety_mask = 0;
|
|
||||||
|
|
||||||
if (boardconfig != nullptr) {
|
request_version = true;
|
||||||
// mask of channels to allow with safety on
|
}
|
||||||
safety_mask = boardconfig->get_safety_mask();
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue