From e2d0cc55d038cec1eca7407ff94d491c5bb48236 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 2 Mar 2021 11:49:09 -0700 Subject: [PATCH] AP_Periph: Support APD ESC telemetry --- Tools/AP_Periph/AP_Periph.cpp | 11 +++- Tools/AP_Periph/AP_Periph.h | 9 ++- Tools/AP_Periph/Parameters.cpp | 42 ++++++++++++- Tools/AP_Periph/Parameters.h | 26 +++++++-- Tools/AP_Periph/can.cpp | 39 ++++++++++++- Tools/AP_Periph/esc_apd_telem.cpp | 97 +++++++++++++++++++++++++++++++ Tools/AP_Periph/esc_apd_telem.h | 61 +++++++++++++++++++ 7 files changed, 275 insertions(+), 10 deletions(-) create mode 100644 Tools/AP_Periph/esc_apd_telem.cpp create mode 100644 Tools/AP_Periph/esc_apd_telem.h diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index f416388833..965a59edaa 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -242,6 +242,15 @@ void AP_Periph_FW::init() hwesc_telem.init(hal.serial(HAL_PERIPH_HWESC_SERIAL_PORT)); #endif +#ifdef HAL_PERIPH_ENABLE_ESC_APD + for (uint8_t i = 0; i < ESC_NUMBERS; i++) { + const uint8_t port = g.esc_serial_port[i]; + if (port < SERIALMANAGER_NUM_PORTS) { // skip bad ports + apd_esc_telem[i] = new ESC_APD_Telem (hal.serial(port), g.pole_count[i]); + } + } +#endif + #ifdef HAL_PERIPH_ENABLE_MSP if (g.msp_port >= 0) { msp_init(hal.serial(g.msp_port)); @@ -555,4 +564,4 @@ AP_Periph_FW& AP::periph() return *AP_Periph_FW::get_singleton(); } -AP_HAL_MAIN(); +AP_HAL_MAIN(); \ No newline at end of file diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 82451d3353..76b78f4797 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -41,6 +41,8 @@ #include "GCS_MAVLink.h" #endif +#include "esc_apd_telem.h" + #if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY) #define AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY #endif @@ -237,6 +239,11 @@ public: #if AP_KDECAN_ENABLED AP_KDECAN kdecan; #endif + +#ifdef HAL_PERIPH_ENABLE_ESC_APD + ESC_APD_Telem *apd_esc_telem[APD_ESC_INSTANCES]; + void apd_esc_telem_update(); +#endif #ifdef HAL_PERIPH_ENABLE_RC_OUT #if HAL_WITH_ESC_TELEM @@ -329,5 +336,3 @@ namespace AP } extern AP_Periph_FW periph; - - diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 67c3febb85..57156a3645 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -57,6 +57,14 @@ extern const AP_HAL::HAL &hal; #define MAV_SYSTEM_ID HAL_DEFAULT_MAV_SYSTEM_ID #endif +#ifndef APD_ESC_SERIAL_0 + #define APD_ESC_SERIAL_0 -1 +#endif + +#ifndef APD_ESC_SERIAL_1 + #define APD_ESC_SERIAL_1 -1 +#endif + /* * AP_Periph parameter definitions * @@ -327,13 +335,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(hardpoint_rate, "HARDPOINT_RATE", 100), #endif -#ifdef HAL_PERIPH_ENABLE_HWESC +#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD) // @Param: ESC_NUMBER // @DisplayName: ESC number // @Description: This is the ESC number to report as in UAVCAN ESC telemetry feedback packets. // @Increment: 1 // @User: Advanced - GSCALAR(esc_number, "ESC_NUMBER", 0), + GARRAY(esc_number, 0, "ESC_NUMBER", 0), #endif #ifdef HAL_PERIPH_ENABLE_RC_OUT @@ -513,6 +521,36 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(kdecan, "KDE_", AP_KDECAN), #endif +#if defined(HAL_PERIPH_ENABLE_ESC_APD) + GARRAY(pole_count, 0, "ESC_NUM_POLES", 22), +#endif + +#if defined(HAL_PERIPH_ENABLE_ESC_APD) + // @Param: ESC_APD_SERIAL_1 + // @DisplayName: ESC APD Serial 1 + // @Description: Which serial port to use for APD ESC data + // @Range: 0 6 + // @Increment: 1 + // @User: Advanced + // @RebootRequired: True + GARRAY(esc_serial_port, 0, "ESC_APD_SERIAL_1", APD_ESC_SERIAL_0), + + #if APD_ESC_INSTANCES > 1 + GARRAY(esc_number, 1, "ESC_NUMBER2", 1), + + GARRAY(pole_count, 1, "ESC_NUM_POLES2", 22), + + // @Param: ESC_APD_SERIAL_2 + // @DisplayName: ESC APD Serial 2 + // @Description: Which serial port to use for APD ESC data + // @Range: 0 6 + // @Increment: 1 + // @User: Advanced + // @RebootRequired: True + GARRAY(esc_serial_port, 1, "ESC_APD_SERIAL_2", APD_ESC_SERIAL_1), + #endif +#endif + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 9fbdaec8ab..c2ad8e678c 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -30,7 +30,7 @@ public: k_param_hardpoint_id, k_param_hardpoint_rate, k_param_baro_enable, - k_param_esc_number, + k_param_esc_number0, k_param_battery, k_param_debug, k_param_serial_number, @@ -71,6 +71,11 @@ public: k_param_proximity_max_rate, k_param_nmea, k_param_kdecan, + k_param_pole_count0, + k_param_esc_serial_port0, + k_param_esc_number1, + k_param_pole_count1, + k_param_esc_serial_port1, }; AP_Int16 format_version; @@ -121,8 +126,21 @@ public: AP_Int8 hardpoint_rate; #endif -#ifdef HAL_PERIPH_ENABLE_HWESC - AP_Int8 esc_number; +#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APD) + #if defined ESC_NUMBERS + #error "ESC_NUMBERS should not have been previously defined" + #endif + #if defined(APD_ESC_INSTANCES) + #define ESC_NUMBERS APD_ESC_INSTANCES + #else + #define ESC_NUMBERS 2 + #endif // defined(APD_ESC_INSTANCES) + AP_Int8 esc_number[ESC_NUMBERS]; + AP_Int8 esc_serial_port[ESC_NUMBERS]; +#endif + +#if defined(ESC_NUMBERS) + AP_Int8 pole_count[ESC_NUMBERS]; #endif #ifdef HAL_PERIPH_ENABLE_GPS @@ -173,4 +191,4 @@ public: Parameters() {} }; -extern const AP_Param::Info var_info[]; +extern const AP_Param::Info var_info[]; \ No newline at end of file diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 4db517f6e2..6ae3c8e2cf 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1671,8 +1671,42 @@ void AP_Periph_FW::esc_telem_update() } } #endif // HAL_WITH_ESC_TELEM -#endif // HAL_PERIPH_ENABLE_RC_OUT +#ifdef HAL_PERIPH_ENABLE_ESC_APD +void AP_Periph_FW::apd_esc_telem_update() +{ + for(uint8_t i = 0; i < ARRAY_SIZE(apd_esc_telem); i++) { + if (apd_esc_telem[i] == nullptr) { + continue; + } + ESC_APD_Telem &esc = *apd_esc_telem[i]; + + if (esc.update()) { + const ESC_APD_Telem::telem &t = esc.get_telem(); + + uavcan_equipment_esc_Status pkt {}; + static_assert(APD_ESC_INSTANCES <= ARRAY_SIZE(g.esc_number), "There must be an ESC instance number for each APD ESC"); + pkt.esc_index = g.esc_number[i]; + pkt.voltage = t.voltage; + pkt.current = t.current; + pkt.temperature = t.temperature; + pkt.rpm = t.rpm; + pkt.power_rating_pct = t.power_rating_pct; + pkt.error_count = t.error_count; + + uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout()); + canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + } + +} +#endif // HAL_PERIPH_ENABLE_ESC_APD +#endif // HAL_PERIPH_ENABLE_RC_OUT void AP_Periph_FW::can_update() { @@ -1733,6 +1767,9 @@ void AP_Periph_FW::can_update() #ifdef HAL_PERIPH_ENABLE_HWESC hwesc_telem_update(); #endif +#ifdef HAL_PERIPH_ENABLE_ESC_APD + apd_esc_telem_update(); +#endif #ifdef HAL_PERIPH_ENABLE_MSP msp_sensor_update(); #endif diff --git a/Tools/AP_Periph/esc_apd_telem.cpp b/Tools/AP_Periph/esc_apd_telem.cpp new file mode 100644 index 0000000000..fccc266331 --- /dev/null +++ b/Tools/AP_Periph/esc_apd_telem.cpp @@ -0,0 +1,97 @@ +/* + ESC Telemetry for the APD HV Pro ESC + + Protocol is here: https://docs.powerdrives.net/products/hv_pro/uart-telemetry-output + */ +#include "esc_apd_telem.h" +#include +#include +#include +#include + +#ifdef HAL_PERIPH_ENABLE_ESC_APD + +extern const AP_HAL::HAL& hal; + +#define TELEM_HEADER 0x9B +#define TELEM_LEN 0x16 + +ESC_APD_Telem::ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles) : + pole_count(num_poles), + uart(_uart) { + uart->begin(115200); +} + +bool ESC_APD_Telem::update() { + uint32_t n = uart->available(); + if (n == 0) { + return false; + } + + // don't read too much in one loop to prevent too high CPU load + if (n > 50) { + n = 50; + } + + bool ret = false; + + while (n--) { + uint8_t b = uart->read(); + received.bytes[len++] = b; + + // check the packet size first + if ((size_t)len >= sizeof(received.packet)) { + // we have a full packet, check the stop byte + if (received.packet.stop == 65535) { + // valid stop byte, check the CRC + if (crc_fletcher16(received.bytes, 18) == received.packet.checksum) { + // valid packet, copy the data we need and reset length + decoded.voltage = le16toh(received.packet.voltage) * 1e-2f; + decoded.temperature = convert_temperature(le16toh(received.packet.temperature)); + decoded.current = le16toh(received.packet.bus_current) * (1 / 12.5f); + decoded.rpm = le32toh(received.packet.erpm) / pole_count; + decoded.power_rating_pct = le16toh(received.packet.motor_duty) * 1e-2f; + ret = true; + len = 0; + } else { + // we have an invalid packet, shift it back a byte + shift_buffer(); + } + } else { + // invalid stop byte, we've lost sync, shift the packet by 1 byte + shift_buffer(); + } + + } + } + return ret; +} + +// shift the decode buffer left by 1 byte, and rewind the progress +void ESC_APD_Telem::shift_buffer(void) { + memmove(received.bytes, received.bytes + 1, sizeof(received.bytes) - 1); + len--; +} + +// convert the raw ESC temperature to a useful value (in Kelvin) +// based on the 1.1 example C code found here https://docs.powerdrives.net/products/hv_pro/uart-telemetry-output +float ESC_APD_Telem::convert_temperature(uint16_t raw) const { + const float series_resistor = 10000; + const float nominal_resistance = 10000; + const float nominal_temperature = 25; + const float b_coefficent = 3455; + + + const float Rntc = series_resistor / ((4096 / float(raw)) - 1); + + float temperature = Rntc / nominal_resistance; // (R/Ro) + temperature = logf(temperature); // ln(R/Ro) + temperature /= b_coefficent; // 1/B * ln(R/Ro) + temperature += 1 / C_TO_KELVIN(nominal_temperature); // + (1/To) + temperature = 1 / temperature; // invert + + // the example code rejected anything below 0C, or above 200C, the 200C clamp makes some sense, the below 0C is harder to accept + return temperature; +} + +#endif // HAL_PERIPH_ENABLE_ESC_APD diff --git a/Tools/AP_Periph/esc_apd_telem.h b/Tools/AP_Periph/esc_apd_telem.h new file mode 100644 index 0000000000..42f2f416ad --- /dev/null +++ b/Tools/AP_Periph/esc_apd_telem.h @@ -0,0 +1,61 @@ +/* + ESC Telemetry for APD ESC. + */ + +#pragma once + +#include + +#ifdef HAL_PERIPH_ENABLE_ESC_APD + +class ESC_APD_Telem { +public: + ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles); + bool update(); + + CLASS_NO_COPY(ESC_APD_Telem); + + struct telem { + uint32_t error_count; + float voltage; + float current; + float temperature; // kelvin + int32_t rpm; + uint8_t power_rating_pct; + }; + + const telem &get_telem(void) { + return decoded; + } + +private: + AP_HAL::UARTDriver *uart; + + union { + struct PACKED { + uint16_t voltage; + uint16_t temperature; + int16_t bus_current; + uint16_t reserved0; + uint32_t erpm; + uint16_t input_duty; + uint16_t motor_duty; + uint16_t reserved1; + uint16_t checksum; // 16 bit fletcher checksum + uint16_t stop; // should always be 65535 on a valid packet + } packet; + uint8_t bytes[22]; + } received; + static_assert(sizeof(received.packet) == sizeof(received.bytes), "The packet must be the same size as the raw buffer"); + + uint8_t len; + + struct telem decoded; + + float pole_count; + + float convert_temperature(uint16_t raw) const; + void shift_buffer(void); +}; + +#endif // HAL_PERIPH_ENABLE_ESC_APD