diff --git a/build/patches/improve_support_gimbal_device_attitude_status.patch b/build/patches/improve_support_gimbal_device_attitude_status.patch new file mode 100644 index 0000000000..d513f845c0 --- /dev/null +++ b/build/patches/improve_support_gimbal_device_attitude_status.patch @@ -0,0 +1,48 @@ +diff --git a/message_definitions/v1.0/common.xml b/message_definitions/v1.0/common.xml +index 1ccad646..509cfc37 100644 +--- a/message_definitions/v1.0/common.xml ++++ b/message_definitions/v1.0/common.xml +@@ -6478,18 +6478,34 @@ + Z component of angular velocity, positive is panning to the right, NaN to be ignored. + + +- +- +- Message reporting the status of a gimbal device. This message should be broadcasted by a gimbal device component. The angles encoded in the quaternion are in the global frame (roll: positive is tilt to the right, pitch: positive is tilting up, yaw is turn to the right). This message should be broadcast at a low regular rate (e.g. 10Hz). ++ Message reporting the status of a gimbal device. ++ This message should be broadcast by a gimbal device component at a low regular rate (e.g. 5 Hz). ++ For the angles encoded in the quaternion and the angular velocities holds: ++ If the flag GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME is set, then they are relative to the vehicle heading (vehicle frame). ++ If the flag GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME is set, then they are relative to absolute North (earth frame). ++ If neither of these flags are set, then (for backwards compatibility) it holds: ++ If the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, then they are relative to absolute North (earth frame), ++ else they are relative to the vehicle heading (vehicle frame). ++ Other conditions of the flags are not allowed. ++ The quaternion and angular velocities in the other frame can be calculated from delta_yaw and delta_yaw_velocity as ++ q_earth = q_delta_yaw * q_vehicle and w_earth = w_delta_yaw_velocity + w_vehicle (if not NaN). ++ If neither the GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME nor the GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME flag is set, ++ then (for backwards compatibility) the data in the delta_yaw and delta_yaw_velocity fields are to be ignored. ++ New implementations should always set either GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME or GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME, ++ and always should set delta_yaw and delta_yaw_velocity either to the proper value or NaN. + System ID + Component ID + Timestamp (time since system boot). +- Current gimbal flags set. +- Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) +- X component of angular velocity (NaN if unknown) +- Y component of angular velocity (NaN if unknown) +- Z component of angular velocity (NaN if unknown) +- Failure flags (0 for no failure) ++ Current gimbal flags set. ++ Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. ++ X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown. ++ Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown. ++ Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown. ++ Failure flags (0 for no failure) ++ ++ Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown. ++ Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown. ++ This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. + + +