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

AP_Periph: fixed HWESC temperature decoding 

now have protocol docs
This commit is contained in:
Andrew Tridgell 2020-04-01 10:49:02 +11:00
parent 4bab3c083e
commit e2d6e70895
3 changed files with 43 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Tools/AP_Periph/can.cpp
View File

@ -1117,9 +1117,10 @@ void AP_Periph_FW::hwesc_telem_update()
pkt.esc_index = g.esc_number; pkt.esc_index = g.esc_number;
pkt.voltage = t.voltage; pkt.voltage = t.voltage;
pkt.current = t.current; pkt.current = t.current;
pkt.temperature = t.temperature; pkt.temperature = MAX(t.mos_temperature, t.cap_temperature);
pkt.rpm = t.rpm; pkt.rpm = t.rpm;
pkt.power_rating_pct = t.phase_current; pkt.power_rating_pct = t.phase_current;
pkt.error_count = t.error_count;
fix_float16(pkt.voltage); fix_float16(pkt.voltage);
fix_float16(pkt.current); fix_float16(pkt.current);

35
Tools/AP_Periph/hwing_esc.cpp
View File

@ -90,6 +90,35 @@ static uint16_t calc_crc(const uint8_t *buf, uint8_t len)
return crc; 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 parse packet
*/ */
@ -107,8 +136,12 @@ bool HWESC_Telem::parse_packet(void)
decoded.voltage = be16toh(pkt.voltage) * 0.1; decoded.voltage = be16toh(pkt.voltage) * 0.1;
decoded.phase_current = int16_t(be16toh(pkt.phase_current)) * 0.01; decoded.phase_current = int16_t(be16toh(pkt.phase_current)) * 0.01;
decoded.current = int16_t(be16toh(pkt.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); decoded.status = be16toh(pkt.status);
if (decoded.status != 0) {
decoded.error_count++;
}
return true; return true;
} }

9
Tools/AP_Periph/hwing_esc.h
View File

@ -25,8 +25,10 @@ public:
float voltage; float voltage;
float phase_current; float phase_current;
float current; float current;
uint16_t temperature; uint8_t mos_temperature;
uint8_t cap_temperature;
uint16_t status; uint16_t status;
uint32_t error_count;
}; };
const HWESC &get_telem(void) { const HWESC &get_telem(void) {
@ -46,17 +48,20 @@ private:
uint16_t voltage; uint16_t voltage;
int16_t current; int16_t current;
int16_t phase_current; int16_t phase_current;
uint16_t temperature; uint8_t mos_temperature;
uint8_t cap_temperature;
uint16_t status; uint16_t status;
uint16_t crc; uint16_t crc;
} pkt; } pkt;
uint8_t len; uint8_t len;
uint32_t last_read_ms; uint32_t last_read_ms;
uint32_t error_count;
struct HWESC decoded; struct HWESC decoded;
bool parse_packet(void); bool parse_packet(void);
uint8_t temperature_decode(uint8_t temp_raw) const;
}; };
#endif // HAL_PERIPH_ENABLE_HWESC #endif // HAL_PERIPH_ENABLE_HWESC