diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-02 17:08:24 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-02 17:08:24 +0100 |
| commit | 17ac30f1b195b5095100abb5d595c781060f218d (patch) | |
| tree | 2d347945ba74832ccc98442eaa6698c347b6b2ab /src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
| parent | f0740df53ae0f1c79f2f1298611a7a8c7e0ae8e7 (diff) | |
| download | px4-firmware-17ac30f1b195b5095100abb5d595c781060f218d.tar.gz px4-firmware-17ac30f1b195b5095100abb5d595c781060f218d.tar.bz2 px4-firmware-17ac30f1b195b5095100abb5d595c781060f218d.zip | |
More fixes, but things are pretty NaN still
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 | 11 |
1 files changed, 8 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 adf679d7e..b238422b6 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 @@ -407,8 +407,8 @@ FixedwingEstimator::task_main() float IMUmsec = _gyro.timestamp / 1e3; - float deltaT = (_gyro.timestamp - last_run) / 1000000.0f; - last_run = _gyro.timestamp / 1e6 ; + float deltaT = (_gyro.timestamp - last_run) / 1e6f; + last_run = _gyro.timestamp; /* guard against too large deltaT's */ if (deltaT > 1.0f) @@ -659,7 +659,12 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (estimator::g_estimator) { - errx(0, "running"); + warnx("running"); + warnx("attitude: %8.4f, %8.4f, %8.4f", eulerEst[0], eulerEst[1], eulerEst[2]); + warnx("states [1-3]: %8.4f, %8.4f, %8.4f", states[0], states[1], states[2]); + warnx("states [4-6]: %8.4f, %8.4f, %8.4f", states[3], states[4], states[5]); + warnx("states [7-9]: %8.4f, %8.4f, %8.4f", states[6], states[7], states[8]); + exit(0); } else { errx(1, "not running"); |