From bc80358e9b946480152849bd2960d2ae1ba7552f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 31 Jul 2024 21:25:29 +1000 Subject: [PATCH] AP_Camera: rely on AHRS for position, not GPS our location may not be coming from a GPS --- libraries/AP_Camera/AP_Camera.cpp | 1 - libraries/AP_Camera/AP_Camera_Backend.cpp | 19 ++++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 57302c8bde..b3869c4818 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -6,7 +6,6 @@ #include #include #include -#include #include "AP_Camera_Backend.h" #include "AP_Camera_Servo.h" #include "AP_Camera_Relay.h" diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index fafe4546aa..40731a6385 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -1,9 +1,13 @@ -#include "AP_Camera_Backend.h" +#include "AP_Camera_config.h" #if AP_CAMERA_ENABLED + +#include "AP_Camera_Backend.h" + #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -57,8 +61,10 @@ void AP_Camera_Backend::update() return; } - // check GPS status - if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) { + const AP_AHRS &ahrs = AP::ahrs(); + + Location current_loc; + if (!ahrs.get_location(current_loc)) { return; } @@ -68,15 +74,10 @@ void AP_Camera_Backend::update() } // check vehicle roll angle is less than configured maximum - const AP_AHRS &ahrs = AP::ahrs(); - if ((_frontend.get_roll_max() > 0) && (fabsf(AP::ahrs().roll_sensor * 1e-2f) > _frontend.get_roll_max())) { + if ((_frontend.get_roll_max() > 0) && (fabsf(ahrs.roll_sensor * 1e-2f) > _frontend.get_roll_max())) { return; } - // get current location. ignore failure because AHRS will provide its best guess - Location current_loc; - IGNORE_RETURN(ahrs.get_location(current_loc)); - // initialise last location to current location if (last_location.lat == 0 && last_location.lng == 0) { last_location = current_loc;