dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Altitute wip

This commit is contained in:
Lorenz Meier 2014-03-21 08:06:06 +01:00
parent bce67c6b03
commit ab088c8ac6
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -735,7 +735,7 @@ FixedwingEstimator::task_main()
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref = _baro.altitude; _baro_ref = _baro.altitude;
baroHgt = _baro.altitude - _baro_ref; 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 // XXX this is not multithreading safe
map_projection_init(lat, lon); map_projection_init(lat, lon);
@ -984,10 +984,11 @@ FixedwingEstimator::task_main()
_global_pos.vel_e = 0.0f; _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) { 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) { if (_local_pos.v_z_valid) {