diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-21 08:06:06 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-21 08:06:06 +0100 |
| commit | ab088c8ac65bfb220ac5ed0853b243026e77600a (patch) | |
| tree | 8c493df10c3d0dda0a460790e02af3bdab5b03c9 /src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
| parent | bce67c6b036cddbe1542f910c6e605256283768f (diff) | |
| download | px4-firmware-ab088c8ac65bfb220ac5ed0853b243026e77600a.tar.gz px4-firmware-ab088c8ac65bfb220ac5ed0853b243026e77600a.tar.bz2 px4-firmware-ab088c8ac65bfb220ac5ed0853b243026e77600a.zip | |
Altitute wip
Diffstat (limited to 'src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp')
| -rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 |
1 files changed, 4 insertions, 3 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 8b41e7479..e46dffde3 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 @@ -735,7 +735,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref = _baro.altitude; baroHgt = _baro.altitude - _baro_ref; - _baro_gps_offset = baroHgt - alt; + _baro_gps_offset = _baro_ref - _local_pos.ref_alt; // XXX this is not multithreading safe map_projection_init(lat, lon); @@ -984,10 +984,11 @@ FixedwingEstimator::task_main() _global_pos.vel_e = 0.0f; } - _global_pos.alt = _local_pos.ref_alt - _local_pos.z; + /* 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 = _baro_ref - _baro_gps_offset - _local_pos.z; + _global_pos.baro_alt = _local_pos.ref_alt - _baro_gps_offset - _local_pos.z; } if (_local_pos.v_z_valid) { |