AP_Scripting: added tracker Pelco-D control script

This script 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.

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 therefor at least 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 # serial port used by luascript

Additionally the PITCH2SRV and 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!

Author: petrosilius

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
+--]]

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!
+