From 43995763db2eb7749b8a69a04746dff70e4ba208 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 11 Apr 2024 15:33:28 +1000 Subject: [PATCH] AP_Camera: stop adjusting logged altitudes manually, use location methods --- libraries/AP_Camera/AP_Camera_Logging.cpp | 29 +++++++++++++---------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera_Logging.cpp b/libraries/AP_Camera/AP_Camera_Logging.cpp index d49a6200c13d9..08da836d21217 100644 --- a/libraries/AP_Camera/AP_Camera_Logging.cpp +++ b/libraries/AP_Camera/AP_Camera_Logging.cpp @@ -28,19 +28,22 @@ void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestam // 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; - altitude_rel = current_loc.alt; - } else { - altitude = current_loc.alt; - altitude_rel = current_loc.alt - ahrs.get_home().alt; + int32_t altitude_cm = 0; + if (!current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, altitude_cm)) { + // ignore this problem... } + int32_t altitude_rel_cm = 0; + if (!current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, altitude_rel_cm)) { + // ignore this problem... + } + + + int32_t altitude_gps_cm = 0; const AP_GPS &gps = AP::gps(); if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { - altitude_gps = gps.location().alt; - } else { - altitude_gps = 0; + if (!gps.location().get_alt_cm(Location::AltFrame::ABSOLUTE, altitude_gps_cm)) { + // ignore this problem... + } } // if timestamp is zero set to current system time @@ -57,9 +60,9 @@ void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestam gps_week : gps.time_week(), latitude : current_loc.lat, longitude : current_loc.lng, - altitude : altitude, - altitude_rel: altitude_rel, - altitude_gps: altitude_gps, + altitude : altitude_cm, + altitude_rel: altitude_rel_cm, + altitude_gps: altitude_gps_cm, roll : (int16_t)ahrs.roll_sensor, pitch : (int16_t)ahrs.pitch_sensor, yaw : (uint16_t)ahrs.yaw_sensor