diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-21 13:44:34 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-21 13:44:34 +0100 |
| commit | 57df53d27c5f910cbff0d2fb803df0ef30ce5fc7 (patch) | |
| tree | a82cc8c0e74607916c71fdf4274564c87a6447f4 /src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
| parent | c5311b18fef0e215c019a4686ca9697224d1cd31 (diff) | |
| download | px4-firmware-57df53d27c5f910cbff0d2fb803df0ef30ce5fc7.tar.gz px4-firmware-57df53d27c5f910cbff0d2fb803df0ef30ce5fc7.tar.bz2 px4-firmware-57df53d27c5f910cbff0d2fb803df0ef30ce5fc7.zip | |
GPS reinit completed, put in NaN catcher
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 | 20 |
1 files changed, 17 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 688a44c47..95549ad21 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 @@ -694,13 +694,20 @@ FixedwingEstimator::task_main() velNED[1] = _gps.vel_e_m_s; velNED[2] = _gps.vel_d_m_s; InitialiseFilter(velNED); + warnx("GPS REINIT"); _gps_initialized = true; } else if (!statesInitialised) { - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + posNED[0] = 0.0f; + posNED[1] = 0.0f; + posNED[2] = 0.0f; + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; InitialiseFilter(velNED); } } @@ -970,7 +977,14 @@ FixedwingEstimator::task_main() /* advertise and publish */ _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } + } + + if (!isfinite(states[0])) { + print_status(); + _exit(0); + } + if (_gps_initialized) { _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = states[7]; _local_pos.y = states[8]; |