From bdf7f3a765df582f8aa3300d53f3af6f62b65779 Mon Sep 17 00:00:00 2001 From: Lokesh-Ramina Date: Tue, 21 Nov 2023 20:24:37 -0800 Subject: [PATCH 1/3] AP_Periph: add support for APD ESC telemetry --- Tools/AP_Periph/AP_Periph.cpp | 9 +++ Tools/AP_Periph/AP_Periph.h | 9 ++- Tools/AP_Periph/Parameters.cpp | 53 ++++++++++++++++- Tools/AP_Periph/Parameters.h | 24 +++++++- 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, 284 insertions(+), 8 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 2953f664ae..446078ba0f 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)); diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 9388e67f5f..f3908cf35c 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; @@ -291,5 +298,3 @@ namespace AP } extern AP_Periph_FW periph; - - diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 981e86dd2d..83c6ac7df6 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 @@ -470,6 +478,47 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { #endif +#if defined(HAL_PERIPH_ENABLE_ESC_APD) + // @Param: ESC_NUM_POLES + // @DisplayName: Number of motor poles + // @Description: Sets the number of motor poles to calculate the correct RPM value + 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 + // @Param: ESC_NUMBER2 + // @DisplayName: ESC number 2 + // @Description: This is the ESC number to report as in UAVCAN ESC telemetry feedback packets. + // @Increment: 1 + // @User: Advanced + GARRAY(esc_number, 1, "ESC_NUMBER2", 1), + + // @Param: ESC_NUM_POLES2 + // @DisplayName: Number of motor poles + // @Description: Sets the number of motor poles to calculate the correct RPM value + 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 19f990f962..c0671e5873 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, @@ -61,6 +61,11 @@ class Parameters { k_param_efi_baudrate, k_param_disarm_delay, k_param_esc_command_timeout_ms, + 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; @@ -100,8 +105,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 diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 4afcc10bff..510f46eb70 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1660,8 +1660,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() { @@ -1721,6 +1755,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 From 0c1ab03099c16396186fe0da87fa787530b6b76c Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 2 Mar 2021 11:48:34 -0700 Subject: [PATCH 2/3] AP_Math: Add fletcher16 CRC --- libraries/AP_Math/crc.cpp | 14 +++++++++++++- libraries/AP_Math/crc.h | 4 +++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index 9027477166..8aab8d78e0 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -354,7 +354,7 @@ uint16_t crc16_ccitt_GDL90(const uint8_t *buf, uint32_t len, uint16_t crc) * @param [in] len size of buffer * @return CRC value */ -uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len) +uint16_t calc_crc_modbus(const uint8_t *buf, uint16_t len) { uint16_t crc = 0xFFFF; for (uint16_t pos = 0; pos < len; pos++) { @@ -372,6 +372,18 @@ uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len) return crc; } +// fletcher 16 implementation +uint16_t crc_fletcher16(const uint8_t *buffer, uint32_t len) { + uint16_t c0 = 0; + uint16_t c1 = 0; + for (uint32_t i = 0; i < len; i++) { + c0 = (c0 + buffer[i]) % 255; + c1 = (c1 + c0) % 255; + } + + return (c1 << 8) | c0; +} + // FNV-1a implementation #define FNV_1_PRIME_64 1099511628211UL void hash_fnv_1a(uint32_t len, const uint8_t* buf, uint64_t* hash) diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h index dc8daaaf6d..c5d06a93cf 100644 --- a/libraries/AP_Math/crc.h +++ b/libraries/AP_Math/crc.h @@ -42,7 +42,9 @@ uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc); // https://www.faa.gov/nextgen/programs/adsb/archival/media/gdl90_public_icd_reva.pdf uint16_t crc16_ccitt_GDL90(const uint8_t *buf, uint32_t len, uint16_t crc); -uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len); +uint16_t calc_crc_modbus(const uint8_t *buf, uint16_t len); + +uint16_t crc_fletcher16(const uint8_t * buffer, uint32_t len); // generate 64bit FNV1a hash from buffer #define FNV_1_OFFSET_BASIS_64 14695981039346656037UL From ed1cdb0776070ecdb69dfb60c88980b47135ce45 Mon Sep 17 00:00:00 2001 From: Jonathan Loong Date: Tue, 21 Nov 2023 15:57:50 -0800 Subject: [PATCH 3/3] AP_BattMonitor: patch for fuel level calibration function from ottanoX --- .../AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp index 1b0f00bb86..7f176b17f6 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp @@ -21,6 +21,10 @@ extern const AP_HAL::HAL& hal; +// ADC Calibration for CX31021020 Rev D Voltage Divider for ADC #3 +#define CPN_ADC_5V_SLP 797.76 +#define CPN_ADC_5V_OFF 1.11 + const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = { // @Param: FL_VLT_MIN @@ -128,7 +132,8 @@ void AP_BattMonitor_FuelLevel_Analog::read() const uint32_t dt_us = tnow - _state.last_time_micros; // get voltage from an ADC pin - const float raw_voltage = _analog_source->voltage_average(); + //const float raw_voltage = _analog_source->voltage_average(); + const float raw_voltage = (_analog_source->read_average() - CPN_ADC_5V_OFF) / CPN_ADC_5V_SLP; // Converting sensor reading to actual volume in tank in Litres (quadratic fit) const float voltage =