From a16742f2eb740adef925cc0d1c90faceaceb97b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Mar 2014 14:23:49 +0100 Subject: [PATCH] Fixes to altitude units --- .../fw_att_pos_estimator_main.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index d598052222..082c695dc9 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -701,23 +701,24 @@ FixedwingEstimator::task_main() velNED[0] = _gps.vel_n_m_s; velNED[1] = _gps.vel_e_m_s; velNED[2] = _gps.vel_d_m_s; + + double lat = _gps.lat * 1e-7; + double lon = _gps.lon * 1e-7; + float alt = _gps.alt * 1e-3; + InitialiseFilter(velNED); // Initialize projection _local_pos.ref_lat = _gps.lat; _local_pos.ref_lon = _gps.lon; - _local_pos.ref_alt = _gps.alt; + _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; // Store _baro_ref = baroHgt; - _baro_gps_offset = baroHgt - _gps.alt; + _baro_gps_offset = baroHgt - alt; // XXX this is not multithreading safe - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; - map_projection_init(lat, lon); mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);