Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_Scripting: mount-djirs2 script handles latest gimbal software #25692

Merged
merged 4 commits into from
Dec 13, 2023
Merged

AP_Scripting: mount-djirs2 script handles latest gimbal software #25692

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
a87cdd7
AP_Scripting: mount-djirs2: fix parsing for latest firmware
tpwrules Jul 31, 2023
223d0a6
AP_Scripting: mount-djir2 handles both legacy and latest ver
rmackay9 Dec 4, 2023
4db987d
AP_Scripting: mount-djirs2 yaw angle reporting fix
tpwrules Dec 4, 2023
2c1d197
AP_Scripting: djirs2 reply timeout reduced to 0.1 sec
rmackay9 Dec 4, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 38 additions & 10 deletions libraries/AP_Scripting/drivers/mount-djirs2-driver.lua
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@
-- 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 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
Expand Down Expand Up @@ -118,6 +118,9 @@ 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
Expand Down Expand Up @@ -653,12 +656,27 @@ function parse_byte(b)
end

-- parse attitude reply message
if (expected_reply == REPLY_TYPE.ATTITUDE) and (parse_length >= 20) then
local ret_code = parse_buff[13]
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
Comment on lines +663 to +664
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would like to see a comment here that this field ordering contradicts the spec but is how an actual gimbal of version XX (whichever version yours is) is observed to behave.

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[16],parse_buff[15]) * 0.1
local pitch_deg = int16_value(parse_buff[18],parse_buff[17]) * 0.1
local roll_deg = int16_value(parse_buff[20],parse_buff[19]) * 0.1
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))
Expand All @@ -669,16 +687,26 @@ function parse_byte(b)
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 (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 >= 13) then
local ret_code = parse_buff[13]
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
Expand Down
7 changes: 7 additions & 0 deletions libraries/AP_Scripting/drivers/mount-djirs2-driver.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,3 +12,10 @@ DJI RS2 and RS3-Pro gimbal mount driver lua script
- 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

## Issues

If the ground station reports "Pre-arm: Mount not healthy", update the
gimbal firmware using the DJI Ronin phone app to version 01.04.00.20 or
later to correct a mismatch in the way data is received from the gimbal.
Completing this update may take more than an hour.