mirror of https://github.com/ArduPilot/ardupilot
98 lines
3.3 KiB
C++
98 lines
3.3 KiB
C++
/*
|
|
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 = ((int16_t)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
|