diff --git a/libraries/AP_Scripting/applets/pelco_d_antennatracker.lua b/libraries/AP_Scripting/applets/pelco_d_antennatracker.lua new file mode 100644 index 0000000000..9f80c82fa7 --- /dev/null +++ b/libraries/AP_Scripting/applets/pelco_d_antennatracker.lua @@ -0,0 +1,110 @@ +--[[ + Pelco-D control implementation for antennatracker. + Implemented by using knowledge from the excellent python implementation in https://gist.github.com/jn0/cc5c78f4a0f447a6fb2e45a5d9efa13d. +--]] + +local SERVO_PAN = 71 +local SERVO_TILT = 72 +local SERIAL_BAUD = 9600 + +-- Antennattracker modes +local MODE_STOP = 1 +local MODE_SCAN = 2 +local MODE_SERVOTEST = 3 +local MODE_GUIDED = 4 + +gcs:send_text(0, "Starting Pelco-D Control") + +local port = assert(serial:find_serial(0), "Pelco-D: No Scripting Serial Port") +port:begin(SERIAL_BAUD) +port:set_flow_control(0) + +function set_bit(value, n) + return value | (0x01 << n) +end + +function PelcoD_msg_addchecksum(msg) + local sum = 0 + for i = 2, #msg-1 do + sum = sum + msg[i] + end + checksum = sum % 256 + msg[7] = checksum +end + +function PelcoD_move(panspeed, tiltspeed) + local command = 0x00 + local scale = 63 -- max pelcod speed + if panspeed < 0 then -- left + command = set_bit(command, 2) + elseif panspeed > 0 then -- right + command = set_bit(command, 1) + end + if tiltspeed < 0 then -- down + command = set_bit(command, 4) + elseif tiltspeed > 0 then -- up + command = set_bit(command, 3) + end + local msg = {0xFF, 0x01, 0x00, command, math.floor(math.abs(panspeed) * scale), math.floor(math.abs(tiltspeed) * scale), 0x00} + PelcoD_msg_addchecksum(msg) + return msg +end + +-- write msg to the serial port +function send_message(msg) + for _, v in ipairs(msg) do + port:write(v) + end +end + +function update() + tilt_norm = SRV_Channels:get_output_scaled(SERVO_TILT) + pan_norm = SRV_Channels:get_output_scaled(SERVO_PAN) + if (vehicle:get_mode() == MODE_SCAN or vehicle:get_mode() == MODE_SERVOTEST or vehicle:get_mode() == MODE_GUIDED) then + -- Limit pan and tilt to -1...+1 + pan_norm=math.max(pan_norm,-1.0) + pan_norm=math.min(pan_norm,1.0) + tilt_norm=math.max(tilt_norm,-1.0) + tilt_norm=math.min(tilt_norm,1.0) + local msg=PelcoD_move(-pan_norm,tilt_norm) + send_message(msg) + elseif (vehicle:get_mode() == MODE_STOP) then + local msg=PelcoD_move(0,0) + send_message(msg) + end + + return update, 20 -- 50 hz +end + +PelcoD_move(0,0) +return update() + + +--[[ +function PelcoD_pan_absolute_position(degrees) + centidegrees = degrees*100 + local msg = {0xFF, 0x01, 0x00, 0x4b, (centidegrees >> 8) & 255 , centidegrees & 255, 0x00} + PelcoD_msg_addchecksum(msg) + return msg +end + +function PelcoD_tilt_absolute_position(degrees) + centidegrees = degrees*100 + local msg = {0xFF, 0x01, 0x00, 0x4d, (centidegrees >> 8) & 255 , centidegrees & 255, 0x00} + PelcoD_msg_addchecksum(msg) + return msg +end + +function PelcoD_zoom_absolute_position(position) + local msg = {0xFF, 0x01, 0x00, 0x4f, (position >> 8) & 255 , position & 255, 0x00} + PelcoD_msg_addchecksum(msg) + return msg +end + +function PelcoD_zero_absolute_position(degrees) + centidegrees = degrees*100 + local msg = {0xFF, 0x01, 0x00, 0x49, 0x00, 0x00, 0x00} + PelcoD_msg_addchecksum(msg) + return msg +end +--]] \ No newline at end of file diff --git a/libraries/AP_Scripting/applets/pelco_d_antennatracker.md b/libraries/AP_Scripting/applets/pelco_d_antennatracker.md new file mode 100644 index 0000000000..3f79577c2d --- /dev/null +++ b/libraries/AP_Scripting/applets/pelco_d_antennatracker.md @@ -0,0 +1,16 @@ +# Pelco-D antennatracker lua script + +This scripts uses the scaled output from the antennatracker servos and map them to corresponding Pelco-D messages to be sent via a RS-485 interface to a motorized base (can be anything from motorized tracker to a PTZ camera). If your FCU doesnt offer a RS-485 interface by default, you can use or TTL-RS485- or USB-RS485-adapters. I tested this script using a USB-RS485 adapter using Linux/Obal board and a Hikvision PTZ camera. + +Pelco-D allows to control using either speed-/differential- or absolute-control control of the pan-/tilt-axis. Currently the script uses speed based control using by mapping the "ContinuousRotation" type servos outputs to the corresponding Pelco-D messages. The absolute control messages are implemented nevertheless for future use. + +The script assumes the following parameters to be set: + +SCR_ENABLE = 1 +SERVO_PITCH_TYPE = 2 # ContinuousRotation type servo +SERVO_YAW_TYPE = 2 # ContinuousRotation type servo +SERIALx_PROTOCOL = 28 # replace 'x' with the serial port used by luascript + +Additionally the PITCH2SRV, YAW2SRV tuning needs to be done as described by the antennatracker description. +Also keep attention to the PITCH_MIN, PITCH_MAX and YAW_RANGE parameters to fit your Pelco-D hardware! +