AP_Scripting: Torqeedo TorqLink lua driver

libraries/AP_Scripting/drivers/torqeedo-torqlink.lua

@@ -0,0 +1,306 @@
+ --[[
+    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
+    Create an APM/scripts/modules directory on the SD card
+    Copy the ardupilot/libraries/AP_Scripting/modules/NMEA_2000.lua script to the above modules directory
+    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 = 50							-- update interval in milliseconds, 50 = 20hz
+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 = 100					-- output interval in milliseconds, 100 = 10hz
+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)
+
+-- 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 (output_low + p * (output_high - output_low))
+end
+
+-- send speed and direction control commands
+-- speed should be in the range of -1000 to +1000
+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, 250, speed_abs, 0, 1000)
+
+    -- 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
+    if TRQL_DEBUG:get() > 0 then
+        gcs:send_text(MAV_SEVERITY.INFO, string.format("%s speed:%d dir:%d", TEXT_PREFIX_STR, speed_cmd, dir_cmd) .. " now:" .. tostring(millis()))
+    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
+			left_device_last_recv_ms = millis()
+		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(), pgn))
+    end
+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, UPDATE_MS
+    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
+    -- notify user if device has timedout
+    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
+    -- notify user if device has timedout
+    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
+
+        -- suppress output for 2 seconds after recovery
+        if recovery_start_ms > 0 and ((now_ms - recovery_start_ms) < RECOVERY_DELAY_MS) then
+            send_speed_and_direction(left_device, 0)
+            send_speed_and_direction(right_device, 0)
+			return update, UPDATE_MS
+		end
+
+        -- clear recovery start time
+        recovery_start_ms = uint32_t(0)
+
+        -- determine whether we should send throttle or left and right throttle
+        local throttle_defined = (SRV_Channels:find_channel(SRV_FN_THROTTLE) ~= nil)
+        local left_defined = (SRV_Channels:find_channel(SRV_FN_THROTTLE_LEFT) ~= nil)
+        local right_defined = (SRV_Channels:find_channel(SRV_FN_THROTTLE_RIGHT) ~= nil)
+
+        -- if left throttle is defined then send left and right to devices
+        if left_defined and right_defined then
+            send_speed_and_direction(left_device, SRV_Channels:get_output_scaled(SRV_FN_THROTTLE_LEFT))
+            send_speed_and_direction(right_device, SRV_Channels:get_output_scaled(SRV_FN_THROTTLE_RIGHT))
+        elseif throttle_defined then
+            local throttle_speed = SRV_Channels:get_output_scaled(SRV_FN_THROTTLE)
+            send_speed_and_direction(left_device, throttle_speed)
+            send_speed_and_direction(right_device, throttle_speed)
+        else
+            -- print warning to user
+            gcs:send_text(MAV_SEVERITY.WARNING, TEXT_PREFIX_STR .. "check SERVOx_FUNCTION")
+        end
+        last_send_ms = now_ms
+
+        -- print debug throttle values
+        if TRQL_DEBUG:get() > 0 then
+            local throttle_speed = SRV_Channels:get_output_scaled(SRV_FN_THROTTLE)
+            local left_speed = SRV_Channels:get_output_scaled(SRV_FN_THROTTLE_LEFT)
+            local right_speed = SRV_Channels:get_output_scaled(SRV_FN_THROTTLE_RIGHT)
+            gcs:send_text(MAV_SEVERITY.INFO, TEXT_PREFIX_STR .. "thr:" .. throttle_speed .. " left: " .. left_speed .. " right: " .. right_speed)
+        end
+    end
+
+    return update, UPDATE_MS
+end
+
+return update()

libraries/AP_Scripting/drivers/torqeedo-torqlink.md

@@ -0,0 +1,23 @@
+# Torqeedo TroqLink Driver
+
+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
+  Create an APM/scripts/modules directory on the SD card
+  Copy the ardupilot/libraries/AP_Scripting/modules/NMEA_2000.lua script to the above modules directory
+  Restart the autopilot