From e9f45a82b8ee48caa7eecd2371e8dedda87ec2c4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 17:20:37 +0400 Subject: [PATCH] fw_att_pos_estimator: map_projection_XXX usage fixed, vehicle_global_position topic publication fixed --- .../fw_att_pos_estimator_main.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 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 c9d75bce49..22d321907e 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 @@ -167,6 +167,8 @@ private: struct sensor_combined_s _sensor_combined; #endif + struct map_projection_reference_s _pos_ref; + float _baro_ref; /**< barometer reference altitude */ float _baro_gps_offset; /**< offset between GPS and baro */ @@ -803,7 +805,7 @@ FixedwingEstimator::task_main() _baro_gps_offset = _baro_ref - _local_pos.ref_alt; // XXX this is not multithreading safe - map_projection_init(lat, lon); + map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); _gps_initialized = true; @@ -1029,18 +1031,14 @@ FixedwingEstimator::task_main() _global_pos.timestamp = _local_pos.timestamp; - _global_pos.baro_valid = true; - _global_pos.global_valid = true; - if (_local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; } - /* set valid values even if position is not valid */ if (_local_pos.v_xy_valid) { _global_pos.vel_n = _local_pos.vx; _global_pos.vel_e = _local_pos.vy; @@ -1052,16 +1050,15 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); - if (_local_pos.z_valid) { - _global_pos.baro_alt = _local_pos.ref_alt - _baro_gps_offset - _local_pos.z; - } - if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; } _global_pos.yaw = _local_pos.yaw; + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; + _global_pos.timestamp = _local_pos.timestamp; /* lazily publish the global position only once available */