diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-27 16:29:43 -0800 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-27 16:29:43 -0800 |
| commit | d64fa6e255450b3b642acf6b8bf29e86ed87753d (patch) | |
| tree | 158164fe0cd69707afbd39745ce60273204b3372 /src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
| parent | 44c5726703160619b2a49f4eb4c3d0b0d07925fa (diff) | |
| download | px4-firmware-d64fa6e255450b3b642acf6b8bf29e86ed87753d.tar.gz px4-firmware-d64fa6e255450b3b642acf6b8bf29e86ed87753d.tar.bz2 px4-firmware-d64fa6e255450b3b642acf6b8bf29e86ed87753d.zip | |
Handling GPS and baro offset in altitude init / estimate
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, 5 insertions, 2 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 c8ef008d9..d59805222 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 @@ -164,7 +164,8 @@ private: struct sensor_combined_s _sensor_combined; #endif - float _baro_ref; /**< barometer reference altitude */ + float _baro_ref; /**< barometer reference altitude */ + float _baro_gps_offset; /**< offset between GPS and baro */ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _perf_gyro; ///<local performance counter for gyro updates @@ -261,6 +262,7 @@ FixedwingEstimator::FixedwingEstimator() : _local_pos_pub(-1), _baro_ref(0.0f), + _baro_gps_offset(0.0f), /* performance counters */ _loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")), @@ -709,6 +711,7 @@ FixedwingEstimator::task_main() // Store _baro_ref = baroHgt; + _baro_gps_offset = baroHgt - _gps.alt; // XXX this is not multithreading safe double lat = _gps.lat * 1e-7; @@ -963,7 +966,7 @@ FixedwingEstimator::task_main() _global_pos.alt = _local_pos.ref_alt - _local_pos.z; if (_local_pos.z_valid) { - _global_pos.baro_alt = _baro_ref - _local_pos.z; + _global_pos.baro_alt = _baro_ref - _baro_gps_offset - _local_pos.z; } if (_local_pos.v_z_valid) { |