-- mount-viewpro-driver.lua: Viewpro mount/gimbal driver --[[ How to use Connect gimbal UART to one of the autopilot's serial ports Set SERIALx_PROTOCOL = 28 (Scripting) where "x" corresponds to the serial port connected to the gimbal Set SCR_ENABLE = 1 to enable scripting and reboot the autopilot Set MNT1_TYPE = 9 (Scripting) to enable the mount/gimbal scripting driver Set CAM_TRIGG_TYPE = 3 (Mount) to enable the camera driver Set RCx_OPTION = 300 (Scripting1) to allow real-time selection of the video feed and camera control Copy this script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot Set VIEP_CAM_SWLOW, VIEP_CAM_SWMID, VIEP_CAM_SWHIGH to which cameras are controlled by the auxiliary switch 0: No change in camera selection 1: EO1 2: IR thermal 3: EO1 + IR Picture-in-picture 4: IR + EO1 Picture-in-picture 5: Fusion 6: IR1 13mm 7: IR2 52mm Set VIEP_ZOOM_SPEED to control speed of zoom (value between 0 and 7) Set VIEP_DEBUG = 1 or 2 to increase level of debug output to the GCS Packet format byte description notes 0~2 header 0x55 0xAA 0xDC 3 body length bit0~5: body length, n=all bytes from byte3 to checksum, min=4, max=63. bits6~7: frame counter 4 frame id 5~n+1 data 1st byte is command id? n+2 checksum XOR of byte3 to n+1 (inclusive) --]] -- parameters local PARAM_TABLE_KEY = 39 assert(param:add_table(PARAM_TABLE_KEY, "VIEP_", 5), "could not add param table") assert(param:add_param(PARAM_TABLE_KEY, 1, "DEBUG", 0), "could not add VIEP_DEBUG param") assert(param:add_param(PARAM_TABLE_KEY, 2, "CAM_SWLOW", 1), "could not add VIEP_CAM_SWLOW param") assert(param:add_param(PARAM_TABLE_KEY, 3, "CAM_SWMID", 2), "could not add VIEP_CAM_SWMID param") assert(param:add_param(PARAM_TABLE_KEY, 4, "CAM_SWHIGH", 6), "could not add VIEP_CAM_CAM_SWHIGH param") assert(param:add_param(PARAM_TABLE_KEY, 5, "ZOOM_SPEED", 7), "could not add VIEP_ZOOM_SPEED param") -- bind parameters to variables local MNT1_TYPE = Parameter("MNT1_TYPE") -- should be 9:Scripting local VIEP_DEBUG = Parameter("VIEP_DEBUG") -- debug level. 0:disabled 1:enabled 2:enabled with attitude reporting local VIEP_CAM_SWLOW = Parameter("VIEP_CAM_SWLOW") -- RC swith low position's camera selection local VIEP_CAM_SWMID = Parameter("VIEP_CAM_SWMID") -- RC swith middle position's camera selection local VIEP_CAM_SWHIGH = Parameter("VIEP_CAM_SWHIGH") -- RC swith high position's camera selection local VIEP_ZOOM_SPEED = Parameter("VIEP_ZOOM_SPEED") -- zoom speed from 0 (slow) to 7 (fast) -- global definitions local CAM_SELECT_RC_OPTION = 300 -- rc channel option used to control which camera/video is used. RCx_OPTION = 300 (scripting1) local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} local INIT_INTERVAL_MS = 3000 -- attempt to initialise the gimbal at this interval local UPDATE_INTERVAL_MS = 100 -- update at 10hz local MOUNT_INSTANCE = 0 -- always control the first mount/gimbal -- packet parsing definitions local HEADER1 = 0x55 -- 1st header byte local HEADER2 = 0xAA -- 2nd header byte local HEADER3 = 0xDC -- 3rd header byte local PACKET_LENGTH_MIN = 4 -- serial packet minimum length. used for sanity checks local PACKET_LENGTH_MAX = 63 -- serial packet maximum length. used for sanity checks -- parsing state definitions local PARSE_STATE_WAITING_FOR_HEADER1 = 0 local PARSE_STATE_WAITING_FOR_HEADER2 = 1 local PARSE_STATE_WAITING_FOR_HEADER3 = 2 local PARSE_STATE_WAITING_FOR_LENGTH = 3 local PARSE_STATE_WAITING_FOR_FRAMEID = 4 local PARSE_STATE_WAITING_FOR_DATA = 5 -- received FrameId local T1_F1_B1_D1_FRAMEID = 0x40 -- includes roll, pitch, yaw angles -- camera operation commands local CAM_COMMAND_NO_ACTION = 0x00 local CAM_COMMAND_STOP_FOCUS_AND_ZOOM = 0x01 local CAM_COMMAND_ZOOM_OUT = 0x08 local CAM_COMMAND_ZOOM_IN = 0x09 local CAM_COMMAND_FOCUS_PLUS = 0x0A local CAM_COMMAND_FOCUS_MINUS = 0x0B local CAM_COMMAND_TAKE_PICTURE = 0x13 local CAM_COMMAND_START_RECORD = 0x14 local CAM_COMMAND_STOP_RECORD = 0x15 local CAM_COMMAND_AUTO_FOCUS = 0x19 local CAM_COMMAND_MANUAL_FOCUS = 0x1A -- hardcoded outgoing messages local HEARTBEAT_MSG = {0x55,0xAA,0xDC,0x44,0x00,0x00,0x44} -- local variables and definitions local uart -- uart object connected to mount local initialised = false -- true once connection to gimbal has been initialised local parse_state = PARSE_STATE_WAITING_FOR_HEADER1 -- parse state local parse_expected_crc = 0 -- incoming messages expected crc. this is checked against actual crc received local parse_length = 0 -- incoming message parsed length local parse_frameid = 0 -- incoming message command id local parse_data_buff = {} -- data buffer holding roll, pitch and yaw angles from gimbal local parse_data_bytes_recv = 0 -- count of the number of bytes received in the message so far local last_frame_counter = 0 -- last frame counter sent to gimbal. always between 0 and 3 local cam_choice = 0 -- last camera choice (see VIEP_CAM_SWLOW/MID/HIGH parameters) local cam_pic_count = 0 -- last picture count. used to detect trigger pic local cam_rec_video = false -- last record video state. used to detect record video local cam_zoom_step = 0 -- last zoom step state. zoom out = -1, hold = 0, zoom in = 1 local cam_focus_step = 0 -- last focus step state. focus in = -1, focus hold = 0, focus out = 1 local cam_autofocus = false -- last auto focus state -- parsing status reporting variables local last_print_ms = 0 -- system time that debug output was last printed local bytes_read = 0 -- number of bytes read from gimbal local bytes_written = 0 -- number of bytes written to gimbal local bytes_error = 0 -- number of bytes read that could not be parsed local msg_ignored = 0 -- number of ignored messages (because frame id does not match) -- debug variables local last_test_send_md = 0 -- system time that a test message was last sent local debug_buff = {} -- debug buffer to display bytes from gimbal -- get lowbyte of a number function lowbyte(num) return num & 0xFF end -- get highbyte of a number function highbyte(num) return (num >> 8) & 0xFF end -- get uint16 from two bytes function uint16_value(hbyte, lbyte) return ((hbyte & 0xFF) << 8) | (lbyte & 0xFF) end -- get int16 from two bytes function int16_value(hbyte, lbyte) local uret = uint16_value(hbyte, lbyte) if uret <= 0x8000 then return uret else return uret - 0x10000 end end -- wrap yaw angle in degrees to value between 0 and 360 function wrap_360(angle) local res = math.fmod(angle, 360.0) if res < 0 then res = res + 360.0 end return res end -- wrap yaw angle in degrees to value between -180 and +180 function wrap_180(angle_deg) local res = wrap_360(angle_deg) if res > 180 then res = res - 360 end return res end -- calculate crc from existing crc value and new byte function calc_crc(orig_crc, b) local crc = (orig_crc ~ b) & 0xFF return crc end -- find and initialise serial port connected to gimbal function init() -- check mount parameter if MNT1_TYPE:get() ~= 9 then gcs:send_text(MAV_SEVERITY.CRITICAL, "ViewPro: set MNT1_TYPE=9") do return end end -- find and init first instance of SERIALx_PROTOCOL = 28 (Scripting) uart = serial:find_serial(0) if uart == nil then gcs:send_text(3, "ViewPro: no SERIALx_PROTOCOL = 28") -- MAV_SEVERITY_ERR else uart:begin(115200) uart:set_flow_control(0) initialised = true gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: started") end end -- send hard coded message function send_msg(msg) for i=1,#msg do uart:write(msg[i]) -- debug bytes_written = bytes_written + 1 end end -- parse test message function parse_test_msg(msg) for i=1,#msg do parse_byte(msg[i]) end end -- reading incoming packets from gimbal function read_incoming_packets() local n_bytes = uart:available() while n_bytes > 0 do n_bytes = n_bytes - 1 parse_byte(uart:read()) end end -- parse a single byte from gimbal function parse_byte(b) -- record num bytes for reporting bytes_read = bytes_read + 1 -- debug if VIEP_DEBUG:get() > 1 then debug_buff[#debug_buff+1] = b if #debug_buff >= 10 then gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: %x %x %x %x %x %x %x %x %x %x", debug_buff[1], debug_buff[2], debug_buff[3], debug_buff[4], debug_buff[5], debug_buff[6], debug_buff[7], debug_buff[8], debug_buff[9], debug_buff[10])) debug_buff = {} end end -- waiting for header1 if parse_state == PARSE_STATE_WAITING_FOR_HEADER1 then if b == HEADER1 then parse_state = PARSE_STATE_WAITING_FOR_HEADER2 parse_expected_crc = 0 parse_data_bytes_recv = 0 do return end end bytes_error = bytes_error + 1 end -- waiting for header2 if parse_state == PARSE_STATE_WAITING_FOR_HEADER2 then if b == HEADER2 then parse_state = PARSE_STATE_WAITING_FOR_HEADER3 else -- unexpected byte so reset parsing state parse_state = PARSE_STATE_WAITING_FOR_HEADER1 bytes_error = bytes_error + 1 end do return end end -- waiting for header3 if parse_state == PARSE_STATE_WAITING_FOR_HEADER3 then if b == HEADER3 then parse_state = PARSE_STATE_WAITING_FOR_LENGTH else -- unexpected byte so reset parsing state parse_state = PARSE_STATE_WAITING_FOR_HEADER1 bytes_error = bytes_error + 1 end do return end end -- waiting for length if parse_state == PARSE_STATE_WAITING_FOR_LENGTH then parse_expected_crc = calc_crc(parse_expected_crc, b) parse_length = b & 0x3F if parse_length >= PACKET_LENGTH_MIN and parse_length <= PACKET_LENGTH_MAX then parse_state = PARSE_STATE_WAITING_FOR_FRAMEID else -- unexpected length parse_state = PARSE_STATE_WAITING_FOR_HEADER1 bytes_error = bytes_error + 1 if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.ERROR, string.format("ViewPro: invalid len:%d", parse_length)) end end do return end end -- waiting for command id if parse_state == PARSE_STATE_WAITING_FOR_FRAMEID then parse_expected_crc = calc_crc(parse_expected_crc, b) parse_frameid = b parse_state = PARSE_STATE_WAITING_FOR_DATA do return end end -- waiting for data if parse_state == PARSE_STATE_WAITING_FOR_DATA then -- check for crc if parse_data_bytes_recv >= parse_length - 3 then if b == parse_expected_crc then -- crc matched, process packet local processed = false -- T1_F1_B1_D1 if parse_frameid == T1_F1_B1_D1_FRAMEID then processed = true -- T1 holds target info including target lean angles -- F1 holds tracker sensor status (which camera, tracking vs lost) -- B1 section holds actual lean angles -- D1 section holds camera status including zoom level local servo_status = (parse_data_buff[24] & 0xF0 >> 4) local roll_deg = int16_value(parse_data_buff[24] & 0x0F, parse_data_buff[25]) * (180.0/4095.0) - 90.0 local yaw_deg = int16_value(parse_data_buff[26], parse_data_buff[27]) * (360.0 / 65536.0) local pitch_deg = int16_value(parse_data_buff[28], parse_data_buff[29]) * (360.0 / 65536.0) mount:set_attitude_euler(MOUNT_INSTANCE, roll_deg, pitch_deg, yaw_deg) if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: r:%f p:%f y:%f ss:%x", roll_deg, pitch_deg, yaw_deg, servo_status)) end end if not processed then msg_ignored = msg_ignored + 1 if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: ignored frameid:%x", parse_frameid)) end end else -- crc mismatch bytes_error = bytes_error + 1 if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: crc exp:%x got:%x", parse_expected_crc, b)) end end parse_state = PARSE_STATE_WAITING_FOR_HEADER1 do return end end -- add latest byte to crc and buffer parse_expected_crc = calc_crc(parse_expected_crc, b) parse_data_bytes_recv = parse_data_bytes_recv + 1 parse_data_buff[parse_data_bytes_recv] = b end end -- write a byte to the uart and update the checksum function write_byte(b, checksum) if b == nil or checksum == nil then gcs:send_text(3, "ViewPro: failed to write byte") -- MAV_SEVERITY_ERR return end local byte_to_write = b & 0xFF uart:write(byte_to_write) bytes_written = bytes_written + 1 local checksum_ret = (checksum ~ byte_to_write) & 0xFF return checksum_ret end -- calculate length and frame count byte -- length is all bytes after the header including CRC function calc_length_and_frame_byte(length) -- increment frame counter last_frame_counter = last_frame_counter + 1 & 0x03 return (last_frame_counter << 6 | length & 0x3F) & 0xFF end -- send target angles (in degrees) to gimbal -- yaw_angle_deg is always a body-frame angle function send_target_angles(pitch_angle_deg, yaw_angle_deg) -- prepare A1 message, FrameId 0x1A, CmdId 0x0B local length_and_frame_counter = calc_length_and_frame_byte(0x0C) local pitch_angle_output = math.floor((-pitch_angle_deg / 360.0 * 65536.0) + 0.5) local yaw_angle_output = math.floor((yaw_angle_deg / 360.0 * 65536.0) + 0.5) write_byte(HEADER1, 0) write_byte(HEADER2, 0) write_byte(HEADER3, 0) local checksum = write_byte(length_and_frame_counter, 0) -- length and frame count checksum = write_byte(0x1A, checksum) -- 0x1A: A1 FrameId checksum = write_byte(0x0B, checksum) -- 0x0B: absolute angle checksum = write_byte(highbyte(yaw_angle_output), checksum) -- yaw angle MSB checksum = write_byte(lowbyte(yaw_angle_output), checksum) -- yaw angle LSB checksum = write_byte(highbyte(pitch_angle_output), checksum) -- pitch angle MSB checksum = write_byte(lowbyte(pitch_angle_output), checksum) -- pitch angle LSB checksum = write_byte(0, checksum) -- unused checksum = write_byte(0, checksum) -- unused checksum = write_byte(0, checksum) -- unused checksum = write_byte(0, checksum) -- unused write_byte(checksum, 0) -- checksum end -- send target rates (in deg/sec) to gimbal function send_target_rates(pitch_rate_degs, yaw_rate_degs) -- prepare A1 message, FrameId 0x1A, CmdId 0x01 local length_and_frame_counter = calc_length_and_frame_byte(0x0C) local pitch_rate_output = math.floor((-pitch_rate_degs * 100.0) + 0.5) local yaw_rate_output = math.floor((yaw_rate_degs * 100.0) + 0.5) write_byte(HEADER1, 0) write_byte(HEADER2, 0) write_byte(HEADER3, 0) local checksum = write_byte(length_and_frame_counter, 0) -- length and frame count checksum = write_byte(0x1A, checksum) -- 0x1A: A1 FrameId checksum = write_byte(0x01, checksum) -- 0x01: manual rate angle checksum = write_byte(highbyte(yaw_rate_output), checksum) -- yaw rate MSB checksum = write_byte(lowbyte(yaw_rate_output), checksum) -- yaw rate LSB checksum = write_byte(highbyte(pitch_rate_output), checksum) -- pitch angle MSB checksum = write_byte(lowbyte(pitch_rate_output), checksum) -- pitch angle LSB checksum = write_byte(0, checksum) -- unused checksum = write_byte(0, checksum) -- unused checksum = write_byte(0, checksum) -- unused checksum = write_byte(0, checksum) -- unused write_byte(checksum, 0) -- checksum end -- send camera commands function send_camera_control(camera_choice, cam_command) -- prepare C1 message, FrameId 0x1C -- bits 0~2 : video choose -- bits 3~5 : zoom speed -- bits 6~12 : operation command -- bits 13~15 : LRF (rangefinder) local length_and_frame_counter = calc_length_and_frame_byte(0x05) local video_choose = camera_choice & 0x07 local zoom_speed = 0 if cam_command == CAM_COMMAND_ZOOM_OUT or cam_command == CAM_COMMAND_ZOOM_IN then zoom_speed = (VIEP_ZOOM_SPEED:get() & 0x07) << 3 end local operation_cmd = ((cam_command & 0xFFFF) & 0x7F) << 6 local data_bytes = video_choose | zoom_speed | operation_cmd -- debug --gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: camcmd:%x msb:%x lsb:%x", cam_command, highbyte(cmd_shifted), lowbyte(cmd_shifted))) gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: cam choice:%x", video_choose)) write_byte(HEADER1, 0) write_byte(HEADER2, 0) write_byte(HEADER3, 0) local checksum = write_byte(length_and_frame_counter, 0) -- length and frame count checksum = write_byte(0x1C, checksum) -- 0x1C: C1 FrameId checksum = write_byte(highbyte(data_bytes), checksum) -- msb checksum = write_byte(lowbyte(data_bytes), checksum) -- lsb write_byte(checksum, 0) -- checksum end -- return camera selection according to RC switch position and VIEW_CAM_SWxxx parameter -- used in C1 message's "video choose" to specify which cameras should be controlled function get_camera_choice() local cam_switch_pos = rc:get_aux_cached(CAM_SELECT_RC_OPTION) if cam_switch_pos == 0 then return VIEP_CAM_SWLOW:get() end if cam_switch_pos == 1 then return VIEP_CAM_SWMID:get() end return VIEP_CAM_SWHIGH:get() end -- check for changes in camera state and send messages to gimbal if required function check_camera_state() -- check for change in camera local curr_cam_choice = get_camera_choice() if cam_choice ~= curr_cam_choice then cam_choice = curr_cam_choice send_camera_control(cam_choice, CAM_COMMAND_NO_ACTION) end -- get latest state from AP driver local pic_count, rec_video, zoom_step, focus_step, auto_focus = mount:get_camera_state(MOUNT_INSTANCE) -- check for take picture if pic_count and pic_count ~= cam_pic_count then cam_pic_count = pic_count send_camera_control(cam_choice, CAM_COMMAND_TAKE_PICTURE) if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: took pic %u", pic_count)) end end -- check for start/stop recording video if rec_video ~= cam_rec_video then cam_rec_video = rec_video if cam_rec_video == true then send_camera_control(cam_choice, CAM_COMMAND_START_RECORD) elseif cam_rec_video == false then send_camera_control(cam_choice, CAM_COMMAND_STOP_RECORD) end if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: rec video:" .. tostring(cam_rec_video)) end end -- check manual zoom -- zoom out = -1, hold = 0, zoom in = 1 if zoom_step ~= cam_zoom_step then cam_zoom_step = zoom_step if cam_zoom_step < 0 then send_camera_control(cam_choice, CAM_COMMAND_ZOOM_OUT) elseif cam_zoom_step == 0 then send_camera_control(cam_choice, CAM_COMMAND_STOP_FOCUS_AND_ZOOM) elseif cam_zoom_step > 0 then send_camera_control(cam_choice, CAM_COMMAND_ZOOM_IN) end if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: zoom:" .. tostring(cam_zoom_step)) end end -- check manual focus -- focus in = -1, focus hold = 0, focus out = 1 if focus_step ~= cam_focus_step then cam_focus_step = focus_step if cam_focus_step < 0 then send_camera_control(cam_choice, CAM_COMMAND_MANUAL_FOCUS) send_camera_control(cam_choice, CAM_COMMAND_FOCUS_MINUS) elseif cam_focus_step == 0 then send_camera_control(cam_choice, CAM_COMMAND_STOP_FOCUS_AND_ZOOM) elseif cam_focus_step > 0 then send_camera_control(cam_choice, CAM_COMMAND_MANUAL_FOCUS) send_camera_control(cam_choice, CAM_COMMAND_FOCUS_PLUS) end if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: focus:" .. tostring(cam_focus_step)) end end -- check auto focus if auto_focus ~= cam_autofocus then cam_autofocus = auto_focus if cam_autofocus then send_camera_control(cam_choice, CAM_COMMAND_AUTO_FOCUS) end if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: auto focus:" .. tostring(cam_autofocus)) end end end -- the main update function that performs a simplified version of RTL function update() -- get current system time local now_ms = millis() -- initialise connection to gimbal if not initialised then init() return update, INIT_INTERVAL_MS end -- status reporting if (VIEP_DEBUG:get() > 0) and (now_ms - last_print_ms > 5000) then last_print_ms = now_ms gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: r:%u w:%u err:%u ign:%u", bytes_read, bytes_written, bytes_error, msg_ignored)) end -- consume incoming bytes read_incoming_packets() -- check camera state and send control messages check_camera_state() -- send heartbeat, gimbal should respond with T1+F1+B1+D1 send_msg(HEARTBEAT_MSG) -- request gimbal attitude by sending heartbeat if now_ms - last_test_send_md > 1000 then last_test_send_md = now_ms send_msg(HEARTBEAT_MSG) end -- send target angle to gimbal local roll_deg, pitch_deg, yaw_deg, yaw_is_ef = mount:get_angle_target(MOUNT_INSTANCE) if roll_deg and pitch_deg and yaw_deg then if yaw_is_ef then yaw_deg = wrap_180(yaw_deg - math.deg(ahrs:get_yaw())) end send_target_angles(pitch_deg, yaw_deg) return update, UPDATE_INTERVAL_MS end -- send target rate to gimbal local roll_degs, pitch_degs, yaw_degs, _ = mount:get_rate_target(MOUNT_INSTANCE) if roll_degs and pitch_degs and yaw_degs then send_target_rates(pitch_degs, yaw_degs) return update, UPDATE_INTERVAL_MS end return update, UPDATE_INTERVAL_MS end return update()