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 | |
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')
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.h | 4 | ||||
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 20 |
2 files changed, 19 insertions, 5 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index bc7181018..ff31a84ac 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -122,8 +122,8 @@ extern float baroHgt; extern bool statesInitialised; -const float covTimeStepMax = 0.2f; // maximum time allowed between covariance predictions -const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions +const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions +const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions void UpdateStrapdownEquationsNED(); 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]; |