--[[ Torqeedo TorqLink driver lua script How To Use: Connect the Torqeedo motor(s) to the autopilot's CAN ports. If only one motor is used it should be connected to CAN1 If two motors are used, connect the left motor to CAN1 and the right motor to CAN2 Enable CAN1 by setting these parameters: - CAN_P1_DRIVER = 1 (First driver) - CAN_D1_PROTOCOL = 10 (Scripting) If CAN2 is being used set these parameters: - CAN_P2_DRIVER = 2 (Second driver) - CAN_D2_PROTOCOL = 12 (Scripting2) Copy this script to the autopilot's SD card in the APM/scripts directory and restart the autopilot --]] local PARAM_TABLE_KEY = 91 local PARAM_TABLE_PREFIX = "TRQL_" -- add a parameter and bind it to a variable 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 Parameter(PARAM_TABLE_PREFIX .. name) end -- setup script specific parameters assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 2), 'could not add param table') --[[ // @Param: TRQL_ENABLE // @DisplayName: Torqeedo TorqLink Enable // @Description: Torqeedo TorqLink Enable // @Values: 0:Disabled, 1:Enabled // @User: Standard --]] local TRQL_ENABLE = bind_add_param('ENABLE', 1, 1) --[[ // @Param: TRQL_DEBUG // @DisplayName: Torqeedo TorqLink Debug Level // @Description: Torqeedo TorqLink Debug Level // @Values: 0:None, 1:Low, 2:Medium, 3:High // @User: Standard --]] local TRQL_DEBUG = bind_add_param('DEBUG', 2, 0) -- global definitions local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} local TEXT_PREFIX_STR = "torqeedo-torqlink:" -- prefix for text messages sent to user local UPDATE_MS = 10 -- update interval in milliseconds, 10 = 100hz local SRV_FN_THROTTLE = 70 -- servo function for throttle local SRV_FN_THROTTLE_LEFT = 73 -- servo function for left motor local SRV_FN_THROTTLE_RIGHT = 74 -- servo function for right motor local CAN_BUF_LEN = 8 -- CAN frame length local CAN_WRITE_TIMEOUT_US = 10000 -- timeout in microseconds for writing a frame to the Torqeedo device local TORQCAN_DIR_FORWARD = 126 -- forward direction command local TORQCAN_DIR_NEUTRAL = 125 -- neutral direction command local TORQCAN_DIR_BACKWARDS = 124 -- reverse direction command local TORQCAN_PF_MASK = 0x00FF0000 -- bitmask used to extract health from PGN message local TORQCAN_DA_MASK = 0x0000FF00 -- bitmask used to extract health from PGN message local RECEIVE_TIMEOUT_MS = 1000 -- timeout for receiving a frame from the Torqeedo device local OUTPUT_INTERVAL_MS = 50 -- output interval in milliseconds, 50 = 20hz local RECOVERY_DELAY_MS = 2000 -- delay (in ms) after motor recovers before sending outputs -- local variables local left_device_last_recv_ms = uint32_t(0) -- system time of last frame received from CAN1 device local right_device_last_recv_ms = uint32_t(0) -- system time of last frame received from CAN2 device local left_device_timeout = true -- flag to indicate if the left device is in a timeout state local right_device_timeout = true -- flag to indicate if the right device is in a timeout state local last_send_ms = uint32_t(0) -- system time of last command sent to Torqeedo devices local recovery_start_ms = uint32_t(0) -- system time motor communication was recovered (0 if not recovered recently) local debug_send_speed_ms = uint32_t(0) -- system time of last debug speed message -- get CAN drivers with protocol configured for scripting, set buffer size to 8 frames local left_device = CAN:get_device(CAN_BUF_LEN) local right_device = CAN:get_device2(CAN_BUF_LEN) -- swap two floats function swap_float(f1, f2) return f2, f1 end -- interpolate function function interpolate(output_low, output_high, input_value, input_low, input_high) -- support either polarity if (input_low > input_high) then input_low, input_high = swap_float(input_low, input_high) output_low, output_high = swap_float(output_low, output_high) end if (input_value <= input_low) then return output_low end if (input_value > input_high) then return output_high end local p = (input_value - input_low) / (input_high - input_low) return math.floor((output_low + p * (output_high - output_low))) end -- send speed and direction control commands -- speed should be in the range of -1 to +1 function send_speed_and_direction(device, speed) -- sanity check device if not device then return end -- get absolute speed local speed_abs = math.abs(speed) -- convert speed into range 0 to 250 local speed_cmd = interpolate(0.0, 250.0, speed_abs, 0.0, 1.0) -- extended frame, priority 12, PGN 0xF003, and node ID 208 - (0x8CF003d0) -- lua cannot handle numbers so large, so we have to use uint32_t userdata local speed_frame = CANFrame() speed_frame:id((uint32_t(1) << 31) | (uint32_t(12) << 24) | (uint32_t(tonumber("0xF003")) << 8) | uint32_t(208)) speed_frame:dlc(8) speed_frame:data(0, 255) speed_frame:data(1, speed_cmd) speed_frame:data(2, 255) speed_frame:data(3, 255) speed_frame:data(4, 255) speed_frame:data(5, 255) speed_frame:data(6, 255) speed_frame:data(7, 255) -- write the speed frame with a 10000us timeout device:write_frame(speed_frame, CAN_WRITE_TIMEOUT_US) -- calculate direction local dir_cmd = TORQCAN_DIR_NEUTRAL if speed < 0 then dir_cmd = TORQCAN_DIR_BACKWARDS elseif speed > 0 then dir_cmd = TORQCAN_DIR_FORWARD end -- set transmission to forward local dir_frame = CANFrame() -- extended frame, priority 12, PGN 0xF005, and node ID 208 - (0x8CF005d0) -- lua cannot handle numbers so large, so we have to use uint32_t userdata dir_frame:id((uint32_t(1) << 31) | (uint32_t(12) << 24) | (uint32_t(tonumber("0xF005")) << 8) | uint32_t(208)) dir_frame:dlc(8) dir_frame:data(0, dir_cmd) -- transmission gear (124-126) dir_frame:data(1, 255) dir_frame:data(2, 255) dir_frame:data(3, 255) dir_frame:data(4, 255) dir_frame:data(5, 255) dir_frame:data(6, 255) dir_frame:data(7, 255) -- write the direction frame with a 10000us timeout device:write_frame(dir_frame, CAN_WRITE_TIMEOUT_US) -- print frame debug at 1hz if TRQL_DEBUG:get() > 0 then local now_ms = millis() if (now_ms - debug_send_speed_ms) > 1000 then debug_send_speed_ms = now_ms gcs:send_text(MAV_SEVERITY.INFO, string.format("%s speed:%4.2f dir:%d", TEXT_PREFIX_STR, speed_cmd, dir_cmd)) end end end -- get parameter group number (PGN) from frame id function get_PGN_from_frameid(can_frame_id) local pf = (TORQCAN_PF_MASK & can_frame_id) >> 16 local da = (TORQCAN_DA_MASK & can_frame_id) >> 8 local pgn if pf >= 240 then pgn = pf * 256 + da else pgn = pf * 256 end return pgn end -- parse a frame, returns true if motor is healthy function parse_frame_for_health(frame) -- extract Parameter Group Number (PGN) from frame id local pgn = tostring(get_PGN_from_frameid(frame:id())) if pgn == "65299" then -- check if Torqeedo is ready local ready = frame:data(4) >> 7 -- check first bit of thruster status bitmap if ready == 1 then do return true end end end -- print incoming frame debug if TRQL_DEBUG:get() > 1 then gcs:send_text(MAV_SEVERITY.INFO, string.format("%s read id:%d PGN:%s", TEXT_PREFIX_STR, frame:id():toint(), pgn)) end return false end -- read incoming frames function read_incoming_frames() -- read incoming frame from left device if left_device then local frame = left_device:read_frame() if frame and parse_frame_for_health(frame) then left_device_last_recv_ms = millis() end end -- read incoming frame from right device if right_device then local frame = right_device:read_frame() if frame and parse_frame_for_health(frame) then right_device_last_recv_ms = millis() end end end -- print welcome message gcs:send_text(MAV_SEVERITY.INFO, "torqeedo-torqlink script loaded") -- update function runs at about 20hz function update() -- check if script is enabled if TRQL_ENABLE:get() == 0 then return update, 1000 end -- read incoming frames from devices read_incoming_frames() -- get current time local now_ms = millis() -- check for timeouts of left device local left_device_timeout_prev = left_device_timeout left_device_timeout = (now_ms - left_device_last_recv_ms) > RECEIVE_TIMEOUT_MS if left_device_timeout ~= left_device_timeout_prev then if left_device_timeout then gcs:send_text(MAV_SEVERITY.WARNING, TEXT_PREFIX_STR .. "device1 timeout") else gcs:send_text(MAV_SEVERITY.INFO, TEXT_PREFIX_STR .. "device1 healthy") recovery_start_ms = now_ms end end -- check for timeouts of right device local right_device_timeout_prev = right_device_timeout right_device_timeout = (now_ms - right_device_last_recv_ms) > RECEIVE_TIMEOUT_MS if right_device_timeout ~= right_device_timeout_prev then if right_device_timeout then gcs:send_text(MAV_SEVERITY.WARNING, TEXT_PREFIX_STR .. "device2 timeout") else gcs:send_text(MAV_SEVERITY.INFO, TEXT_PREFIX_STR .. "device2 healthy") recovery_start_ms = now_ms end end -- send commands to motors at specified interval if (now_ms - last_send_ms) >= OUTPUT_INTERVAL_MS then -- record last send time (we will certainly send a command below) last_send_ms = now_ms -- suppress output for 2 seconds after recovery local recovery_active = false if recovery_start_ms > 0 then if ((now_ms - recovery_start_ms) < RECOVERY_DELAY_MS) then recovery_active = true else -- clear recovery start time recovery_start_ms = uint32_t(0) end end -- determine whether we should send throttle or left and right throttle local throttle_defined = (SRV_Channels:find_channel(SRV_FN_THROTTLE) ~= nil) local left_throttle_defined = (SRV_Channels:find_channel(SRV_FN_THROTTLE_LEFT) ~= nil) local right_throttle_defined = (SRV_Channels:find_channel(SRV_FN_THROTTLE_RIGHT) ~= nil) -- default left and right speed to 0 (in range -1 to +1) local left_device_speed = 0 local right_device_speed = 0 -- use throttle by default but use left and/or right throttle if defined if throttle_defined then left_device_speed = SRV_Channels:get_output_scaled(SRV_FN_THROTTLE) * 0.01 right_device_speed = SRV_Channels:get_output_scaled(SRV_FN_THROTTLE) * 0.01 end if left_throttle_defined then left_device_speed = SRV_Channels:get_output_scaled(SRV_FN_THROTTLE_LEFT) * 0.001 end if right_throttle_defined then right_device_speed = SRV_Channels:get_output_scaled(SRV_FN_THROTTLE_RIGHT) * 0.001 end -- override speed if device timesout or in recovery if left_device_timeout or recovery_active then left_device_speed = 0 end if right_device_timeout or recovery_active then right_device_speed = 0 end -- if no outputs defined print warning to user if not (left_throttle_defined or right_throttle_defined or throttle_defined) then gcs:send_text(MAV_SEVERITY.WARNING, TEXT_PREFIX_STR .. "check SERVOx_FUNCTION") else -- send speed and direction to left and right devices send_speed_and_direction(left_device, left_device_speed) send_speed_and_direction(right_device, right_device_speed) end end return update, UPDATE_MS end return update()