aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Scripting: mount-djir2 handles both legacy and latest ver

Browse Source
This commit is contained in:
Randy Mackay 2023-12-04 11:46:13 +09:00 committed by Peter Barker
parent c6c41cb609
commit b507075701
1 changed files with 33 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Scripting/drivers/mount-djirs2-driver.lua
View File

@ -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
@ -653,12 +656,23 @@ function parse_byte(b)
end
-- parse attitude reply message
if (expected_reply == REPLY_TYPE.ATTITUDE) and (parse_length >= 22) then
local ret_code = parse_buff[15]
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[18],parse_buff[17]) * 0.1
local pitch_deg = int16_value(parse_buff[20],parse_buff[19]) * 0.1
local roll_deg = int16_value(parse_buff[22],parse_buff[21]) * 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
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))
@ -669,16 +683,26 @@ function parse_byte(b)
end
-- parse position control reply message
if (expected_reply == REPLY_TYPE.POSITION_CONTROL) and (parse_length >= 15) then
local ret_code = parse_buff[15]
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 >= 15) then
local ret_code = parse_buff[15]
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