diff --git a/libraries/AP_HAL_QURT/RCOutput.cpp b/libraries/AP_HAL_QURT/RCOutput.cpp index eb509d0709..70b9ceee81 100644 --- a/libraries/AP_HAL_QURT/RCOutput.cpp +++ b/libraries/AP_HAL_QURT/RCOutput.cpp @@ -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"); - AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton(); - uint32_t safety_mask = 0; + uint8_t data = 0; + send_packet(PACKET_TYPE_VERSION_EXT_REQUEST, &data, 1); + request_version = false; + } else { + HAP_PRINTF("RCOUTPUT checking response"); - if (boardconfig != nullptr) { - // mask of channels to allow with safety on - safety_mask = boardconfig->get_safety_mask(); + 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] {}; - for (uint8_t i=0; i<4; i++) { - uint16_t v = period[i]; - if (safety_on && (safety_mask & (1U<get_safety_mask(); + } + + for (uint8_t i=0; i= 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; diff --git a/libraries/AP_HAL_QURT/RCOutput.h b/libraries/AP_HAL_QURT/RCOutput.h index ee3f95aac2..c2be76ccb7 100644 --- a/libraries/AP_HAL_QURT/RCOutput.h +++ b/libraries/AP_HAL_QURT/RCOutput.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include "AP_HAL_QURT.h" #include @@ -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; diff --git a/libraries/AP_HAL_QURT/interface.h b/libraries/AP_HAL_QURT/interface.h index fb3019f127..1c919da1c2 100644 --- a/libraries/AP_HAL_QURT/interface.h +++ b/libraries/AP_HAL_QURT/interface.h @@ -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