Skip to content

Added Hobbywing ESC telemetry (DO NOT MERGE) #106

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.

Sign up for GitHub

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

Open
wants to merge 10 commits into
base: mttr-rebase-3.6.7
Choose a base branch
from
Open

Added Hobbywing ESC telemetry (DO NOT MERGE) #106

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
73cd057
AP_ESC_Telem: added support for Hobbywing 80A HV Pro ESC telem
tridge Feb 13, 2020
fd724f9
HAL_ChibiOS: HACK to pulldown TELEM2 RX for ESC telem
tridge Feb 13, 2020
7a90719
GCS_MAVLink: send ESC telem on mavlink
tridge Feb 13, 2020
c329915
Copter: added ESC telemetry
tridge Feb 13, 2020
fa7d93b
AP_ESC_Telem: fixed ESC current
tridge Feb 13, 2020
5e4e638
AP_ESC_Telem: fixed current reporting
tridge Feb 14, 2020
1cc58fe
Removed un-needed pre-arm checks for this configuration
Aksellence Feb 20, 2020
46f376a
AP_ESC_Telem: fixed scaling of RPM
tridge Mar 9, 2020
83883ea
AP_ESC_Telem: update based on new protocol docs
tridge Apr 1, 2020
55429c6
DataFlash: DISABLE persist logging for 60s after disarm or arming fai…
Aksellence Apr 17, 2020
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
Prev Previous commit
Next Next commit
AP_ESC_Telem: update based on new protocol docs
  • Loading branch information
@tridge
tridge committed Apr 1, 2020
commit 83883ea150653b486855cfb526fbc39f8e36c0cf
69 changes: 51 additions & 18 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,35 @@ static uint16_t calc_crc(const uint8_t *buf, uint8_t len)
return crc;
}

static const struct {
uint8_t adc_temp;
uint8_t temp_C;
} temp_table[] = {
{ 241, 0}, { 240, 1}, { 239, 2}, { 238, 3}, { 237, 4}, { 236, 5}, { 235, 6}, { 234, 7}, { 233, 8}, { 232, 9},
{ 231, 10}, { 230, 11}, { 229, 12}, { 228, 13}, { 227, 14}, { 226, 15}, { 224, 16}, { 223, 17}, { 222, 18}, { 220, 19},
{ 219, 20}, { 217, 21}, { 216, 22}, { 214, 23}, { 213, 24}, { 211, 25}, { 209, 26}, { 208, 27}, { 206, 28}, { 204, 29},
{ 202, 30}, { 201, 31}, { 199, 32}, { 197, 33}, { 195, 34}, { 193, 35}, { 191, 36}, { 189, 37}, { 187, 38}, { 185, 39},
{ 183, 40}, { 181, 41}, { 179, 42}, { 177, 43}, { 174, 44}, { 172, 45}, { 170, 46}, { 168, 47}, { 166, 48}, { 164, 49},
{ 161, 50}, { 159, 51}, { 157, 52}, { 154, 53}, { 152, 54}, { 150, 55}, { 148, 56}, { 146, 57}, { 143, 58}, { 141, 59},
{ 139, 60}, { 136, 61}, { 134, 62}, { 132, 63}, { 130, 64}, { 128, 65}, { 125, 66}, { 123, 67}, { 121, 68}, { 119, 69},
{ 117, 70}, { 115, 71}, { 113, 72}, { 111, 73}, { 109, 74}, { 106, 75}, { 105, 76}, { 103, 77}, { 101, 78}, { 99, 79},
{ 97, 80}, { 95, 81}, { 93, 82}, { 91, 83}, { 90, 84}, { 88, 85}, { 85, 86}, { 84, 87}, { 82, 88}, { 81, 89},
{ 79, 90}, { 77, 91}, { 76, 92}, { 74, 93}, { 73, 94}, { 72, 95}, { 69, 96}, { 68, 97}, { 66, 98}, { 65, 99},
{ 64, 100}, { 62, 101}, { 62, 102}, { 61, 103}, { 59, 104}, { 58, 105}, { 56, 106}, { 54, 107}, { 54, 108}, { 53, 109},
{ 51, 110}, { 51, 111}, { 50, 112}, { 48, 113}, { 48, 114}, { 46, 115}, { 46, 116}, { 44, 117}, { 43, 118}, { 43, 119},
{ 41, 120}, { 41, 121}, { 39, 122}, { 39, 123}, { 39, 124}, { 37, 125}, { 37, 126}, { 35, 127}, { 35, 128}, { 33, 129},
};

uint8_t AP_ESC_Telem::temperature_decode(uint8_t temp_raw) const
{
for (uint8_t i=0; i<ARRAY_SIZE(temp_table); i++) {
if (temp_table[i].adc_temp <= temp_raw) {
return temp_table[i].temp_C;
}
}
return 130U;
}

/*
parse packet
*/
Expand All @@ -111,38 +140,41 @@ void AP_ESC_Telem::parse_packet(void)

sem->take_blocking();
decoded.counter = be32toh(pkt.counter);
decoded.throttle_req = be16toh(pkt.throttle_req);
decoded.throttle = be16toh(pkt.throttle);
decoded.rpm = be16toh(pkt.erpm) * 5.0 / 7.0;
decoded.throttle_req = be16toh(pkt.throttle_req) * 100.0 / 1024.0;
decoded.throttle = be16toh(pkt.throttle) * 100.0 / 1024.0;
decoded.rpm = be16toh(pkt.erpm) * 10.0 / 14.0;
decoded.voltage = be16toh(pkt.voltage) * 0.1;
decoded.current = int16_t(be16toh(pkt.current)) * 0.01;
decoded.phase_current = int16_t(be16toh(pkt.phase_current)) * 0.01;
decoded.temperature = be16toh(pkt.temperature);
decoded.current = int16_t(be16toh(pkt.current)) / 64.0;
decoded.phase_current = int16_t(be16toh(pkt.phase_current)) / 64.0;
decoded.mos_temperature = temperature_decode(pkt.mos_temperature);
decoded.cap_temperature = temperature_decode(pkt.cap_temperature);
decoded.status = be16toh(pkt.status);
sem->give();

#if 0
hal.console->printf("%u RPM:%.0f THR:%u:%u V:%.2f PC:%.2f C:%.2f T:0x%x S:0x%x\n",
uint32_t now = AP_HAL::millis();
static uint32_t last_ms;
uint32_t dt = now - last_ms;
last_ms = now;
hal.console->printf("dt=%u %u RPM:%.1f THR:%.1f:%.1f V:%.2f C:%.1f CP:%.1f\n", dt,
unsigned(decoded.counter),
decoded.rpm,
unsigned(decoded.throttle_req), unsigned(decoded.throttle),
decoded.voltage,
decoded.phase_current,
decoded.current,
unsigned(decoded.temperature),
unsigned(decoded.status));
decoded.throttle_req, decoded.throttle,
decoded.voltage, decoded.current, decoded.phase_current);
#endif

DataFlash_Class::instance()->Log_Write("HESC", "TimeUS,CNT,RPM,ThrR,Thr,Volt,CurrP,Curr,Temp,Status",
"QIfHHfffHH",
DataFlash_Class::instance()->Log_Write("HESC", "TimeUS,CNT,RPM,ThrR,Thr,Volt,Curr,CurrP,TempM,TempC,Status",
"QIffffffBBH",
AP_HAL::micros64(),
decoded.counter,
decoded.rpm,
decoded.throttle_req, decoded.throttle,
decoded.throttle_req,
decoded.throttle,
decoded.voltage,
decoded.phase_current,
decoded.current,
decoded.temperature,
decoded.phase_current,
decoded.mos_temperature,
decoded.cap_temperature,
decoded.status);
}

Expand All @@ -167,6 +199,7 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t chan)
voltage[0] = decoded.voltage * 1000;
current[0] = decoded.current;
rpm[0] = decoded.rpm;
temperature[0] = MAX(decoded.mos_temperature, decoded.cap_temperature);
sem->give();
mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)chan, temperature, voltage, current, totalcurrent, rpm, count);
}
11 changes: 7 additions & 4 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,8 @@ class AP_ESC_Telem {
uint16_t voltage;
int16_t current;
int16_t phase_current;
uint16_t temperature;
uint8_t mos_temperature;
uint8_t cap_temperature;
uint16_t status;
uint16_t crc;
} pkt;
Expand All @@ -53,13 +54,14 @@ class AP_ESC_Telem {

struct {
uint32_t counter;
uint16_t throttle_req;
uint16_t throttle;
float throttle_req;
float throttle;
float rpm;
float voltage;
float current;
float phase_current;
uint16_t temperature;
uint8_t mos_temperature;
uint8_t cap_temperature;
uint16_t status;
} decoded;

Expand All @@ -68,4 +70,5 @@ class AP_ESC_Telem {
AP_HAL::Semaphore *sem;

void parse_packet(void);
uint8_t temperature_decode(uint8_t temp_raw) const;
};