diff --git a/libraries/AP_Scripting/drivers/UltraMotion.lua b/libraries/AP_Scripting/drivers/UltraMotion.lua index b9d309109f0fea..18ba808d2b45ca 100644 --- a/libraries/AP_Scripting/drivers/UltraMotion.lua +++ b/libraries/AP_Scripting/drivers/UltraMotion.lua @@ -27,6 +27,15 @@ assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 8), 'could not add p --]] UM_SERVO_MASK = bind_add_param("SERVO_MASK", 1, 0) +--[[ + // @Param: UM_CANDRV + // @DisplayName: Set CAN driver + // @Description: Set CAN driver + // @Values: 0:None,1:1stCANDriver,2:2ndCanDriver + // @User: Standard +--]] +local UM_CANDRV = bind_add_param('CANDRV', 2, 1) -- CAN driver to use + --[[ // @Param: UM_RATE_HZ // @DisplayName: Update rate for UltraMotion servos @@ -35,7 +44,7 @@ UM_SERVO_MASK = bind_add_param("SERVO_MASK", 1, 0) // @Range: 1 400 // @User: Standard --]] -UM_RATE_HZ = bind_add_param("RATE_HZ", 2, 70) +UM_RATE_HZ = bind_add_param("RATE_HZ", 3, 70) --[[ // @Param: UM_TORQUE_MAX @@ -45,7 +54,7 @@ UM_RATE_HZ = bind_add_param("RATE_HZ", 2, 70) // @Range: 1 100 // @User: Standard --]] -UM_TORQUE_MAX = bind_add_param("TORQUE_MAX", 3, 30.5) -- default matches datasheet +UM_TORQUE_MAX = bind_add_param("TORQUE_MAX", 4, 30.5) -- default matches datasheet --[[ // @Param: UM_OPTIONS @@ -54,7 +63,7 @@ UM_TORQUE_MAX = bind_add_param("TORQUE_MAX", 3, 30.5) -- default matches datashe // @Bitmask: 0:LogAllFrames,1:ParseTelemetry // @User: Standard --]] -UM_OPTIONS = bind_add_param("OPTIONS", 4, 0) +UM_OPTIONS = bind_add_param("OPTIONS", 5, 0) --[[ // @Param: UM_TELEM_TXID @@ -62,7 +71,7 @@ UM_OPTIONS = bind_add_param("OPTIONS", 4, 0) // @Description: Telemetry ID // @User: Standard --]] -UM_TELEM_TXID = bind_add_param("TELEM_TXID", 5, 0) +UM_TELEM_TXID = bind_add_param("TELEM_TXID", 6, 0) local OPTION_LOGALLFRAMES = 0x01 local OPTION_PARSETELEM = 0x02 @@ -74,7 +83,12 @@ end -- Load CAN driver, using the scripting protocol -- use a buffer size of 25 -local driver = CAN:get_device(25) +local CAN_BUF_LEN = 25 +if UM_CANDRV:get() == 1 then + driver = CAN.get_device(CAN_BUF_LEN) +elseif UM_CANDRV:get() == 2 then + driver = CAN.get_device2(CAN_BUF_LEN) +end if not driver then gcs:send_text(MAV_SEVERITY.INFO, "UltraMotion: init failed") return diff --git a/libraries/AP_Scripting/drivers/UltraMotion.md b/libraries/AP_Scripting/drivers/UltraMotion.md new file mode 100644 index 00000000000000..feddbbc1122c5b --- /dev/null +++ b/libraries/AP_Scripting/drivers/UltraMotion.md @@ -0,0 +1,38 @@ +# UltraMotion CAN Driver + +This driver implements support for the UltraMotion CAN servos + +# Parameters + +The script used the following parameters: + +## UM_SERVO_MASK + +Mask of servo channels to transmit using UltraMotion CAN messages + +## UM_CANDRV + +This sets the CAN scripting driver number to attach to. This is +normally set to 1 to use a CAN driver with CAN_Dx_PROTOCOL=10. To use +the 2nd scripting CAN driver set this to 2 and set CAN_Dx_PROTOCOL=12. + +## UM_RATE_HZ + +This sets the update rate of the script in Hz (how often it checks for +new data from the ECU). A value of 200 is reasonable. + +## UM_TORQUE_MAX + +This sets the commanded maximum torque as a percentage + +# 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 + - UM_SERVO_MASK needs to be set to a mask of servos + +then the flight controller should rebooted and parameters should be +refreshed.