Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Camera: CAMERA_FOV_STATUS always sent and attitude is earth-frame #28818

Merged
merged 3 commits into from
Dec 10, 2024

Conversation

rmackay9
Copy link
Contributor

@rmackay9 rmackay9 commented Dec 6, 2024

This makes two improvements to the CAMERA_FOV_STATUS message sent to the GCS:

  1. the message is always sent even the gimbal is pointing upwards meaning the image location cannot be calculated
  2. the camera's attitude is sent in earth-frame resolving issue AP_Camera: FOV quaternion should be Earth-Frame #28113

This has been lightly tested in SITL. I'm quite confident that the first enhancement is all correct and below are some screen shots
fov-status-before-vs-after

I tested the 2nd change using printf debug and it seemed correct but SITL's gimbal simulator is a simple servo gimbal and reports even its roll and pitch in body-frame which made debugging slightly difficult. So I'd like @robertlong13 to confirm he's happy.

@rmackay9 rmackay9 requested a review from robertlong13 December 6, 2024 12:40
@rmackay9 rmackay9 force-pushed the camera-always-send-fov-status branch from 6001ecc to 3d61d5c Compare December 9, 2024 07:57
@rmackay9 rmackay9 force-pushed the camera-always-send-fov-status branch from 3d61d5c to ea8b6f2 Compare December 9, 2024 09:59
Copy link
Collaborator

@robertlong13 robertlong13 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks good to me. Tested the quaternion output with my gimbal video testbed. I also validated that your math returns identical results to the dirty workaround I posted in my issue ticket:

mount->get_attitude_euler(get_mount_instance(), roll, pitch, yaw);
quat.from_euler(radians(roll), radians(pitch), radians(yaw) + AP::ahrs().get_yaw());

@rmackay9 rmackay9 merged commit dd37065 into ArduPilot:master Dec 10, 2024
99 checks passed
@rmackay9 rmackay9 deleted the camera-always-send-fov-status branch December 10, 2024 11:31
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants