Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: Support APD ESC telemetry

This commit is contained in:
Michael du Breuil 2021-03-02 11:49:09 -07:00 committed by WickedShell
parent 32fd21592a
commit e2d0cc55d0
7 changed files with 275 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Tools/AP_Periph/AP_Periph.cpp
View File

@ -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();

9
Tools/AP_Periph/AP_Periph.h
View File

@ -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;

42
Tools/AP_Periph/Parameters.cpp
View File

@ -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
};

26
Tools/AP_Periph/Parameters.h
View File

@ -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[];

39
Tools/AP_Periph/can.cpp
View File

@ -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

97
Tools/AP_Periph/esc_apd_telem.cpp Normal file
View File

@ -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

61
Tools/AP_Periph/esc_apd_telem.h Normal file
View File

@ -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