-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
dc7bf3b
commit 5eb4329
Showing
8 changed files
with
287 additions
and
11 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <AP_HAL/utility/sparse-endian.h> | ||
| #include <AP_Math/crc.h> | ||
| #include <AP_Math/definitions.h> | ||
| #include <string.h> | ||
|
|
||
| #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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,61 @@ | ||
| /* | ||
| ESC Telemetry for APD ESC. | ||
| */ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <AP_HAL/AP_HAL.h> | ||
|
|
||
| #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 |
Oops, something went wrong.