mirror of https://github.com/ArduPilot/ardupilot
399 lines
11 KiB
C++
399 lines
11 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include "RCOutput.h"
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
|
|
|
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
|
|
|
|
// 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()
|
|
{
|
|
baudrate = ESC_BAUDRATE;
|
|
fd = sl_client_config_uart(QURT_UART_ESC_IO, baudrate);
|
|
if (fd == -1) {
|
|
HAP_PRINTF("Failed to open ESC / IO UART");
|
|
}
|
|
HAP_PRINTF("ESC / IO UART: %d", fd);
|
|
}
|
|
|
|
void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
|
|
{
|
|
// no support for changing frequency
|
|
}
|
|
|
|
uint16_t RCOutput::get_freq(uint8_t ch)
|
|
{
|
|
// return fixed fake value
|
|
return 490;
|
|
}
|
|
|
|
void RCOutput::enable_ch(uint8_t ch)
|
|
{
|
|
if (ch >= ARRAY_SIZE(period)) {
|
|
return;
|
|
}
|
|
enable_mask |= 1U<<ch;
|
|
}
|
|
|
|
void RCOutput::disable_ch(uint8_t ch)
|
|
{
|
|
if (ch >= ARRAY_SIZE(period)) {
|
|
return;
|
|
}
|
|
enable_mask &= ~1U<<ch;
|
|
}
|
|
|
|
void RCOutput::write(uint8_t ch, uint16_t period_us)
|
|
{
|
|
if (ch >= ARRAY_SIZE(period)) {
|
|
return;
|
|
}
|
|
period[ch] = period_us;
|
|
if (!corked) {
|
|
need_write = true;
|
|
}
|
|
}
|
|
|
|
uint16_t RCOutput::read(uint8_t ch)
|
|
{
|
|
if (ch >= ARRAY_SIZE(period)) {
|
|
return 0;
|
|
}
|
|
return period[ch];
|
|
}
|
|
|
|
void RCOutput::read(uint16_t *period_us, uint8_t len)
|
|
{
|
|
for (auto i = 0; i < len; i++) {
|
|
period_us[i] = read(i);
|
|
}
|
|
}
|
|
|
|
/*
|
|
send a packet with CRC to the ESC or IO board
|
|
*/
|
|
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] = PKT_HEADER;
|
|
out[1] = packet_size;
|
|
out[2] = type;
|
|
|
|
memcpy(&out[3], data, size);
|
|
|
|
uint16_t crc = calc_crc_modbus(&out[1], packet_size - 3);
|
|
|
|
memcpy(&out[packet_size - 2], &crc, sizeof(uint16_t));
|
|
|
|
sl_client_uart_write(fd, (const char *)out, packet_size);
|
|
}
|
|
|
|
/*
|
|
convert 1000 to 2000 PWM to -800 to 800 for QURT ESCs
|
|
*/
|
|
static int16_t pwm_to_esc(uint16_t pwm)
|
|
{
|
|
const float p = constrain_float((pwm-1000)*0.001, 0, 1);
|
|
return int16_t(800*p);
|
|
}
|
|
|
|
void RCOutput::scan_for_hardware(void)
|
|
{
|
|
// 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;
|
|
}
|
|
|
|
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
|
|
*/
|
|
void RCOutput::handle_esc_feedback(const struct esc_response_v2 &pkt)
|
|
{
|
|
const uint8_t idx = pkt.id_state>>4;
|
|
if (idx >= ARRAY_SIZE(period)) {
|
|
return;
|
|
}
|
|
update_rpm(idx, pkt.rpm);
|
|
|
|
AP_ESC_Telem_Backend::TelemetryData tdata {};
|
|
tdata.voltage = pkt.voltage*0.001;
|
|
tdata.current = pkt.current*0.008;
|
|
tdata.temperature_cdeg = pkt.temperature;
|
|
|
|
update_telem_data(idx, tdata,
|
|
AP_ESC_Telem_Backend::TelemetryType::CURRENT |
|
|
AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
|
|
AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
|
|
}
|
|
|
|
/*
|
|
handle a power status packet, making it available to AnalogIn
|
|
*/
|
|
void RCOutput::handle_power_status(const struct esc_power_status &pkt)
|
|
{
|
|
esc_voltage = pkt.voltage * 0.001;
|
|
esc_current = pkt.current * 0.008;
|
|
}
|
|
|
|
// check for responses
|
|
void RCOutput::check_response(void)
|
|
{
|
|
uint8_t buf[256];
|
|
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 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;
|
|
case ESC_PACKET_TYPE_FB_POWER_STATUS:
|
|
handle_power_status(pkt->u.power_status);
|
|
break;
|
|
default:
|
|
HAP_PRINTF("Unknown pkt %u", pkt->type);
|
|
break;
|
|
}
|
|
if (n == pkt->length) {
|
|
break;
|
|
}
|
|
memmove(&buf[0], &buf[pkt->length], n - pkt->length);
|
|
n -= pkt->length;
|
|
}
|
|
}
|
|
|
|
void RCOutput::cork(void)
|
|
{
|
|
corked = true;
|
|
}
|
|
|
|
void RCOutput::push(void)
|
|
{
|
|
if (corked) {
|
|
corked = false;
|
|
need_write = true;
|
|
send_receive();
|
|
}
|
|
}
|