diff --git a/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua b/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua index 013c28b0c7..77e5f95b5a 100644 --- a/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua +++ b/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua @@ -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