From e2d6e70895b0420f807bdae548e9f127bf18c3a5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 1 Apr 2020 10:49:02 +1100
Subject: [PATCH] AP_Periph: fixed HWESC temperature decoding

now have protocol docs
---
 Tools/AP_Periph/can.cpp       |  3 ++-
 Tools/AP_Periph/hwing_esc.cpp | 35 ++++++++++++++++++++++++++++++++++-
 Tools/AP_Periph/hwing_esc.h   |  9 +++++++--
 3 files changed, 43 insertions(+), 4 deletions(-)

diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index cdf8ba033d..71a3a84122 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1117,9 +1117,10 @@ void AP_Periph_FW::hwesc_telem_update()
     pkt.esc_index = g.esc_number;
     pkt.voltage = t.voltage;
     pkt.current = t.current;
-    pkt.temperature = t.temperature;
+    pkt.temperature = MAX(t.mos_temperature, t.cap_temperature);
     pkt.rpm = t.rpm;
     pkt.power_rating_pct = t.phase_current;
+    pkt.error_count = t.error_count;
 
     fix_float16(pkt.voltage);
     fix_float16(pkt.current);
diff --git a/Tools/AP_Periph/hwing_esc.cpp b/Tools/AP_Periph/hwing_esc.cpp
index 861c4a6f4c..d98424b735 100644
--- a/Tools/AP_Periph/hwing_esc.cpp
+++ b/Tools/AP_Periph/hwing_esc.cpp
@@ -90,6 +90,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 HWESC_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
  */
@@ -107,8 +136,12 @@ bool HWESC_Telem::parse_packet(void)
     decoded.voltage = be16toh(pkt.voltage) * 0.1;
     decoded.phase_current = int16_t(be16toh(pkt.phase_current)) * 0.01;
     decoded.current = int16_t(be16toh(pkt.current)) * 0.01;
-    decoded.temperature = be16toh(pkt.temperature);
+    decoded.mos_temperature = temperature_decode(pkt.mos_temperature);
+    decoded.cap_temperature = temperature_decode(pkt.cap_temperature);
     decoded.status = be16toh(pkt.status);
+    if (decoded.status != 0) {
+        decoded.error_count++;
+    }
 
     return true;
 }
diff --git a/Tools/AP_Periph/hwing_esc.h b/Tools/AP_Periph/hwing_esc.h
index 5db0d8974a..bfb53645f6 100644
--- a/Tools/AP_Periph/hwing_esc.h
+++ b/Tools/AP_Periph/hwing_esc.h
@@ -25,8 +25,10 @@ public:
         float voltage;
         float phase_current;
         float current;
-        uint16_t temperature;
+        uint8_t mos_temperature;
+        uint8_t cap_temperature;
         uint16_t status;
+        uint32_t error_count;
     };
 
     const HWESC &get_telem(void) {
@@ -46,17 +48,20 @@ private:
         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;
 
     uint8_t len;
     uint32_t last_read_ms;
+    uint32_t error_count;
 
     struct HWESC decoded;
 
     bool parse_packet(void);
+    uint8_t temperature_decode(uint8_t temp_raw) const;
 };
 
 #endif // HAL_PERIPH_ENABLE_HWESC