CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/AP_Periph/hwing_esc.cpp
Views: 1798
1
/*

2
  ESC Telemetry for Hobbywing Pro 80A HV ESC. This will be

3
  incorporated into a broader ESC telemetry library in ArduPilot

4
  master in the future

5


6
  This protocol only allows for one ESC per UART RX line, so using a

7
  CAN node per ESC works well.

8
 */

9


10
#include "AP_Periph.h"

11
#include "hwing_esc.h"

12
#include <AP_HAL/utility/sparse-endian.h>

13
#include <dronecan_msgs.h>

14


15
#ifdef HAL_PERIPH_ENABLE_HWESC

16


17
#include <SITL/SITL.h>

18


19
extern const AP_HAL::HAL& hal;

20


21
#define TELEM_HEADER 0x9B

22
#define TELEM_LEN    0x16

23


24
// constructor

25
HWESC_Telem::HWESC_Telem(void)

26
{

27
}

28


29
void HWESC_Telem::init(AP_HAL::UARTDriver *_uart)

30
{

31
    uart = _uart;

32
    uart->begin(19200);

33
    uart->set_options(AP_HAL::UARTDriver::OPTION_PULLDOWN_RX);

34
}

35


36
/*

37
  update ESC telemetry

38
 */

39
bool HWESC_Telem::update()

40
{

41
    uint32_t n = uart->available();

42
    if (n == 0) {

43
        return false;

44
    }

45


46
    // we expect at least 50ms idle between frames

47
    uint32_t now = AP_HAL::millis();

48
    bool frame_gap = (now - last_read_ms) > 10;

49


50
    last_read_ms = now;

51


52
    // don't read too much in one loop to prevent too high CPU load

53
    if (n > 500) {

54
        n = 500;

55
    }

56
    if (len == 0 && !frame_gap) {

57
        uart->discard_input();

58
        return false;

59
    }

60


61
    if (frame_gap) {

62
        len = 0;

63
    }

64


65
    bool ret = false;

66


67
    while (n--) {

68
        uint8_t b = uart->read();

69
        //hal.console->printf("t=%u 0x%02x\n", now, b);

70
        if (len == 0 && b != TELEM_HEADER) {

71
            continue;

72
        }

73
        if (len == 1 && b != TELEM_LEN) {

74
            continue;

75
        }

76
        uint8_t *buf = (uint8_t *)&pkt;

77
        buf[len++] = b;

78
        if (len == sizeof(pkt)) {

79
            ret = parse_packet();

80
            len = 0;

81
        }

82
    }

83
    return ret;

84
}

85


86
static uint16_t calc_crc(const uint8_t *buf, uint8_t len)

87
{

88
    uint16_t crc = 0;

89
    while (len--) {

90
        crc += *buf++;

91
    }

92
    return crc;

93
}

94


95
static const struct {

96
    uint8_t adc_temp;

97
    uint8_t temp_C;

98
} temp_table[] = {

99
    { 241, 	0}, 	{ 240, 	1}, 	{ 239, 	2}, 	{ 238, 	3}, 	{ 237, 	4}, 	{ 236, 	5}, 	{ 235, 	6}, 	{ 234, 	7}, 	{ 233, 	8}, 	{ 232, 	9},

100
    { 231, 	10}, 	{ 230, 	11}, 	{ 229, 	12}, 	{ 228, 	13}, 	{ 227, 	14}, 	{ 226, 	15}, 	{ 224, 	16}, 	{ 223, 	17}, 	{ 222, 	18}, 	{ 220, 	19},

101
    { 219, 	20}, 	{ 217, 	21}, 	{ 216, 	22}, 	{ 214, 	23}, 	{ 213, 	24}, 	{ 211, 	25}, 	{ 209, 	26}, 	{ 208, 	27}, 	{ 206, 	28}, 	{ 204, 	29},

102
    { 202, 	30}, 	{ 201, 	31}, 	{ 199, 	32}, 	{ 197, 	33}, 	{ 195, 	34}, 	{ 193, 	35}, 	{ 191, 	36}, 	{ 189, 	37}, 	{ 187, 	38}, 	{ 185, 	39},

103
    { 183, 	40}, 	{ 181, 	41}, 	{ 179, 	42}, 	{ 177, 	43}, 	{ 174, 	44}, 	{ 172, 	45}, 	{ 170, 	46}, 	{ 168, 	47}, 	{ 166, 	48}, 	{ 164, 	49},

104
    { 161, 	50}, 	{ 159, 	51}, 	{ 157, 	52}, 	{ 154, 	53}, 	{ 152, 	54}, 	{ 150, 	55}, 	{ 148, 	56}, 	{ 146, 	57}, 	{ 143, 	58}, 	{ 141, 	59},

105
    { 139, 	60}, 	{ 136, 	61}, 	{ 134, 	62}, 	{ 132, 	63}, 	{ 130, 	64}, 	{ 128, 	65}, 	{ 125, 	66}, 	{ 123, 	67}, 	{ 121, 	68}, 	{ 119, 	69},

106
    { 117, 	70}, 	{ 115, 	71}, 	{ 113, 	72}, 	{ 111, 	73}, 	{ 109, 	74}, 	{ 106, 	75}, 	{ 105, 	76}, 	{ 103, 	77}, 	{ 101, 	78}, 	{ 99, 	79},

107
    { 97, 	80}, 	{ 95, 	81}, 	{ 93, 	82}, 	{ 91, 	83}, 	{ 90, 	84}, 	{ 88, 	85}, 	{ 85, 	86}, 	{ 84, 	87}, 	{ 82, 	88}, 	{ 81, 	89},

108
    { 79, 	90}, 	{ 77, 	91}, 	{ 76, 	92}, 	{ 74, 	93}, 	{ 73, 	94}, 	{ 72, 	95}, 	{ 69, 	96}, 	{ 68, 	97}, 	{ 66, 	98}, 	{ 65, 	99},

109
    { 64, 	100}, 	{ 62, 	101}, 	{ 62, 	102}, 	{ 61, 	103}, 	{ 59, 	104}, 	{ 58, 	105}, 	{ 56, 	106}, 	{ 54, 	107}, 	{ 54, 	108}, 	{ 53, 	109},

110
    { 51, 	110}, 	{ 51, 	111}, 	{ 50, 	112}, 	{ 48, 	113}, 	{ 48, 	114}, 	{ 46, 	115}, 	{ 46, 	116}, 	{ 44, 	117}, 	{ 43, 	118}, 	{ 43, 	119},

111
    { 41, 	120}, 	{ 41, 	121}, 	{ 39, 	122}, 	{ 39, 	123}, 	{ 39, 	124}, 	{ 37, 	125}, 	{ 37, 	126}, 	{ 35, 	127}, 	{ 35, 	128}, 	{ 33, 	129},

112
};

113


114
uint8_t HWESC_Telem::temperature_decode(uint8_t temp_raw) const

115
{

116
    for (uint8_t i=0; i<ARRAY_SIZE(temp_table); i++) {

117
        if (temp_table[i].adc_temp <= temp_raw) {

118
            return temp_table[i].temp_C;

119
        }

120
    }

121
    return 130U;

122
}

123


124
/*

125
  parse packet

126
 */

127
bool HWESC_Telem::parse_packet(void)

128
{

129
    uint16_t crc = calc_crc((uint8_t *)&pkt, sizeof(pkt)-2);

130
    if (crc != pkt.crc) {

131
        return false;

132
    }

133


134
    decoded.counter = be32toh(pkt.counter);

135
    decoded.throttle_req = be16toh(pkt.throttle_req);

136
    decoded.throttle = be16toh(pkt.throttle);

137
    decoded.rpm = be16toh(pkt.rpm) * 5.0 / 7.0; // scale from eRPM to RPM

138
    decoded.voltage = be16toh(pkt.voltage) * 0.1;

139
    decoded.phase_current = int16_t(be16toh(pkt.phase_current)) * 0.01;

140
    decoded.current = int16_t(be16toh(pkt.current)) * 0.01;

141
    decoded.mos_temperature = temperature_decode(pkt.mos_temperature);

142
    decoded.cap_temperature = temperature_decode(pkt.cap_temperature);

143
    decoded.status = be16toh(pkt.status);

144
    if (decoded.status != 0) {

145
        decoded.error_count++;

146
    }

147


148
    return true;

149
}

150


151
void AP_Periph_FW::hwesc_telem_update()

152
{

153
    if (!hwesc_telem.update()) {

154
        return;

155
    }

156
    const HWESC_Telem::HWESC &t = hwesc_telem.get_telem();

157


158
    uavcan_equipment_esc_Status pkt {};

159
    pkt.esc_index = g.esc_number[0]; // only supports a single ESC

160
    pkt.voltage = t.voltage;

161
    pkt.current = t.current;

162
    pkt.temperature = C_TO_KELVIN(MAX(t.mos_temperature, t.cap_temperature));

163
    pkt.rpm = t.rpm;

164
    pkt.power_rating_pct = t.phase_current;

165
    pkt.error_count = t.error_count;

166


167
    uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE];

168
    uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !canfdout());

169
    canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,

170
                    UAVCAN_EQUIPMENT_ESC_STATUS_ID,

171
                    CANARD_TRANSFER_PRIORITY_LOW,

172
                    &buffer[0],

173
                    total_size);

174
}

175


176
#endif // HAL_PERIPH_ENABLE_HWESC

177


178


179