From 0384b76524f6e6ced504dadab32f37396890bb86 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 23 Feb 2023 13:43:44 +1100 Subject: [PATCH] mavproxy_gimbal: cope with GIMBAL_DEVICE_ATTITUDE_STATUS --- MAVProxy/modules/mavproxy_gimbal.py | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/MAVProxy/modules/mavproxy_gimbal.py b/MAVProxy/modules/mavproxy_gimbal.py index 1275d19bdc..4304f902fa 100644 --- a/MAVProxy/modules/mavproxy_gimbal.py +++ b/MAVProxy/modules/mavproxy_gimbal.py @@ -14,6 +14,7 @@ from math import radians if mp_util.has_wxpython: from MAVProxy.modules.lib.mp_menu import * +import pymavlink class GimbalModule(mp_module.MPModule): def __init__(self, mpstate): @@ -184,24 +185,31 @@ def mavlink_packet(self, m): # don't draw if no map return - if m.get_type() != 'GIMBAL_REPORT': - return - - needed = ['ATTITUDE', 'GLOBAL_POSITION_INT'] - for n in needed: + for n in frozenset(['ATTITUDE', 'GLOBAL_POSITION_INT']): if not n in self.master.messages: return + gpi = self.master.messages['GLOBAL_POSITION_INT'] + att = self.master.messages['ATTITUDE'] + + rotmat_copter_gimbal = Matrix3() + if m.get_type() == 'GIMBAL_REPORT': + rotmat_copter_gimbal.from_euler312(m.joint_roll, m.joint_el, m.joint_az) + elif m.get_type() == 'GIMBAL_DEVICE_ATTITUDE_STATUS': + r, p, y = pymavlink.quaternion.Quaternion(m.q).euler + # rotate back from earth-frame on roll and pitch to body-frame + r -= att.roll + p -= att.pitch + rotmat_copter_gimbal.from_euler(r, p, y) + else: + return + # clear the camera icon self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('GimbalView')) - gpi = self.master.messages['GLOBAL_POSITION_INT'] - att = self.master.messages['ATTITUDE'] vehicle_dcm = Matrix3() vehicle_dcm.from_euler(att.roll, att.pitch, att.yaw) - rotmat_copter_gimbal = Matrix3() - rotmat_copter_gimbal.from_euler312(m.joint_roll, m.joint_el, m.joint_az) gimbal_dcm = vehicle_dcm * rotmat_copter_gimbal lat = gpi.lat * 1.0e-7