From c315524c44a7d728b5d7772a901c636167a42f34 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 15 Jan 2025 10:07:47 +0900 Subject: [PATCH] AP_Scripting: Torqeedo TorqLink lua driver --- .../drivers/torqeedo-torqlink.lua | 306 ++++++++++++++++++ .../AP_Scripting/drivers/torqeedo-torqlink.md | 23 ++ 2 files changed, 329 insertions(+) create mode 100644 libraries/AP_Scripting/drivers/torqeedo-torqlink.lua create mode 100644 libraries/AP_Scripting/drivers/torqeedo-torqlink.md diff --git a/libraries/AP_Scripting/drivers/torqeedo-torqlink.lua b/libraries/AP_Scripting/drivers/torqeedo-torqlink.lua new file mode 100644 index 00000000000000..09ba68fddac79d --- /dev/null +++ b/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() \ No newline at end of file diff --git a/libraries/AP_Scripting/drivers/torqeedo-torqlink.md b/libraries/AP_Scripting/drivers/torqeedo-torqlink.md new file mode 100644 index 00000000000000..aea5a1224653fb --- /dev/null +++ b/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