Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua

806 lines
34 KiB
Lua
Raw Blame History

-- 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
--]]
---@diagnostic disable: cast-local-type
-- 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 = 100 -- timeout waiting for reply after 0.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}
local ATTITUDE_PACKET_LEN = {LEGACY=24, LATEST=26} -- attitude packet expected length. Legacy must be less than latest
local POSITION_CONTROL_PACKET_LEN = {LEGACY=17, LATEST=19} -- position control packet expected length. Legacy must be less than latest
local SPEED_CONTROL_PACKET_LEN = {LEGACY=17, LATEST=19} -- speed control packet expected length. Legacy must be less than latest
-- 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 >= ATTITUDE_PACKET_LEN.LEGACY) then
-- default to legacy format but also handle latest format
local ret_code_field = 13
local yaw_field = 15
local pitch_field = 17
local roll_field = 19
if (parse_length >= ATTITUDE_PACKET_LEN.LATEST) then
ret_code_field = 15
yaw_field = 17
pitch_field = 21
roll_field = 19
end
local ret_code = parse_buff[ret_code_field]
if ret_code == RETURN_CODE.SUCCESS then
local yaw_deg = int16_value(parse_buff[yaw_field+1],parse_buff[yaw_field]) * 0.1
local pitch_deg = int16_value(parse_buff[pitch_field+1],parse_buff[pitch_field]) * 0.1
local roll_deg = int16_value(parse_buff[roll_field+1],parse_buff[roll_field]) * 0.1
-- if upsidedown, subtract 180deg from yaw to undo addition of target
if DJIR_UPSIDEDOWN:get() > 0 then
yaw_deg = wrap_180(yaw_deg - 180)
end
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 >= POSITION_CONTROL_PACKET_LEN.LEGACY) then
-- default to legacy format but also handle latest format
local ret_code_field = 13
if (parse_length >= POSITION_CONTROL_PACKET_LEN.LATEST) then
ret_code_field = 15
end
local ret_code = parse_buff[ret_code_field]
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 >= SPEED_CONTROL_PACKET_LEN.LEGACY) then
-- default to legacy format but also handle latest format
local ret_code_field = 13
if (parse_length >= SPEED_CONTROL_PACKET_LEN.LATEST) then
ret_code_field = 15
end
local ret_code = parse_buff[ret_code_field]
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()
Reference in New Issue View Git Blame Copy Permalink