From 86b11c75fea2a3cd54274f844506e16e9740bc23 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 6 Dec 2024 20:42:58 +0900 Subject: [PATCH 1/3] AP_Mount: add get_attitude_quaternion method --- libraries/AP_Mount/AP_Mount.cpp | 15 +++++++++++---- libraries/AP_Mount/AP_Mount.h | 5 +++++ 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 8dc31bbfa33de..0b64b1063bb0e 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -655,18 +655,25 @@ bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Locati } #endif -// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame -// returns true on success -bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) +// get attitude as a quaternion. returns true on success. +// att_quat will be an earth-frame quaternion rotated such that +// yaw is in body-frame. +bool AP_Mount::get_attitude_quaternion(uint8_t instance, Quaternion& att_quat) { auto *backend = get_instance(instance); if (backend == nullptr) { return false; } + return backend->get_attitude_quaternion(att_quat); +} +// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame +// returns true on success +bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) +{ // re-use get_attitude_quaternion and convert to Euler angles Quaternion att_quat; - if (!backend->get_attitude_quaternion(att_quat)) { + if (!get_attitude_quaternion(instance, att_quat)) { return false; } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 54762e915307f..d34f581636893 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -206,6 +206,11 @@ class AP_Mount bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const; #endif + // get attitude as a quaternion. returns true on success. + // att_quat will be an earth-frame quaternion rotated such that + // yaw is in body-frame. + bool get_attitude_quaternion(uint8_t instance, Quaternion& att_quat); + // get mount's current attitude in euler angles in degrees. yaw angle is in body-frame // returns true on success bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg); From b7ad6b3cc7b9d1185dcca272be3d54cc00f7dd0f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 6 Dec 2024 20:21:47 +0900 Subject: [PATCH 2/3] AP_Camera: always send camera-fov-status --- libraries/AP_Camera/AP_Camera_Backend.cpp | 29 +++++++++++++++-------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index c632bf71ab9f2..696bd6481a330 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -295,16 +295,25 @@ void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const { // getting corresponding mount instance for camera - const AP_Mount* mount = AP::mount(); + AP_Mount* mount = AP::mount(); if (mount == nullptr) { return; } + + // get latest POI from mount Quaternion quat; - Location loc; + Location camera_loc; Location poi_loc; - if (!mount->get_poi(get_mount_instance(), quat, loc, poi_loc)) { - return; + const bool have_poi_loc = mount->get_poi(get_mount_instance(), quat, camera_loc, poi_loc); + + // if failed to get POI, get camera location directly from AHRS + // and attitude directly from mount + bool have_camera_loc = have_poi_loc; + if (!have_camera_loc) { + have_camera_loc = AP::ahrs().get_location(camera_loc); + mount->get_attitude_quaternion(get_mount_instance(), quat); } + // send camera fov status message only if the last calculated values aren't stale const float quat_array[4] = { quat.q1, @@ -315,12 +324,12 @@ void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const mavlink_msg_camera_fov_status_send( chan, AP_HAL::millis(), - loc.lat, - loc.lng, - loc.alt * 10, - poi_loc.lat, - poi_loc.lng, - poi_loc.alt * 10, + have_camera_loc ? camera_loc.lat : INT32_MAX, + have_camera_loc ? camera_loc.lng : INT32_MAX, + have_camera_loc ? camera_loc.alt * 10 : INT32_MAX, + have_poi_loc ? poi_loc.lat : INT32_MAX, + have_poi_loc ? poi_loc.lng : INT32_MAX, + have_poi_loc ? poi_loc.alt * 10 : INT32_MAX, quat_array, horizontal_fov() > 0 ? horizontal_fov() : NaNf, vertical_fov() > 0 ? vertical_fov() : NaNf From ea8b6f20c0b631caf6b09c922dfe06f510883e55 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 6 Dec 2024 21:32:20 +0900 Subject: [PATCH 3/3] AP_Camera: camera-status-fov attitude in earth frame --- libraries/AP_Camera/AP_Camera_Backend.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index 696bd6481a330..6954c8b9e2cf2 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -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); } + // 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 const float quat_array[4] = { - quat.q1, - quat.q2, - quat.q3, - quat.q4 + quat_ef.q1, + quat_ef.q2, + quat_ef.q3, + quat_ef.q4 }; mavlink_msg_camera_fov_status_send( chan,