--[[ driver for HobbyWing DataLink ESC telemetry --]] ---@diagnostic disable: param-type-mismatch local PARAM_TABLE_KEY = 44 local PARAM_TABLE_PREFIX = "ESC_HW_" local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} -- bind a parameter to a variable given local function bind_param(name) local p = Parameter() assert(p:init(name), string.format('could not find %s parameter', name)) return p end -- add a parameter and bind it to a variable local function bind_add_param(name, idx, default_value) assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) return bind_param(PARAM_TABLE_PREFIX .. name) end -- setup script specific parameters assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 8), 'could not add param table') --[[ // @Param: ESC_HW_ENABLE // @DisplayName: Hobbywing ESC Enable // @Description: Enable Hobbywing ESC telemetry // @Values: 0:Disabled,1:Enabled // @User: Standard --]] ESC_HW_ENABLE = bind_add_param("ENABLE", 1, 0) --[[ // @Param: ESC_HW_POLES // @DisplayName: Hobbywing ESC motor poles // @Description: Number of motor poles for eRPM scaling // @Range: 1 50 // @User: Standard --]] ESC_HW_POLES = bind_add_param("POLES", 2, 14) --[[ // @Param: ESC_HW_OFS // @DisplayName: Hobbywing ESC motor offset // @Description: Motor number offset of first ESC // @Range: 0 31 // @User: Standard --]] ESC_HW_OFS = bind_add_param("OFS", 3, 0) if ESC_HW_ENABLE:get() ~= 1 then gcs:send_text(MAV_SEVERITY.INFO, "ESC_HW: disabled") return end local uart = serial:find_serial(0) -- first scripting serial if not uart then gcs:send_text(MAV_SEVERITY.ERROR, "ESC_HW: unable to find serial port") return end uart:begin(115200) local function read_bytes(n) local ret = "" for _ = 1, n do ret = ret .. string.char(uart:read()) end return ret end --[[ discard pending bytes --]] local function discard_pending() local n = uart:available():toint() for _ = 1, n do uart:read() end end --[[ xmodem CRC implementation thanks to https://github.com/cloudwu/skynet under MIT license --]] local XMODEMCRC16Lookup = { 0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7, 0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef, 0x1231,0x0210,0x3273,0x2252,0x52b5,0x4294,0x72f7,0x62d6, 0x9339,0x8318,0xb37b,0xa35a,0xd3bd,0xc39c,0xf3ff,0xe3de, 0x2462,0x3443,0x0420,0x1401,0x64e6,0x74c7,0x44a4,0x5485, 0xa56a,0xb54b,0x8528,0x9509,0xe5ee,0xf5cf,0xc5ac,0xd58d, 0x3653,0x2672,0x1611,0x0630,0x76d7,0x66f6,0x5695,0x46b4, 0xb75b,0xa77a,0x9719,0x8738,0xf7df,0xe7fe,0xd79d,0xc7bc, 0x48c4,0x58e5,0x6886,0x78a7,0x0840,0x1861,0x2802,0x3823, 0xc9cc,0xd9ed,0xe98e,0xf9af,0x8948,0x9969,0xa90a,0xb92b, 0x5af5,0x4ad4,0x7ab7,0x6a96,0x1a71,0x0a50,0x3a33,0x2a12, 0xdbfd,0xcbdc,0xfbbf,0xeb9e,0x9b79,0x8b58,0xbb3b,0xab1a, 0x6ca6,0x7c87,0x4ce4,0x5cc5,0x2c22,0x3c03,0x0c60,0x1c41, 0xedae,0xfd8f,0xcdec,0xddcd,0xad2a,0xbd0b,0x8d68,0x9d49, 0x7e97,0x6eb6,0x5ed5,0x4ef4,0x3e13,0x2e32,0x1e51,0x0e70, 0xff9f,0xefbe,0xdfdd,0xcffc,0xbf1b,0xaf3a,0x9f59,0x8f78, 0x9188,0x81a9,0xb1ca,0xa1eb,0xd10c,0xc12d,0xf14e,0xe16f, 0x1080,0x00a1,0x30c2,0x20e3,0x5004,0x4025,0x7046,0x6067, 0x83b9,0x9398,0xa3fb,0xb3da,0xc33d,0xd31c,0xe37f,0xf35e, 0x02b1,0x1290,0x22f3,0x32d2,0x4235,0x5214,0x6277,0x7256, 0xb5ea,0xa5cb,0x95a8,0x8589,0xf56e,0xe54f,0xd52c,0xc50d, 0x34e2,0x24c3,0x14a0,0x0481,0x7466,0x6447,0x5424,0x4405, 0xa7db,0xb7fa,0x8799,0x97b8,0xe75f,0xf77e,0xc71d,0xd73c, 0x26d3,0x36f2,0x0691,0x16b0,0x6657,0x7676,0x4615,0x5634, 0xd94c,0xc96d,0xf90e,0xe92f,0x99c8,0x89e9,0xb98a,0xa9ab, 0x5844,0x4865,0x7806,0x6827,0x18c0,0x08e1,0x3882,0x28a3, 0xcb7d,0xdb5c,0xeb3f,0xfb1e,0x8bf9,0x9bd8,0xabbb,0xbb9a, 0x4a75,0x5a54,0x6a37,0x7a16,0x0af1,0x1ad0,0x2ab3,0x3a92, 0xfd2e,0xed0f,0xdd6c,0xcd4d,0xbdaa,0xad8b,0x9de8,0x8dc9, 0x7c26,0x6c07,0x5c64,0x4c45,0x3ca2,0x2c83,0x1ce0,0x0cc1, 0xef1f,0xff3e,0xcf5d,0xdf7c,0xaf9b,0xbfba,0x8fd9,0x9ff8, 0x6e17,0x7e36,0x4e55,0x5e74,0x2e93,0x3eb2,0x0ed1,0x1ef0 } local function crc_xmodem(bytes) local crc = 0 for i=1,#bytes do local b = string.byte(bytes,i,i) crc = ((crc<<8) & 0xffff) ~ XMODEMCRC16Lookup[(((crc>>8)~b) & 0xff) + 1] end return crc end local 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}, } local function temperature_decode(temp_raw) if temp_raw == 0 then return 0 end for i = 1, #temp_table do if temp_table[i][1] <= temp_raw then return temp_table[i][2] end end return 130 end local function decode_current(curr) return curr / 64.0 end local telem_data = ESCTelemetryData() --[[ check for input and parse data --]] local function check_input() local n_bytes = uart:available():toint() if n_bytes < 160 then return end --gcs:send_text(0,string.format("n_bytes=%u", n_bytes)) if n_bytes > 160 then discard_pending() return end local s = read_bytes(n_bytes) local head, frame_len, ver, cmd, _ = string.unpack(">BBBBH", string.sub(s,1,6)) if head ~= 0x9B or frame_len ~= 158 or ver ~= 1 or cmd ~= 2 then gcs:send_text(MAV_SEVERITY.INFO, string.format("bad frame %x %x %x %x", head, frame_len, ver, cmd)) return end local crc1 = string.unpack("BHHHHHhhBBH", e) local RPM = math.floor(eRPM*10.0/ESC_HW_POLES:get()) if volt > 0 or curr > 0 or RPM > 0 or pnum > 1 then -- we have valid ESC data local ofs = ESC_HW_OFS:get() curr = decode_current(curr) pcurr = decode_current(pcurr) volt = volt * 0.1 in_thr = in_thr / 32768.0 out_thr = out_thr / 32768.0 mos_temp = temperature_decode(mos_temp) cap_temp = temperature_decode(cap_temp) telem_data:voltage(volt) telem_data:current(curr) telem_data:temperature_cdeg(math.floor(mos_temp*100)) esc_telem:update_rpm(ofs+i, math.floor(eRPM*10.0/ESC_HW_POLES:get()), 0) -- 0x0D is temperature + voltage + current esc_telem:update_telem_data(ofs+i, telem_data, 0x0D) logger.write('HWES','I,PNum,RPM,Curr,Volt,InT,OutT,PCurr,MosT,CapT,Status', 'BHHfffffBBH', '#-qAv--AOO-', '--00000000-', ofs+i, pnum, RPM, curr, volt, in_thr, out_thr, pcurr, mos_temp, cap_temp, status) end end end --[[ main update function --]] local function update() check_input() return update, 10 end gcs:send_text(MAV_SEVERITY.ALERT, "ESC_HW: loaded") return update, 100