diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index bd74531deb..d1a36bf02b 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -136,6 +136,10 @@ void AP_Periph_FW::init() #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT pwm_hardpoint_init(); #endif + +#ifdef HAL_PERIPH_ENABLE_HWESC + hwesc_telem.init(hal.uartB); +#endif start_ms = AP_HAL::millis(); } diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index d1d44a7d0c..7271b62201 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -6,6 +6,7 @@ #include #include #include "../AP_Bootloader/app_comms.h" +#include "hwing_esc.h" #if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_NCP5623_LED) #define AP_PERIPH_HAVE_LED @@ -81,6 +82,11 @@ public: } pwm_hardpoint; #endif +#ifdef HAL_PERIPH_ENABLE_HWESC + HWESC_Telem hwesc_telem; + void hwesc_telem_update(); +#endif + // setup the var_info table AP_Param param_loader{var_info}; diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index b3966acd46..8a09c68b97 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -94,6 +94,10 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(hardpoint_id, "HARDPOINT_ID", HAL_PWM_HARDPOINT_ID_DEFAULT), GSCALAR(hardpoint_rate, "HARDPOINT_RATE", 100), #endif + +#ifdef HAL_PERIPH_ENABLE_HWESC + GSCALAR(esc_number, "ESC_NUMBER", 0), +#endif AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index a8ceb91809..b729915377 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -27,6 +27,7 @@ public: k_param_hardpoint_id, k_param_hardpoint_rate, k_param_baro_enable, + k_param_esc_number, }; AP_Int16 format_version; @@ -57,6 +58,10 @@ public: AP_Int16 hardpoint_id; AP_Int8 hardpoint_rate; #endif + +#ifdef HAL_PERIPH_ENABLE_HWESC + AP_Int8 esc_number; +#endif Parameters() {} }; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 400c471254..e83e9d8b83 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -1104,6 +1105,38 @@ void AP_Periph_FW::pwm_hardpoint_update() } #endif // HAL_PERIPH_ENABLE_PWM_HARDPOINT +#ifdef HAL_PERIPH_ENABLE_HWESC +void AP_Periph_FW::hwesc_telem_update() +{ + if (!hwesc_telem.update()) { + return; + } + const HWESC_Telem::HWESC &t = hwesc_telem.get_telem(); + + uavcan_equipment_esc_Status pkt {}; + pkt.esc_index = g.esc_number; + pkt.voltage = t.voltage; + pkt.current = t.current; + pkt.temperature = t.temperature; + pkt.rpm = t.rpm; + pkt.power_rating_pct = (t.load & 0x7F); + + fix_float16(pkt.voltage); + fix_float16(pkt.current); + fix_float16(pkt.temperature); + + uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE]; + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer); + canardBroadcast(&canard, + UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); +} +#endif // HAL_PERIPH_ENABLE_HWESC + void AP_Periph_FW::can_update() { @@ -1130,6 +1163,10 @@ void AP_Periph_FW::can_update() #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT pwm_hardpoint_update(); #endif +#ifdef HAL_PERIPH_ENABLE_HWESC + hwesc_telem_update(); +#endif + processTx(); processRx(); } diff --git a/Tools/AP_Periph/hwing_esc.cpp b/Tools/AP_Periph/hwing_esc.cpp new file mode 100644 index 0000000000..dd128f4c39 --- /dev/null +++ b/Tools/AP_Periph/hwing_esc.cpp @@ -0,0 +1,117 @@ +/* + ESC Telemetry for Hobbywing Pro 80A HV ESC. This will be + incorporated into a broader ESC telemetry library in ArduPilot + master in the future + + This protocol only allows for one ESC per UART RX line, so using a + CAN node per ESC works well. + */ +#include "hwing_esc.h" +#include + +#ifdef HAL_PERIPH_ENABLE_HWESC + +extern const AP_HAL::HAL& hal; + +#define TELEM_HEADER 0x9B +#define TELEM_LEN 0x16 + +// constructor +HWESC_Telem::HWESC_Telem(void) +{ +} + +void HWESC_Telem::init(AP_HAL::UARTDriver *_uart) +{ + uart = _uart; + uart->begin(19200); + uart->set_options(AP_HAL::UARTDriver::OPTION_PULLDOWN_RX); +} + +/* + update ESC telemetry + */ +bool HWESC_Telem::update() +{ + uint32_t n = uart->available(); + if (n == 0) { + return false; + } + + // we expect at least 50ms idle between frames + uint32_t now = AP_HAL::millis(); + bool frame_gap = (now - last_read_ms) > 10; + + last_read_ms = now; + + // don't read too much in one loop to prevent too high CPU load + if (n > 500) { + n = 500; + } + if (len == 0 && !frame_gap) { + // discard + while (n--) { + uart->read(); + } + return false; + } + + if (frame_gap) { + len = 0; + } + + bool ret = false; + + while (n--) { + uint8_t b = uart->read(); + //hal.console->printf("t=%u 0x%02x\n", now, b); + if (len == 0 && b != TELEM_HEADER) { + continue; + } + if (len == 1 && b != TELEM_LEN) { + continue; + } + uint8_t *buf = (uint8_t *)&pkt; + buf[len++] = b; + if (len == sizeof(pkt)) { + ret = parse_packet(); + len = 0; + } + } + return ret; +} + +static uint16_t calc_crc(const uint8_t *buf, uint8_t len) +{ + uint16_t crc = 0; + while (len--) { + crc += *buf++; + } + return crc; +} + +/* + parse packet + */ +bool HWESC_Telem::parse_packet(void) +{ + uint16_t crc = calc_crc((uint8_t *)&pkt, sizeof(pkt)-2); + if (crc != pkt.crc) { + return false; + } + + decoded.counter = be32toh(pkt.counter); + decoded.throttle_req = be16toh(pkt.throttle_req); + decoded.throttle = be16toh(pkt.throttle); + decoded.rpm = be16toh(pkt.rpm); + decoded.voltage = be16toh(pkt.voltage) * 0.1; + decoded.load = be16toh(pkt.load); + decoded.current = be16toh(pkt.current); + decoded.temperature = be16toh(pkt.temperature); + decoded.unknown = be16toh(pkt.unknown); + + return true; +} + +#endif // HAL_PERIPH_ENABLE_HWESC + diff --git a/Tools/AP_Periph/hwing_esc.h b/Tools/AP_Periph/hwing_esc.h new file mode 100644 index 0000000000..836ac31dc6 --- /dev/null +++ b/Tools/AP_Periph/hwing_esc.h @@ -0,0 +1,62 @@ +/* + ESC Telemetry for Hobbywing Pro 80A HV ESC. This will be + incorporated into a broader ESC telemetry library in ArduPilot + master in the future + */ + +#pragma once + +#include + +#ifdef HAL_PERIPH_ENABLE_HWESC + +class HWESC_Telem { +public: + HWESC_Telem(); + + void init(AP_HAL::UARTDriver *uart); + bool update(); + + struct HWESC { + uint32_t counter; + uint16_t throttle_req; + uint16_t throttle; + uint16_t rpm; + float voltage; + uint16_t load; + float current; + uint16_t temperature; + uint16_t unknown; + }; + + const HWESC &get_telem(void) { + return decoded; + } + +private: + AP_HAL::UARTDriver *uart; + + struct PACKED { + uint8_t header; // 0x9B + uint8_t pkt_len; // 0x16 + uint32_t counter; + uint16_t throttle_req; + uint16_t throttle; + uint16_t rpm; + uint16_t voltage; + uint16_t load; + uint16_t current; + uint16_t temperature; + uint16_t unknown; + uint16_t crc; + } pkt; + + uint8_t len; + uint32_t last_read_ms; + + struct HWESC decoded; + + bool parse_packet(void); +}; + +#endif // HAL_PERIPH_ENABLE_HWESC