mirror of https://github.com/ArduPilot/ardupilot
AP_Scripting: DJIR2 gimbal driver
This commit is contained in:
Randy Mackay
parent
0cbdd28f82
commit
7266f6ee7b
|
|
@ -0,0 +1,741 @@
|
|||
-- mount-djirs2-driver.lua: DJIRS2 mount/gimbal driver
|
||||
|
||||
--[[
|
||||
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_", 1), "could not add param table")
|
||||
assert(param:add_param(PARAM_TABLE_KEY, 1, "DEBUG", 0), "could not add DJIR_DEBUG param")
|
||||
|
||||
-- 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
|
||||
local DJIR_DEBUG = Parameter("DJIR_DEBUG") -- debug level. 0:disabled 1:enabled 2:enabled with attitude reporting
|
||||
|
||||
-- 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 -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
|
||||
|
||||
-- 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
|
||||
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 angles
|
||||
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 -- reversed with pitch below?
|
||||
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()
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
# DJI RS2 and RS3-Pro Mount Driver
|
||||
|
||||
DJI RS2 and RS3-Pro gimbal mount driver lua script
|
||||
|
||||
## 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 the mount-djirs2-driver.lua script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot
|
||||
Loading…
Reference in New Issue