diff --git a/libraries/AP_Scripting/drivers/Hobbywing_DataLink.lua b/libraries/AP_Scripting/drivers/Hobbywing_DataLink.lua new file mode 100644 index 00000000000000..43d90a70cfe211 --- /dev/null +++ b/libraries/AP_Scripting/drivers/Hobbywing_DataLink.lua @@ -0,0 +1,231 @@ +--[[ + driver for HobbyWing DataLink ESC telemetry +--]] + +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) + if curr == 0xffff then + return 0.0 + end + 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 diff --git a/libraries/AP_Scripting/drivers/Hobbywing_DataLink.md b/libraries/AP_Scripting/drivers/Hobbywing_DataLink.md new file mode 100644 index 00000000000000..fe8dd0940d3603 --- /dev/null +++ b/libraries/AP_Scripting/drivers/Hobbywing_DataLink.md @@ -0,0 +1,45 @@ +# HobbyWing ESC DataLink Driver + +This driver implements support the HobbyWing DataLink for HobbyWing +ESCs connected via a UART to an ArduPilot serial port. It supports up +to 8 ESCs. + +# Parameters + +The script used the following parameters: + +## ESC_HW_ENABLE + +this must be set to 1 to enable the driver + +## ESC_HW_POLES + +this should be set to the number of motor poles for eRPM to RPM +scaling. Please confirm the correct RPM using a tachometer + +## ESC_HW_OFS + +this parameter sets an offset for the first ESC number. It is useful +on vehicles where the first ESC is not the first SERVOn output, for +example on quadplanes. Set to zero for no offset. + +# Operation + +This driver should be loaded by placing the lua script in the +APM/SCRIPTS directory on the microSD card, which can be done either +directly or via MAVFTP. The following key parameters should be set: + + - SCR_ENABLE should be set to 1 + - ESC_HW_ENABLE should be set to 1 + - SERIALn_PROTOCOL should be set to 28 for the connected serial port + +then the flight controller should rebooted and parameters should be +refreshed. + +Once loaded the ESC_HW_ parameters will appear and should be +configured as per the above documentation. + +Note that the DataLink does not provide any data unless the motor is +running, so you cannot see any valid data at all until you arm the +motors and they start spinning. +