diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 77df416474..4c30804cdc 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -321,6 +321,10 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo void AP_Camera::send_feedback(mavlink_channel_t chan) const { const AP_AHRS &ahrs = AP::ahrs(); + Location current_loc; + if (!ahrs.get_position(current_loc)) { + // completely ignore this failure! AHRS will provide its best guess. + } float altitude, altitude_rel; if (current_loc.relative_alt) { @@ -358,6 +362,13 @@ void AP_Camera::update() _last_location.lng = 0; return; } + + const AP_AHRS &ahrs = AP::ahrs(); + Location current_loc; + if (!ahrs.get_position(current_loc)) { + // completely ignore this failure! AHRS will provide its best guess. + } + if (_last_location.lat == 0 && _last_location.lng == 0) { _last_location = current_loc; return; diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 395d437741..3f187d92f9 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -18,9 +18,8 @@ class AP_Camera { public: - AP_Camera(uint32_t _log_camera_bit, const struct Location &_loc) + AP_Camera(uint32_t _log_camera_bit) : log_camera_bit(_log_camera_bit) - , current_loc(_loc) { AP_Param::setup_object_defaults(this, var_info); _singleton = this; @@ -130,7 +129,6 @@ private: void Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us=0); uint32_t log_camera_bit; - const struct Location ¤t_loc; // update camera trigger - 50Hz void update_trigger(); diff --git a/libraries/AP_Camera/AP_Camera_Logging.cpp b/libraries/AP_Camera/AP_Camera_Logging.cpp index 7b27d9c9a3..a3d32ef1f7 100644 --- a/libraries/AP_Camera/AP_Camera_Logging.cpp +++ b/libraries/AP_Camera/AP_Camera_Logging.cpp @@ -6,6 +6,11 @@ void AP_Camera::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us) { const AP_AHRS &ahrs = AP::ahrs(); + Location current_loc; + if (!ahrs.get_position(current_loc)) { + // completely ignore this failure! AHRS will provide its best guess. + } + int32_t altitude, altitude_rel, altitude_gps; if (current_loc.relative_alt) { altitude = current_loc.alt+ahrs.get_home().alt;