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

AP_Camera: stop adjusting logged altitudes manually, use location methods

This commit is contained in:
Peter Barker 2024-04-11 15:33:28 +10:00 committed by Peter Barker
parent 0f913b5bdf
commit 43995763db
1 changed files with 16 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_Camera/AP_Camera_Logging.cpp
View File

@ -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