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

AP_Camera: always send camera-fov-status

This commit is contained in:
Randy Mackay 2024-12-06 20:21:47 +09:00
parent 5a1a8d1c10
commit a9f561ac78
1 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_Camera/AP_Camera_Backend.cpp
View File

@ -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 void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const
{ {
// getting corresponding mount instance for camera // getting corresponding mount instance for camera
const AP_Mount* mount = AP::mount(); AP_Mount* mount = AP::mount();
if (mount == nullptr) { if (mount == nullptr) {
return; return;
} }
// get latest POI from mount
Quaternion quat; Quaternion quat;
Location loc; Location camera_loc;
Location poi_loc; Location poi_loc;
if (!mount->get_poi(get_mount_instance(), quat, loc, poi_loc)) { const bool have_poi_loc = mount->get_poi(get_mount_instance(), quat, camera_loc, poi_loc);
return;
// 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 // 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.q1,
@ -315,12 +324,12 @@ void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const
mavlink_msg_camera_fov_status_send( mavlink_msg_camera_fov_status_send(
chan, chan,
AP_HAL::millis(), AP_HAL::millis(),
loc.lat, have_camera_loc ? camera_loc.lat : INT32_MAX,
loc.lng, have_camera_loc ? camera_loc.lng : INT32_MAX,
loc.alt * 10, have_camera_loc ? camera_loc.alt * 10 : INT32_MAX,
poi_loc.lat, have_poi_loc ? poi_loc.lat : INT32_MAX,
poi_loc.lng, have_poi_loc ? poi_loc.lng : INT32_MAX,
poi_loc.alt * 10, have_poi_loc ? poi_loc.alt * 10 : INT32_MAX,
quat_array, quat_array,
horizontal_fov() > 0 ? horizontal_fov() : NaNf, horizontal_fov() > 0 ? horizontal_fov() : NaNf,
vertical_fov() > 0 ? vertical_fov() : NaNf vertical_fov() > 0 ? vertical_fov() : NaNf