diff --git a/libraries/AP_Scripting/drivers/UltraMotion.lua b/libraries/AP_Scripting/drivers/UltraMotion.lua new file mode 100644 index 0000000000000..b9d309109f0fe --- /dev/null +++ b/libraries/AP_Scripting/drivers/UltraMotion.lua @@ -0,0 +1,195 @@ +--[[ + driver for UltraMotion servos + https://www.ultramotion.com/ + based on an earlier driver by Fred Darnell +--]] + +local PARAM_TABLE_KEY = 89 +local PARAM_TABLE_PREFIX = "UM_" + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +-- 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 Parameter(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: UM_SERVO_MASK + // @DisplayName: Mask of UltraMotion servos + // @Description: Mask of UltraMotion servos + // @Bitmask: 0:SERVO1,1:SERVO2,2:SERVO3,3:SERVO4,4:SERVO5,5:SERVO6,6:SERVO7,7:SERVO8,8:SERVO9,9:SERVO10,10:SERVO11,11:SERVO12 + // @User: Standard +--]] +UM_SERVO_MASK = bind_add_param("SERVO_MASK", 1, 0) + +--[[ + // @Param: UM_RATE_HZ + // @DisplayName: Update rate for UltraMotion servos + // @Description: Update rate for UltraMotion servos + // @Units: Hz + // @Range: 1 400 + // @User: Standard +--]] +UM_RATE_HZ = bind_add_param("RATE_HZ", 2, 70) + +--[[ + // @Param: UM_TORQUE_MAX + // @DisplayName: Maximum torque to command + // @Description: Maximum torque to command + // @Units: % + // @Range: 1 100 + // @User: Standard +--]] +UM_TORQUE_MAX = bind_add_param("TORQUE_MAX", 3, 30.5) -- default matches datasheet + +--[[ + // @Param: UM_OPTIONS + // @DisplayName: Optional settings + // @Description: Optional settings + // @Bitmask: 0:LogAllFrames,1:ParseTelemetry + // @User: Standard +--]] +UM_OPTIONS = bind_add_param("OPTIONS", 4, 0) + +--[[ + // @Param: UM_TELEM_TXID + // @DisplayName: Telemetry ID + // @Description: Telemetry ID + // @User: Standard +--]] +UM_TELEM_TXID = bind_add_param("TELEM_TXID", 5, 0) + +local OPTION_LOGALLFRAMES = 0x01 +local OPTION_PARSETELEM = 0x02 + +if UM_SERVO_MASK:get() == 0 then + gcs:send_text(MAV_SEVERITY.INFO, "UltraMotion UM_SERVO_MASK is empty") + return +end + +-- Load CAN driver, using the scripting protocol +-- use a buffer size of 25 +local driver = CAN:get_device(25) +if not driver then + gcs:send_text(MAV_SEVERITY.INFO, "UltraMotion: init failed") + return +end + +local frame_count = 0 + +-- marker for extended frame format +local CAN_FLAG_EFF = uint32_t(1)<<31 + +--[[ + frame logging - can be replayed with Tools/scripts/CAN/CAN_playback.py +--]] +local function log_can_frame(frame) + logger:write("CANF",'Id,DLC,FC,B0,B1,B2,B3,B4,B5,B6,B7','IBIBBBBBBBB', + frame:id(), + frame:dlc(), + frame_count, + frame:data(0), frame:data(1), frame:data(2), frame:data(3), + frame:data(4), frame:data(5), frame:data(6), frame:data(7)) + frame_count = frame_count + 1 +end + +--[[ + create a new actuator object +--]] +function Actuator(unitID) + local o = {} + o.unitID = unitID or 0 + -- pre-fill the msg ID to avoid expensive uint32_t operations at runtime + o.msg = CANFrame() + o.msg:id(CAN_FLAG_EFF | uint32_t(unitID)) + o.msg:dlc(4) + return o +end + +--[[ + put a 16 bit little endian value +--]] +local function put_uint16(msg, ofs, value) + msg:data(ofs, value & 0xFF) + msg:data(ofs+1, value >> 8) +end + +--[[ + use the UM_SERVO_MASK to create a table of actuators + need to restart scripting to change the UM_SERVO_MASK +--]] +local actuators = {} +for i = 1, 32 do + local mask = 1 << (i-1) + if UM_SERVO_MASK:get() & mask ~= 0 then + actuators[i] = Actuator(i) + end +end + +--[[ + send outputs to all servos +--]] +local function send_outputs() + local noutputs = #actuators + local torque_max = math.floor(UM_TORQUE_MAX:get() * 32767.0 * 0.01) + for i = 1, noutputs do + local pwm = SRV_Channels:get_output_pwm_chan(actuators[i].unitID) + local msg = actuators[i].msg + + put_uint16(msg, 0, pwm) + put_uint16(msg, 2, torque_max) + + driver:write_frame(msg, 10000) + end +end + +--[[ + parse one telemetry frame. The telemetry data format depends on + the txData parameter set in the servos + + This code assumes txData is pABCDEFS +--]] +local function parse_telemetry(frame) + local bytes = "" + local dlc = frame:dlc() + for i = 1, dlc do + bytes = bytes .. string.char(frame:data(i-1)) + end + local unitid, status, current, pos = string.unpack("