Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/ov3 1018 cxpilot pr merg apd fuel level #78

Merged
merged 3 commits into from
Nov 22, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions Tools/AP_Periph/AP_Periph.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
9 changes: 7 additions & 2 deletions Tools/AP_Periph/AP_Periph.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -291,5 +298,3 @@ namespace AP
}

extern AP_Periph_FW periph;


53 changes: 51 additions & 2 deletions Tools/AP_Periph/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
*
Expand Down Expand Up @@ -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
loki077 marked this conversation as resolved.
Show resolved Hide resolved
// @User: Advanced
GSCALAR(esc_number, "ESC_NUMBER", 0),
GARRAY(esc_number, 0, "ESC_NUMBER", 0),
#endif

#ifdef HAL_PERIPH_ENABLE_RC_OUT
Expand Down Expand Up @@ -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),
loki077 marked this conversation as resolved.
Show resolved Hide resolved

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

Expand Down
24 changes: 21 additions & 3 deletions Tools/AP_Periph/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down
39 changes: 38 additions & 1 deletion Tools/AP_Periph/can.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand Down Expand Up @@ -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
Expand Down
97 changes: 97 additions & 0 deletions Tools/AP_Periph/esc_apd_telem.cpp
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
61 changes: 61 additions & 0 deletions Tools/AP_Periph/esc_apd_telem.h
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
Loading
Loading