From 57df53d27c5f910cbff0d2fb803df0ef30ce5fc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 13:44:34 +0100 Subject: [PATCH] GPS reinit completed, put in NaN catcher --- src/modules/fw_att_pos_estimator/estimator.h | 4 ++-- .../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 bc71810183..ff31a84acc 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 688a44c470..95549ad219 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];