diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-22 01:18:30 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-22 01:18:30 +0200 |
commit | 706d08055d2bf63d16c72e86cd60f0e33a20bc4f (patch) | |
tree | a3a4d06faf381a1c7ec55d7264978c5d9818e3f3 /src/modules | |
parent | 0a89364e3c687deb060782bfd9fbccbd582e0e6d (diff) | |
download | px4-firmware-706d08055d2bf63d16c72e86cd60f0e33a20bc4f.tar.gz px4-firmware-706d08055d2bf63d16c72e86cd60f0e33a20bc4f.tar.bz2 px4-firmware-706d08055d2bf63d16c72e86cd60f0e33a20bc4f.zip |
Better / cleaner initialization of the attitude estimator
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 24 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.h | 2 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 34 |
3 files changed, 38 insertions, 22 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 95afbc181..5f4bf0729 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -4,7 +4,7 @@ // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely // optimized out by the compiler. -//#define EKF_DEBUG +#define EKF_DEBUG #ifdef EKF_DEBUG #include <stdio.h> @@ -2263,21 +2263,29 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KH[i][j])) { err_report->covarianceNaN = true; - ekf_debug("KH NaN"); err = true; } // intermediate result used for covariance updates + if (err) { + ekf_debug("KH NaN"); + } + if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; - ekf_debug("KHP NaN"); err = true; } // intermediate result used for covariance updates + if (err) { + ekf_debug("KHP NaN"); + } + if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; - ekf_debug("P NaN"); err = true; } // covariance matrix + if (err) { + ekf_debug("P NaN"); + } } if (!isfinite(Kfusion[i])) { @@ -2461,12 +2469,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3]) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) { //store initial lat,long and height - latRef = gpsLat; - lonRef = gpsLon; - hgtRef = gpsHgt; + latRef = referenceLat; + lonRef = referenceLon; + hgtRef = referenceHgt; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 734ad0c05..5e60e506f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -308,7 +308,7 @@ void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3]); +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt); float ConstrainFloat(float val, float min, float max); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 828775719..c8005f3d3 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -606,7 +606,6 @@ FixedwingEstimator::task_main() /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; - warnx("TS fail"); } @@ -650,14 +649,14 @@ FixedwingEstimator::task_main() IMUmsec = _sensor_combined.timestamp / 1e3f; float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; - last_run = _sensor_combined.timestamp; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; - warnx("TS fail"); } + last_run = _sensor_combined.timestamp; + // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; @@ -823,6 +822,8 @@ FixedwingEstimator::task_main() */ int check = _ekf->CheckAndBound(); + const char* ekfname = "[ekf] "; + switch (check) { case 0: /* all ok */ @@ -830,27 +831,34 @@ FixedwingEstimator::task_main() case 1: { const char* str = "NaN in states, resetting"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 2: { const char* str = "stale IMU data, resetting"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 3: { - const char* str = "switching dynamic / static state"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + const char* str = "switching to dynamic state"; + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } + + default: + { + const char* str = "unknown reset condition"; + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + } } - // If non-zero, we got a problem + // If non-zero, we got a filter reset if (check) { struct ekf_status_report ekf_report; @@ -912,7 +920,7 @@ FixedwingEstimator::task_main() double lon = home.lon; float alt = home.alt; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); // Initialize projection _local_pos.ref_lat = home.lat * 1e7; @@ -942,7 +950,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); } } |