diff options
Diffstat (limited to 'src/modules/fw_att_pos_estimator')
| -rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 13 |
1 files 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 d59805222..082c695dc 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); |