diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 4d228cdab3..1e0eba82ac 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -222,6 +222,15 @@ void AP_Periph_FW::init() hwesc_telem.init(hal.serial(3)); #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)); @@ -519,4 +528,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 85f356659d..9a942dcd93 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -26,6 +26,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 @@ -206,6 +208,11 @@ class AP_Periph_FW { uint32_t efi_update_ms; #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 AP_ESC_Telem esc_telem; @@ -288,5 +295,3 @@ namespace AP } extern AP_Periph_FW periph; - - diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 394a77a6fe..a2feb0a06c 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -53,6 +53,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 * @@ -336,13 +344,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 @@ -451,6 +459,36 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(efi, "EFI", AP_EFI), #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 092b4106ad..1bbc6bb87d 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -28,7 +28,7 @@ class Parameters { 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, @@ -59,6 +59,21 @@ class Parameters { k_param_efi, k_param_efi_port, k_param_efi_baudrate, + k_param_esc_telem_rate, + k_param_esc_command_timeout_ms, + k_param_esc_serial_port0, + k_param_esc_number1, + k_param_pole_count1, + k_param_esc_serial_port1, + k_param_can_slcan_cport, + k_param_temperature_sensor, + k_param_proximity, + k_param_proximity_baud, + k_param_proximity_port, + k_param_proximity_max_rate, + k_param_nmea, + k_param_kdecan, + k_param_pole_count0, }; AP_Int16 format_version; @@ -98,8 +113,21 @@ class Parameters { 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 @@ -146,4 +174,4 @@ class Parameters { 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 d7759bda54..65aa621f9a 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1656,8 +1656,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() { @@ -1717,6 +1751,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 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat index ce9333e202..cd8ed70671 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat @@ -142,6 +142,7 @@ COMPASS MMC5XX3 I2C:0:0x30 false ROTATION_NONE define AP_COMPASS_QMC5883P_ENABLED 1 COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 - +define HAL_PERIPH_ENABLE_ESC_APD 1 +define APD_ESC_INSTANCES 4