diff --git a/libraries/AP_Camera/AP_Camera_Logging.cpp b/libraries/AP_Camera/AP_Camera_Logging.cpp index d49a6200c1..08da836d21 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