diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-05 01:57:01 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-05 01:57:01 +0100 |
commit | 8bd532c8556d7f99d1d66c161f182aeebd1ab325 (patch) | |
tree | 7b0d1df6ac942edf799574cab5547beb953af195 | |
parent | d8f04556beca666d1f932a289f2437cbe6b53958 (diff) | |
download | px4-firmware-8bd532c8556d7f99d1d66c161f182aeebd1ab325.tar.gz px4-firmware-8bd532c8556d7f99d1d66c161f182aeebd1ab325.tar.bz2 px4-firmware-8bd532c8556d7f99d1d66c161f182aeebd1ab325.zip |
Change slightly the way we handle timestamps, apply same unknown scaling as on logfiles
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 22 |
1 files changed, 12 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 ef51c7524..44c25c407 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 @@ -84,10 +84,11 @@ extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); static uint64_t last_run = 0; +static uint32_t IMUmsec = 0; uint32_t millis() { - return last_run / 1e3; + return IMUmsec; } class FixedwingEstimator @@ -427,7 +428,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } - float IMUmsec = _gyro.timestamp / 1e3; + IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; last_run = _gyro.timestamp; @@ -501,8 +502,6 @@ FixedwingEstimator::task_main() gpsLon = math::radians(_gps.lon / (double)1e7); gpsHgt = _gps.alt / 1e3f; - newDataGps = true; - if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { InitialiseFilter(); _initialized = true; @@ -540,12 +539,13 @@ FixedwingEstimator::task_main() fuseVelData = false; fusePosData = false; fuseHgtData = false; - - newDataGps = false; } } else { - newDataGps = false; + /* do not fuse anything, we got no position / vel update */ + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; } bool mag_updated; @@ -555,13 +555,15 @@ FixedwingEstimator::task_main() perf_count(_perf_mag); // XXX we compensate the offsets upfront - should be close to zero. - magData.x = _mag.x; + // XXX the purpose of the 0.001 factor is unclear + // 0.001f + magData.x = 0.001f * _mag.x; magBias.x = 0.0f; // _mag_offsets.x_offset - magData.y = _mag.y; + magData.y = 0.001f * _mag.y; magBias.y = 0.0f; // _mag_offsets.y_offset - magData.z = _mag.z; + magData.z = 0.001f * _mag.z; magBias.z = 0.0f; // _mag_offsets.y_offset if (_initialized) { |