ardupilot/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua

-- mount-djirs2-driver.lua: DJIRS2 mount/gimbal driver
-- luacheck: only 0

--[[
  How to use
    Connect gimbal to autopilot's CAN1 port or CAN2 port
    If connected to CAN1, set CAN_D1_PROTOCOL = 10 (Scripting), CAN_P1_DRIVER = 1 (First driver)
    If connected to CAN2, set CAN_D2_PROTOCOL = 10 (Scripting), CAN_P2_DRIVER = 2 (Second driver)
    Set SCR_ENABLE = 1 to enable scripting
    Set SCR_HEAP_SIZE = 120000 (or higher)
    Set MNT1_TYPE = 9 (Scripting) to enable the mount/gimbal scripting driver
    Reboot the autopilot
    Copy this script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot
    set DJIR_DEBUG to 1 to display parsing and errors stats at 5sec.  Set to 2 to display gimbal angles
 
  The following sources were used as a reference during the development of this script
    Constant Robotics DJIR SDK: https://github.com/ConstantRobotics/DJIR_SDK
    Ceinem's ROS node for DJI RS2: https://github.com/ceinem/dji_rs2_ros_controller

  CAN format
    packet's data segment holds up to 8 serial bytes.  These should be extracted and sent to the serial packet parser
    the CAN packets will never hold the contents of more than one serial packet

    the external caller (e.g. this script) must send CAN frames with frameId = 0x223
    gimbal will reply with frameId = 0x222

  Serial Packet format
     byte      description     notes
     0         header          AA
     1~2       Ver/Length      bits 0~9 are length of entire frame, LSB first. bits 10~15 are version number
     3         CmdType         bits 0~4 are Reply type, 0=No Reply required, 1=Can reply or not after data sent, 2~31=Reply is required after data sent
                               bit 5 is Frame type, 0=Command frame, 1=Reply Frame
                               bits 6~7 reserved (0 by default)
     4         ENC             bits 0~4 length of supplementary bytes when encrypting
                               bits 5~7 Encryption type, 0=Unencrypted, 1=AES256 encyrption
     5~7       RES             reserved
     8~9       SEQ             sequence number.  increments with each packet
     10~11     CRC-16          frame header check
     12~n      Data            Data segment Start
       12        CmdSet        Data segment Command set
       13        CmdID         Data segment Command code
       14~n      Data content  Data content
     n+1       CRC-32          frame check (the entire frame)

  Used CmdSet and CmdId
     0x0E, 0x00: Handheld Gimbal Position Control
       Command frame bytes
        0~1: yaw angle * 10, int16, -1800 to +1800
        2~3: roll angle * 10, int16, -300 to +300
        4~5: pitch angle * 10, int16, -560 to +1460
        6: ctrl_byte, uint8
              bit0 = 0:relative control, 1:absolute control
              bit1 = 0:yaw axis valid, 1:invalid
              bit2 = 0:roll axis valid, 1:invalid
              bit3 = 0:pitch axis valid, 1:invalid
              bit4~7 = reserved, must be zero
        7: time for action, uint8_t, unit: 0.1s.  e.g. if 20, gimbal will rotate to the position desired within 2sec
      Reply frame bytes
        0: return code, uint8_t

     0x0E, 0x02: Obtain the angle information of handheld gimbal, including joint angle and attitude angle
       Command frame bytes
        0: ctrl_byte, uint8_t, 0x00:No operation, 0x01:angle of handlheld gimbal, 0x02:joint angle of handheld gimbal
      Reply frame bytes
        0: return code, uint8_t
        1: data_type, uint8_t, 0x00:Data is not ready, 0x01:attitude angle, 0x02:joint angle
        2~3: yaw angle * 10, int16, -1800 to +1800
        4~5: roll angle * 10, int16, -300 to +300
        6~7: pitch angle * 10, int16, -560 to +1460


--]]

-- global definitions
local INIT_INTERVAL_MS = 3000           -- attempt to initialise the gimbal at this interval
local UPDATE_INTERVAL_MS = 1            -- update interval in millis
local REPLY_TIMEOUT_MS = 1000           -- timeout waiting for reply after 1 sec
local REQUEST_ATTITUDE_INTERVAL_MS = 100-- request attitude at 10hz
local SET_ATTITUDE_INTERVAL_MS = 100    -- set attitude at 10hz
local MOUNT_INSTANCE = 0                -- always control the first mount/gimbal
local SEND_FRAMEID = 0x223              -- send CAN messages with this frame id
local RECEIVE_FRAMEID = 0x222           -- receive CAN messages with this frame id
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}

-- parameters
local PARAM_TABLE_KEY = 38
assert(param:add_table(PARAM_TABLE_KEY, "DJIR_", 2), "could not add param table")
assert(param:add_param(PARAM_TABLE_KEY, 1, "DEBUG", 0), "could not add DJIR_DEBUG param")
assert(param:add_param(PARAM_TABLE_KEY, 2, "UPSIDEDOWN", 0), "could not add DJIR_UPSIDEDOWN param")

--[[
  // @Param: DJIR_DEBUG
  // @DisplayName: DJIRS2 debug
  // @Description: Enable DJIRS2 debug
  // @Values: 0:Disabled,1:Enabled,2:Enabled with attitude reporting
  // @User: Advanced
--]]
local DJIR_DEBUG = Parameter("DJIR_DEBUG")              -- debug level. 0:disabled 1:enabled 2:enabled with attitude reporting

--[[
  // @Param: DJIR_UPSIDEDOWN
  // @DisplayName: DJIRS2 upside down
  // @Description: DJIRS2 upside down
  // @Values: 0:Right side up,1:Upside down
  // @User: Standard
--]]
local DJIR_UPSIDEDOWN = Parameter("DJIR_UPSIDEDOWN")    -- 0:rightsideup, 1:upsidedown

-- bind parameters to variables
local CAN_P1_DRIVER = Parameter("CAN_P1_DRIVER")        -- If using CAN1, should be 1:First driver
local CAN_P1_BITRATE = Parameter("CAN_P1_BITRATE")      -- If using CAN1, should be 1000000
local CAN_D1_PROTOCOL = Parameter("CAN_D1_PROTOCOL")    -- If using CAN1, should be 10:Scripting
local CAN_P2_DRIVER = Parameter("CAN_P2_DRIVER")        -- If using CAN2, should be 2:Second driver
local CAN_P2_BITRATE = Parameter("CAN_P2_BITRATE")      -- If using CAN2, should be 1000000
local CAN_D2_PROTOCOL = Parameter("CAN_D2_PROTOCOL")    -- If using CAN2, should be 10:Scripting
local MNT1_TYPE = Parameter("MNT1_TYPE")                -- should be 9:Scripting

-- message definitions
local HEADER = 0xAA
local RETURN_CODE = {SUCCESS=0x00, PARSE_ERROR=0x01, EXECUTION_FAILED=0x02, UNDEFINED=0xFF}

-- parsing state definitions
local PARSE_STATE_WAITING_FOR_HEADER        = 0
local PARSE_STATE_WAITING_FOR_VERLENGTH     = 1
local PARSE_STATE_WAITING_FOR_DATA          = 2

-- other parsing definitions
local CAN_PACKET_LENGTH_MAX = 8         -- CAN packet maximum length
local SERIAL_PACKET_LENGTH_MAX = 32     -- serial packet maximum length.  used to sanity check length of incoming messages
local SERIAL_PACKET_LENGTH_MIN = 16     -- serial packet minimum length.  used to sanity check sends

-- local variables and definitions
local driver                            -- CAN bus
local initialised = false               -- true once connection to gimbal has been initialised
local parse_state = PARSE_STATE_WAITING_FOR_HEADER  -- parse state
local parse_length = 0                  -- incoming message's packet length
local parse_buff = {}                   -- message buffer holding roll, pitch and yaw angles from gimbal
local parse_bytes_recv = 0              -- message buffer length.  count of the number of bytes received in the message so far
local last_send_seq = 0                 -- last sequence number sent
local last_req_attitude_ms = 0          -- system time of last request for attitude
local last_set_attitude_ms = 0          -- system time of last set attitude call
local REPLY_TYPE = {NONE=0, ATTITUDE=1, POSITION_CONTROL=2, SPEED_CONTROL=3} -- enum of expected reply types
local expected_reply = REPLY_TYPE.NONE  -- currently expected reply type
local expected_reply_ms = 0             -- system time that reply is first expected.  used for timeouts

-- 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)
local write_fails = 0                   -- number of times write failed
local execute_fails = 0                 -- number of times that gimbal was unable to execute the command
local reply_timeouts = 0                -- number of timeouts waiting for replies

local crc16_lookup = {
    0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241,
    0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440,
    0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40,
    0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841,
    0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40,
    0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41,
    0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641,
    0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040,
    0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240,
    0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441,
    0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41,
    0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840,
    0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41,
    0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40,
    0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640,
    0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041,
    0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240,
    0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441,
    0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41,
    0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840,
    0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41,
    0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40,
    0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640,
    0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041,
    0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241,
    0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440,
    0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40,
    0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841,
    0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40,
    0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41,
    0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641,
    0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040
}

local crc32_lookup = {
    0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
    0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
    0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
    0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
    0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
    0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
    0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
    0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
    0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
    0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
    0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
    0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
    0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
    0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
    0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
    0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
    0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
    0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
    0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
    0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
    0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
    0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
    0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
    0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
    0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
    0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
    0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
    0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
    0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
    0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
    0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
    0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
}

-- calculate crc16 for a series of bytes
-- byte_array should be a table of uint8 values
-- start_byte should be the first byte to start from (using 1 indexing) or left as nil to default to the first byte
-- num_bytes should be the number of bytes to process or left as nil to use the entire message
function calc_crc16(byte_array, start_byte, num_bytes)
  start_byte = start_byte or 1
  num_bytes = num_bytes or #byte_array - start_byte + 1
  local crc = 0x3AA3
  for i = start_byte, num_bytes do
    local b = byte_array[i] & 0xFF
    local crc16_lookup_index = ((crc ~ b) % 256) + 1
    local lookup_val = crc16_lookup[crc16_lookup_index]
    crc = ((crc >> 8) & 0xFF) ~ lookup_val
  end
  return crc
end

-- calculate crc32 for a series of bytes
-- byte_array should be a table of uint8 values
-- start_byte should be the first byte to start from (using 1 indexing) or left as nil to default to the first byte
-- num_bytes should be the number of bytes to process or left as nil to use the entire message
function calc_crc32(byte_array, start_byte, num_bytes)
  start_byte = start_byte or 1
  num_bytes = num_bytes or #byte_array - start_byte + 1
  local crc = 0x3AA3
  for i = start_byte, num_bytes do
    local b = byte_array[i] & 0xFF
    local crc32_lookup_index = (((crc ~ b) & 0xff) + 1)
    local lookup_val = crc32_lookup[crc32_lookup_index]
    crc = ((crc >> 8) & 0x00FFFFFF) ~ lookup_val
  end
  return crc
end

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

-- get uint16 from two bytes
function uint16_value(hbyte, lbyte)
  return ((hbyte & 0xFF) << 8) | (lbyte & 0xFF)
end

-- get uint32 from four bytes
function uint32_value(byte3, byte2, byte1, byte0)
  return (((byte3 & 0xFF) << 24) | ((byte2 & 0xFF) << 16) | ((byte1 & 0xFF) << 8) | (byte0 & 0xFF))
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

-- perform any require initialisation
function init()
  -- check parameter settings
  if CAN_D1_PROTOCOL:get() ~= 10 and CAN_D2_PROTOCOL:get() ~= 10 then
    gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_D1_PROTOCOL or CAN_D2_PROTOCOL=10")
    do return end
  end
  if CAN_D1_PROTOCOL:get() == 10 and CAN_D2_PROTOCOL:get() == 10 then
    gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_D1_PROTOCOL or CAN_D2_PROTOCOL=0")
    do return end
  end
  if CAN_D1_PROTOCOL:get() == 10 then
    if CAN_P1_DRIVER:get() <= 0 then
      gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_P1_DRIVER=1")
      do return end
    end
    if CAN_P1_BITRATE:get() ~= 1000000 then
      gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_P1_BITRATE=1000000")
      do return end
    end
  end
  if CAN_D2_PROTOCOL:get() == 10 then
    if CAN_P2_DRIVER:get() <= 0 then
      gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_P2_DRIVER=2")
      do return end
    end
    if CAN_P2_BITRATE:get() ~= 1000000 then
      gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_P2_BITRATE=1000000")
      do return end
    end
  end
  if MNT1_TYPE:get() ~= 9 then
    gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set MNT1_TYPE=9")   
    do return end
  end

  -- get CAN device
  driver = CAN:get_device(25)
  if driver then
    initialised = true
    gcs:send_text(MAV_SEVERITY.INFO, "DJIR: mount driver started")   
  else
    gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: failed to connect to CAN bus")   
  end
end

-- send serial message over CAN bus
-- returns true on success, false on failure
function send_msg(serial_msg)

  if not serial_msg then
    gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: cannot send invalid message")
    do return false end
  end

  -- calculate number of CAN frames required to send mesage
  local num_frames = math.floor(#serial_msg / CAN_PACKET_LENGTH_MAX)
  if #serial_msg % CAN_PACKET_LENGTH_MAX > 0 then
    num_frames = num_frames + 1
  end

  -- create and send CAN messages
  for i=0, num_frames-1 do
    local start_byte = i * CAN_PACKET_LENGTH_MAX + 1
    local finish_byte = math.min(start_byte + CAN_PACKET_LENGTH_MAX - 1 , #serial_msg)
    local num_bytes = finish_byte - start_byte + 1

    local canframe = CANFrame()
    canframe:id(SEND_FRAMEID)
    canframe:dlc(num_bytes)
    for j = 0, num_bytes-1 do
      canframe:data(j, serial_msg[start_byte+j])
    end
    if driver:write_frame(canframe, 10000) then
      bytes_written = bytes_written + num_bytes
    else
      write_fails = write_fails + 1
      -- on failure do not send rest of message
      do return false end
    end
  end

  return true
end

-- get next sequence number that should be used for send commands
function get_next_sequence_number()
  last_send_seq = last_send_seq + 1
  if last_send_seq > 0xFFFF then
    last_send_seq = 0
  end
  return last_send_seq
end

-- update serial message's sequence number, crc-16 and crc-32 fields
function update_msg_seq_and_crc(serial_msg)
  -- sanity checks
  if not serial_msg then
    gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: update_msg_seq_and_crc null arg")
    do return end
  end
  if #serial_msg < SERIAL_PACKET_LENGTH_MIN then
    gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: update_msg_seq_and_crc message too short")
    do return end
  end

  -- update sequence
  local seq = get_next_sequence_number()
  serial_msg[9] = lowbyte(seq)
  serial_msg[10] = highbyte(seq)

  -- update header crc16
  local crc16 = calc_crc16(serial_msg, 1, 10)
  serial_msg[11] = lowbyte(crc16)
  serial_msg[12] = highbyte(crc16)

  -- update entire frame's crc32
  local crc32 = calc_crc32(serial_msg, 1, #serial_msg-4)
  serial_msg[#serial_msg-3] = lowbyte(crc32)
  serial_msg[#serial_msg-2] = lowbyte(crc32 >> 8)
  serial_msg[#serial_msg-1] = lowbyte(crc32 >> 16)
  serial_msg[#serial_msg] = lowbyte(crc32 >> 24)
end

-- request attitude from gimbal
function request_attitude()
  -- Field number                  1     2     3     4     5     6     7     8     9    10    11    12    13    14    15    16    17    18    19
  --                             SOF  LenL  LenH CmdTyp  Enc   RES   RES   RES  SeqL  SeqH  CrcL  CrcH CmdSet CmdId Data1 CRC32 CRC32 CRC32 CRC32
  local request_attitude_msg = {0xAA, 0x13, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x0E, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00}

  -- update_msg_seq_and_crc
  update_msg_seq_and_crc(request_attitude_msg)

  -- send bytes
  if send_msg(request_attitude_msg) then
    expected_reply = REPLY_TYPE.ATTITUDE
    expected_reply_ms = millis()
  else
    expected_reply = REPLY_TYPE.NONE
  end
end

-- send target angles (in degrees) to gimbal
-- yaw_angle_deg is always a body-frame angle
function send_target_angles(roll_angle_deg, pitch_angle_deg, yaw_angle_deg, time_sec)
  -- default argument values
  roll_angle_deg = roll_angle_deg or 0
  pitch_angle_deg = pitch_angle_deg or 0
  yaw_angle_deg = yaw_angle_deg or 0
  time_sec = time_sec or 2

  -- if upsidedown, add 180deg to yaw
  if DJIR_UPSIDEDOWN:get() > 0 then
    yaw_angle_deg = wrap_180(yaw_angle_deg + 180)
  end

  -- ensure angles are integers
  roll_angle_deg = -math.floor(roll_angle_deg + 0.5)
  pitch_angle_deg = math.floor(pitch_angle_deg + 0.5)
  yaw_angle_deg = math.floor(yaw_angle_deg + 0.5)
  time_sec = math.floor(time_sec + 0.5)

  --    0x0E, 0x00: Handheld Gimbal Position Control
  --      Command frame bytes
  --       0~1: yaw angle * 10, int16, -1800 to +1800
  --       2~3: roll angle * 10, int16, -300 to +300
  --       4~5: pitch angle * 10, int16, -560 to +1460
  --       6: ctrl_byte, uint8
  --             bit0 = 0:relative control, 1:absolute control
  --             bit1 = 0:yaw axis valid, 1:invalid
  --             bit2 = 0:roll axis valid, 1:invalid
  --             bit3 = 0:pitch axis valid, 1:invalid
  --             bit4~7 = reserved, must be zero
  --       7: time for action, uint8_t, unit: 0.1s.  e.g. if 20, gimbal will rotate to the position desired within 2sec
  --
  -- Field number                1     2     3     4     5     6     7     8     9    10    11    12    13    14    15    16    17    18    19    20    21    22    23    24    25    26
  -- Field name                SOF  LenL  LenH CmdTyp  Enc   RES   RES   RES  SeqL  SeqH  CrcL  CrcH CmdSet CmdId YawL  YawH  RollL RollH PitL  PitH  Ctrl  Time CRC32 CRC32 CRC32 CRC32
  local set_target_att_msg = {0xAA, 0x1A, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x20, 0x00, 0x30, 0x00, 0x40, 0x00, 0x01, 0x14, 0x00, 0x00, 0x00, 0x00}

  -- set angles
  set_target_att_msg[15] = lowbyte(yaw_angle_deg * 10)
  set_target_att_msg[16] = highbyte(yaw_angle_deg * 10)
  set_target_att_msg[17] = lowbyte(roll_angle_deg * 10)
  set_target_att_msg[18] = highbyte(roll_angle_deg * 10)
  set_target_att_msg[19] = lowbyte(pitch_angle_deg * 10)
  set_target_att_msg[20] = highbyte(pitch_angle_deg * 10)

  -- set time
  set_target_att_msg[22] = lowbyte(time_sec * 10)

  -- update_msg_seq_and_crc
  update_msg_seq_and_crc(set_target_att_msg)

  -- send bytes
  if send_msg(set_target_att_msg) then
    expected_reply = REPLY_TYPE.POSITION_CONTROL
    expected_reply_ms = millis()
  else
    expected_reply = REPLY_TYPE.NONE
  end
end

-- send target rates (in deg/sec) to gimbal
function send_target_rates(roll_rate_degs, pitch_rate_degs, yaw_rate_degs)
  -- default argument values
  roll_rate_degs = roll_rate_degs or 0
  pitch_rate_degs = pitch_rate_degs or 0
  yaw_rate_degs = yaw_rate_degs or 0
  time_sec = time_sec or 2

  -- ensure rates are integers. invert roll direction
  roll_rate_degs = -math.floor(roll_rate_degs + 0.5)
  pitch_rate_degs = math.floor(pitch_rate_degs + 0.5)
  yaw_rate_degs = math.floor(yaw_rate_degs + 0.5)

  --    0x0E, 0x01: Handheld Gimbal Speed Control
  --      Command frame bytes
  --       0~1: yaw speed * 10, int16, -3600 to +3600
  --       2~3: roll speed * 10, int16, -3600 to +3600
  --       4~5: pitch speed * 10, int16, -3600 to +3600
  --       6: ctrl_byte, uint8, always use 0x88
  --             bit0~2 = reserved, must be zero
  --             bit3 = 0:consider focal length, 1:do not consider focal length
  --             bit4~6 = reserved, must be zero
  --             bit7 = 0:release speed control, 1:take over speed control
  --
  -- Field number                  1     2     3     4     5     6     7     8     9    10    11    12    13    14    15    16    17    18    19    20    21    22    23    24    25
  -- Field name                  SOF  LenL  LenH CmdTyp  Enc   RES   RES   RES  SeqL  SeqH  CrcL  CrcH CmdSet CmdId YawL  YawH  RollL RollH PitL  PitH  Ctrl CRC32 CRC32 CRC32 CRC32
  local set_target_speed_msg = {0xAA, 0x19, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x01, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x88, 0x00, 0x00, 0x00, 0x00}

  -- set rates
  set_target_speed_msg[15] = lowbyte(yaw_rate_degs * 10)
  set_target_speed_msg[16] = highbyte(yaw_rate_degs * 10)
  set_target_speed_msg[17] = lowbyte(roll_rate_degs * 10)
  set_target_speed_msg[18] = highbyte(roll_rate_degs * 10)
  set_target_speed_msg[19] = lowbyte(pitch_rate_degs * 10)
  set_target_speed_msg[20] = highbyte(pitch_rate_degs * 10)

  -- update_msg_seq_and_crc
  update_msg_seq_and_crc(set_target_speed_msg)

  -- send bytes
  if send_msg(set_target_speed_msg) then
    expected_reply = REPLY_TYPE.SPEED_CONTROL
    expected_reply_ms = millis()
  else
    expected_reply = REPLY_TYPE.NONE
  end
end

-- consume incoming CAN packets
function read_incoming_packets()
  local canframe
  repeat
    canframe = driver:read_frame()
    if canframe then
      if uint32_t(canframe:id()) == uint32_t(RECEIVE_FRAMEID) then
        for i = 0, canframe:dlc()-1 do
          parse_byte(canframe:data(i))
        end
      else
        msg_ignored = msg_ignored + 1
      end
    end
  until not canframe
end

-- parse an byte from the gimbal
function parse_byte(b)

    -- update num bytes read (for reporting only)
    bytes_read = bytes_read + 1

    -- clear buffer while waiting for header
    if parse_state == PARSE_STATE_WAITING_FOR_HEADER then
      parse_expected_crc = 0
      parse_bytes_recv = 0
    end

    -- add byte to buffer
    parse_bytes_recv = parse_bytes_recv + 1
    parse_buff[parse_bytes_recv] = b

    -- waiting for header
    if parse_state == PARSE_STATE_WAITING_FOR_HEADER then
      if b == HEADER then
        parse_state = PARSE_STATE_WAITING_FOR_VERLENGTH
        do return end
      else
        -- unexpected byte
        bytes_error = bytes_error + 1
      end
    end

    -- waiting for version/length LSB
    if parse_state == PARSE_STATE_WAITING_FOR_VERLENGTH then
      if parse_bytes_recv == 2 then
        parse_length = b
      else
        parse_length = uint16_value(b & 0x03, parse_length)
        if (parse_length < SERIAL_PACKET_LENGTH_MIN) or (parse_length > SERIAL_PACKET_LENGTH_MAX) then
          -- invalid length
          parse_state = PARSE_STATE_WAITING_FOR_HEADER
          bytes_error = bytes_error + 1
        else
          parse_state = PARSE_STATE_WAITING_FOR_DATA
        end
      end
      do return end
    end

    -- waiting for data
    if (parse_state == PARSE_STATE_WAITING_FOR_DATA) and (parse_bytes_recv >= parse_length) then
        -- check crc16
        local expected_crc16 = calc_crc16(parse_buff, 1, 10)
        local received_crc16 = uint16_value(parse_buff[12], parse_buff[11])
        if (expected_crc16 ~= received_crc16) then
          if DJIR_DEBUG:get() > 0 then
            gcs:send_text(MAV_SEVERITY.INFO, string.format("DJIR: crc16 exp:%x got:%x", expected_crc16, received_crc16))
          end
          bytes_error = bytes_error + 1
          parse_state = PARSE_STATE_WAITING_FOR_HEADER
          do return end
        end

        -- check crc32
        local expected_crc32 = calc_crc32(parse_buff, 1, parse_length-4)
        local received_crc32 = uint32_value(parse_buff[parse_length], parse_buff[parse_length-1], parse_buff[parse_length-2], parse_buff[parse_length-3])
        if (expected_crc32 ~= received_crc32) then
          if DJIR_DEBUG:get() > 0 then
            gcs:send_text(MAV_SEVERITY.INFO, string.format("DJIR: crc32 exp:%x got:%x", expected_crc32, received_crc32))
          end
          bytes_error = bytes_error + 1
          parse_state = PARSE_STATE_WAITING_FOR_HEADER
          do return end
        end

        -- check if reply
        local cmd_type_reply = (parse_buff[4] & 0x20) > 0

        -- process reply messages
        if cmd_type_reply then

          if expected_reply == REPLY_TYPE.NONE then
            gcs:send_text(MAV_SEVERITY.INFO, string.format("DJIR: unexpected reply len:%d", parse_length))
          end

          -- parse attitude reply message
          if (expected_reply == REPLY_TYPE.ATTITUDE) and (parse_length >= 20) then
            local ret_code = parse_buff[13]
            if ret_code == RETURN_CODE.SUCCESS then
              local yaw_deg = int16_value(parse_buff[16],parse_buff[15]) * 0.1
              local roll_deg = -int16_value(parse_buff[18],parse_buff[17]) * 0.1
              local pitch_deg = int16_value(parse_buff[20],parse_buff[19]) * 0.1
              mount:set_attitude_euler(MOUNT_INSTANCE, roll_deg, pitch_deg, yaw_deg)
              if DJIR_DEBUG:get() > 1 then
                gcs:send_text(MAV_SEVERITY.INFO, string.format("DJIR: roll:%4.1f pitch:%4.1f yaw:%4.1f", roll_deg, pitch_deg, yaw_deg))
              end
            else
              execute_fails = execute_fails + 1
            end
          end

          -- parse position control reply message
          if (expected_reply == REPLY_TYPE.POSITION_CONTROL) and (parse_length >= 13) then
            local ret_code = parse_buff[13]
            if ret_code ~= RETURN_CODE.SUCCESS then
              execute_fails = execute_fails + 1
            end
          end

          -- parse speed control reply message
          if (expected_reply == REPLY_TYPE.SPEED_CONTROL) and (parse_length >= 13) then
            local ret_code = parse_buff[13]
            if ret_code ~= RETURN_CODE.SUCCESS then
              execute_fails = execute_fails + 1
            end
          end

          -- clear expected reply flag
          expected_reply = REPLY_TYPE.NONE
        else
          -- not attempting to parse
          gcs:send_text(MAV_SEVERITY.INFO, "DJIR: skipped reply:" .. tostring(cmd_type_reply) .. "len:" .. tostring(parse_length))
        end

       parse_state = PARSE_STATE_WAITING_FOR_HEADER
       do return end
    end

end

-- the main update function that performs a simplified version of RTL
function update()

  -- initialise connection to gimbal
  if not initialised then
    init()
    return update, INIT_INTERVAL_MS
  end

  -- consume incoming bytes
  read_incoming_packets()

  -- get system time
  local now_ms = millis()

  -- report parsing status
  if (DJIR_DEBUG:get() > 0) and ((now_ms - last_print_ms) > 5000) then
    last_print_ms = now_ms
    gcs:send_text(MAV_SEVERITY.INFO, string.format("DJIR: r:%u w:%u fail:%u,%u perr:%u to:%u ign:%u", bytes_read, bytes_written, write_fails, execute_fails, bytes_error, reply_timeouts, msg_ignored))
  end

  -- do not send any messages until CAN traffic has been seen
  if msg_ignored == 0 and bytes_read == 0 then
    return update, UPDATE_INTERVAL_MS
  end

  -- handle expected reply timeouts
  if (expected_reply ~= REPLY_TYPE.NONE) then
    if ((now_ms - expected_reply_ms) > REPLY_TIMEOUT_MS) then
      if DJIR_DEBUG:get() > 0 then
        gcs:send_text(MAV_SEVERITY.INFO, string.format("DJIR: timeout expecting %d", expected_reply))
      end
      expected_reply = REPLY_TYPE.NONE
      reply_timeouts = reply_timeouts + 1
    else
      -- do not process any messages
      return update, UPDATE_INTERVAL_MS
    end
  end

  -- request gimbal attitude
  if now_ms - last_req_attitude_ms > REQUEST_ATTITUDE_INTERVAL_MS then
    last_req_attitude_ms = now_ms
    request_attitude()
    return update, UPDATE_INTERVAL_MS
  end

  -- set gimbal attitude or rate
  if now_ms - last_set_attitude_ms > SET_ATTITUDE_INTERVAL_MS then
    last_set_attitude_ms = now_ms

    -- send angle target
    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
        -- convert to body-frame
        yaw_deg = wrap_180(yaw_deg - math.deg(ahrs:get_yaw()))
      end
      send_target_angles(roll_deg, pitch_deg, yaw_deg, 1)
      return update, UPDATE_INTERVAL_MS
    end

    -- send rate target
    local roll_degs, pitch_degs, yaw_degs, yaw_is_ef = mount:get_rate_target(MOUNT_INSTANCE)
    if roll_degs and pitch_degs and yaw_degs then
      send_target_rates(roll_degs, pitch_degs, yaw_degs)
      return update, UPDATE_INTERVAL_MS
    end
    return update, UPDATE_INTERVAL_MS
  end

  return update, UPDATE_INTERVAL_MS
end

return update()