fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw_att_pos_estimator: map_projection_XXX usage fixed, vehicle_global_position topic publication fixed

Browse Source
This commit is contained in:
Anton Babushkin 2014-04-02 17:20:37 +04:00
parent 5c53797c17
commit e9f45a82b8
1 changed files with 7 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
View File

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