Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Camera: camera-status-fov attitude in earth frame

This commit is contained in:
Randy Mackay 2024-12-06 21:32:20 +09:00
parent a9f561ac78
commit dd37065bb5
1 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Camera/AP_Camera_Backend.cpp
View File

@ -314,12 +314,17 @@ void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const
mount->get_attitude_quaternion(get_mount_instance(), quat); mount->get_attitude_quaternion(get_mount_instance(), quat);
} }
// calculate attitude quaternion in earth frame using AHRS yaw
Quaternion quat_ef;
quat_ef.from_euler(0, 0, AP::ahrs().get_yaw());
quat_ef *= quat;
// send camera fov status message only if the last calculated values aren't stale // send camera fov status message only if the last calculated values aren't stale
const float quat_array[4] = { const float quat_array[4] = {
quat.q1, quat_ef.q1,
quat.q2, quat_ef.q2,
quat.q3, quat_ef.q3,
quat.q4 quat_ef.q4
}; };
mavlink_msg_camera_fov_status_send( mavlink_msg_camera_fov_status_send(
chan, chan,