From d1ecfad4cd51d18d6f2b51382a07707ea4ea0f59 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 13:06:37 +0100 Subject: EKFAttPos: Enforce type safety --- .../ekf_att_pos_estimator_main.cpp | 48 +++--- .../ekf_att_pos_estimator/estimator_22states.cpp | 170 ++++++++++----------- .../ekf_att_pos_estimator/estimator_22states.h | 41 ++--- 3 files changed, 130 insertions(+), 129 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 62965976d..eabef2798 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -113,22 +113,22 @@ uint64_t getMicros() return IMUusec; } -class FixedwingEstimator +class AttitudePositionEstimatorEKF { public: /** * Constructor */ - FixedwingEstimator(); + AttitudePositionEstimatorEKF(); /* we do not want people ever copying this class */ - FixedwingEstimator(const FixedwingEstimator& that) = delete; - FixedwingEstimator operator=(const FixedwingEstimator&) = delete; + AttitudePositionEstimatorEKF(const AttitudePositionEstimatorEKF& that) = delete; + AttitudePositionEstimatorEKF operator=(const AttitudePositionEstimatorEKF&) = delete; /** * Destructor, also kills the sensors task. */ - ~FixedwingEstimator(); + ~AttitudePositionEstimatorEKF(); /** * Start the sensors task. @@ -338,10 +338,10 @@ namespace estimator #endif static const int ERROR = -1; -FixedwingEstimator *g_estimator = nullptr; +AttitudePositionEstimatorEKF *g_estimator = nullptr; } -FixedwingEstimator::FixedwingEstimator() : +AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _task_should_exit(false), _task_running(false), @@ -498,7 +498,7 @@ FixedwingEstimator::FixedwingEstimator() : } } -FixedwingEstimator::~FixedwingEstimator() +AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() { if (_estimator_task != -1) { @@ -526,7 +526,7 @@ FixedwingEstimator::~FixedwingEstimator() } int -FixedwingEstimator::enable_logging(bool logging) +AttitudePositionEstimatorEKF::enable_logging(bool logging) { _ekf_logging = logging; @@ -534,7 +534,7 @@ FixedwingEstimator::enable_logging(bool logging) } int -FixedwingEstimator::parameters_update() +AttitudePositionEstimatorEKF::parameters_update() { param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); @@ -579,7 +579,7 @@ FixedwingEstimator::parameters_update() } void -FixedwingEstimator::vehicle_status_poll() +AttitudePositionEstimatorEKF::vehicle_status_poll() { bool vstatus_updated; @@ -593,7 +593,7 @@ FixedwingEstimator::vehicle_status_poll() } int -FixedwingEstimator::check_filter_state() +AttitudePositionEstimatorEKF::check_filter_state() { /* * CHECK IF THE INPUT DATA IS SANE @@ -687,15 +687,15 @@ FixedwingEstimator::check_filter_state() } // Copy all states or at least all that we can fit - unsigned ekf_n_states = ekf_report.n_states; - unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + size_t ekf_n_states = ekf_report.n_states; + size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; - for (unsigned i = 0; i < rep.n_states; i++) { + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; } - for (unsigned i = 0; i < rep.n_states; i++) { + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; } @@ -710,13 +710,13 @@ FixedwingEstimator::check_filter_state() } void -FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) +AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[]) { estimator::g_estimator->task_main(); } void -FixedwingEstimator::task_main() +AttitudePositionEstimatorEKF::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -1638,7 +1638,7 @@ FixedwingEstimator::task_main() } int -FixedwingEstimator::start() +AttitudePositionEstimatorEKF::start() { ASSERT(_estimator_task == -1); @@ -1647,7 +1647,7 @@ FixedwingEstimator::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 7500, - (main_t)&FixedwingEstimator::task_main_trampoline, + (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); if (_estimator_task < 0) { @@ -1659,7 +1659,7 @@ FixedwingEstimator::start() } void -FixedwingEstimator::print_status() +AttitudePositionEstimatorEKF::print_status() { math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); @@ -1688,7 +1688,7 @@ FixedwingEstimator::print_status() printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - if (n_states == 23) { + if (EKF_STATE_ESTIMATES == 23) { printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); @@ -1713,7 +1713,7 @@ FixedwingEstimator::print_status() (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } -int FixedwingEstimator::trip_nan() { +int AttitudePositionEstimatorEKF::trip_nan() { int ret = 0; @@ -1772,7 +1772,7 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) if (estimator::g_estimator != nullptr) errx(1, "already running"); - estimator::g_estimator = new FixedwingEstimator; + estimator::g_estimator = new AttitudePositionEstimatorEKF(); if (estimator::g_estimator == nullptr) errx(1, "alloc failed"); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 15d018ab6..6e654f496 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -322,12 +322,12 @@ void AttPosEKF::CovariancePrediction(float dt) float dvz_b; // arrays - float processNoise[n_states]; + float processNoise[EKF_STATE_ESTIMATES]; float SF[15]; float SG[8]; float SQ[11]; float SPP[8] = {0}; - float nextP[n_states][n_states]; + float nextP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // calculate covariance prediction process noise for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; @@ -343,13 +343,13 @@ void AttPosEKF::CovariancePrediction(float dt) } if (!inhibitMagStates) { for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i < n_states; i++) processNoise[i] = dt * magBodySigma; + for (uint8_t i=19; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = dt * magBodySigma; } else { - for (uint8_t i=16; i < n_states; i++) processNoise[i] = 0; + for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = 0; } // square all sigmas - for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = sq(processNoise[i]); // set variables used to calculate covariance growth dvx = summedDelVel.x; @@ -908,7 +908,7 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[21][20] = P[21][20]; nextP[21][21] = P[21][21]; - for (unsigned i = 0; i < n_states; i++) + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { nextP[i][i] = nextP[i][i] + processNoise[i]; } @@ -921,7 +921,7 @@ void AttPosEKF::CovariancePrediction(float dt) { for (uint8_t i=7; i<=8; i++) { - for (unsigned j = 0; j < n_states; j++) + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { nextP[i][j] = P[i][j]; nextP[j][i] = P[j][i]; @@ -930,12 +930,12 @@ void AttPosEKF::CovariancePrediction(float dt) } // Copy covariance - for (unsigned i = 0; i < n_states; i++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { P[i][i] = nextP[i][i]; } // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) + for (size_t i = 1; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j < i; j++) { @@ -1179,7 +1179,7 @@ void AttPosEKF::FuseVelposNED() } // Don't update magnetic field states if inhibited if (inhibitMagStates) { - for (uint8_t i = 16; i < n_states; i++) + for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) { Kfusion[i] = 0; } @@ -1246,8 +1246,8 @@ void AttPosEKF::FuseMagnetometer() float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; - float H_MAG[n_states]; - for (uint8_t i = 0; i < n_states; i++) { + float H_MAG[EKF_STATE_ESTIMATES]; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { H_MAG[i] = 0.0f; } @@ -1303,7 +1303,7 @@ void AttPosEKF::FuseMagnetometer() SH_MAG[7] = 2*magN*q0; SH_MAG[8] = 2*magE*q3; - for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; H_MAG[1] = SH_MAG[0]; H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; @@ -1360,7 +1360,7 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); } else { - for (uint8_t i=16; i < n_states; i++) { + for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) { Kfusion[i] = 0; } } @@ -1370,7 +1370,7 @@ void AttPosEKF::FuseMagnetometer() else if (obsIndex == 1) // we are now fusing the Y measurement { // Calculate observation jacobians - for (unsigned int i = 0; i < n_states; i++) H_MAG[i] = 0; + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[2]; H_MAG[1] = SH_MAG[1]; H_MAG[2] = SH_MAG[0]; @@ -1439,7 +1439,7 @@ void AttPosEKF::FuseMagnetometer() else if (obsIndex == 2) // we are now fusing the Z measurement { // Calculate observation jacobians - for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[1]; H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; @@ -1512,7 +1512,7 @@ void AttPosEKF::FuseMagnetometer() if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f) { // correct the state vector - for (uint8_t j= 0; j < n_states; j++) + for (uint8_t j= 0; j < EKF_STATE_ESTIMATES; j++) { states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } @@ -1529,7 +1529,7 @@ void AttPosEKF::FuseMagnetometer() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j <= 3; j++) { @@ -1538,22 +1538,22 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; if (!onGround) { - for (uint8_t j = 16; j < n_states; j++) + for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = Kfusion[i] * H_MAG[j]; } } else { - for (uint8_t j = 16; j < n_states; j++) + for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = 0.0f; } } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 3; k++) @@ -1562,7 +1562,7 @@ void AttPosEKF::FuseMagnetometer() } if (!onGround) { - for (uint8_t k = 16; k < n_states; k++) + for (uint8_t k = 16; k < EKF_STATE_ESTIMATES; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } @@ -1570,9 +1570,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1614,8 +1614,8 @@ void AttPosEKF::FuseAirspeed() SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - float H_TAS[n_states]; - for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f; + float H_TAS[EKF_STATE_ESTIMATES]; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_TAS[i] = 0.0f; H_TAS[4] = SH_TAS[2]; H_TAS[5] = SH_TAS[1]; H_TAS[6] = vd*SH_TAS[0]; @@ -1664,7 +1664,7 @@ void AttPosEKF::FuseAirspeed() Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); } else { - for (uint8_t i=16; i < n_states; i++) { + for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) { Kfusion[i] = 0; } } @@ -1676,7 +1676,7 @@ void AttPosEKF::FuseAirspeed() if ((innovVtas*innovVtas*SK_TAS) < 25.0f) { // correct the state vector - for (uint8_t j=0; j < n_states; j++) + for (uint8_t j=0; j < EKF_STATE_ESTIMATES; j++) { states[j] = states[j] - Kfusion[j] * innovVtas; } @@ -1693,7 +1693,7 @@ void AttPosEKF::FuseAirspeed() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in H to reduce the // number of operations - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; for (uint8_t j = 4; j <= 6; j++) @@ -1705,11 +1705,11 @@ void AttPosEKF::FuseAirspeed() { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 16; j < n_states; j++) KH[i][j] = 0.0; + for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KHP[i][j] = 0.0; for (uint8_t k = 4; k <= 6; k++) @@ -1722,9 +1722,9 @@ void AttPosEKF::FuseAirspeed() } } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1736,13 +1736,13 @@ void AttPosEKF::FuseAirspeed() ConstrainVariances(); } -void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +void AttPosEKF::zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; for (row=first; row<=last; row++) { - for (col=0; col 5Sigma if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) { // correct the state vector - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex]; } @@ -1972,7 +1972,7 @@ void AttPosEKF::FuseOptFlow() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j <= 6; j++) { @@ -1983,14 +1983,14 @@ void AttPosEKF::FuseOptFlow() KH[i][j] = 0.0f; } KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9]; - for (uint8_t j = 10; j < n_states; j++) + for (uint8_t j = 10; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = 0.0f; } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 6; k++) @@ -2000,9 +2000,9 @@ void AttPosEKF::FuseOptFlow() KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -2239,13 +2239,13 @@ void AttPosEKF::OpticalFlowEKF() } -void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; for (col=first; col<=last; col++) { - for (row=0; row < n_states; row++) + for (row=0; row < EKF_STATE_ESTIMATES; row++) { covMat[row][col] = 0.0; } @@ -2278,14 +2278,14 @@ float AttPosEKF::min(float valIn1, float valIn2) // Store states in a history array along with time stamp void AttPosEKF::StoreStates(uint64_t timestamp_ms) { - for (unsigned i=0; i 0) { - for (unsigned i=0; i < 3; i++) { + for (size_t i=0; i < 3; i++) { omegaForFusion[i] += storedOmega[i][storeIndexLocal]; } sumIndex += 1; } } if (sumIndex >= 1) { - for (unsigned i=0; i < 3; i++) { + for (size_t i=0; i < 3; i++) { omegaForFusion[i] = omegaForFusion[i] / float(sumIndex); } } else { @@ -2596,22 +2596,22 @@ void AttPosEKF::ConstrainVariances() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) // Constrain quaternion variances - for (unsigned i = 0; i <= 3; i++) { + for (size_t i = 0; i <= 3; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } // Constrain velocity variances - for (unsigned i = 4; i <= 6; i++) { + for (size_t i = 4; i <= 6; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); } // Constrain position variances - for (unsigned i = 7; i <= 9; i++) { + for (size_t i = 7; i <= 9; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); } // Constrain delta angle bias variances - for (unsigned i = 10; i <= 12; i++) { + for (size_t i = 10; i <= 12; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU)); } @@ -2619,17 +2619,17 @@ void AttPosEKF::ConstrainVariances() P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU)); // Wind velocity variances - for (unsigned i = 14; i <= 15; i++) { + for (size_t i = 14; i <= 15; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); } // Earth magnetic field variances - for (unsigned i = 16; i <= 18; i++) { + for (size_t i = 16; i <= 18; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } // Body magnetic field variances - for (unsigned i = 19; i <= 21; i++) { + for (size_t i = 19; i <= 21; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } @@ -2652,17 +2652,17 @@ void AttPosEKF::ConstrainStates() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) // Constrain quaternion - for (unsigned i = 0; i <= 3; i++) { + for (size_t i = 0; i <= 3; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); } // Constrain velocities to what GPS can do for us - for (unsigned i = 4; i <= 6; i++) { + for (size_t i = 4; i <= 6; i++) { states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); } // Constrain position to a reasonable vehicle range (in meters) - for (unsigned i = 7; i <= 8; i++) { + for (size_t i = 7; i <= 8; i++) { states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); } @@ -2671,7 +2671,7 @@ void AttPosEKF::ConstrainStates() states[9] = ConstrainFloat(states[9], -4.0e4f, 4.0e4f); // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i <= 12; i++) { + for (size_t i = 10; i <= 12; i++) { states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); } @@ -2679,18 +2679,18 @@ void AttPosEKF::ConstrainStates() states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 14; i <= 15; i++) { + for (size_t i = 14; i <= 15; i++) { states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); } // Earth magnetic field limits (in Gauss) - for (unsigned i = 16; i <= 18; i++) { + for (size_t i = 16; i <= 18; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); } // Body magnetic field variances (in Gauss). // the max offset should be in this range. - for (unsigned i = 19; i <= 21; i++) { + for (size_t i = 19; i <= 21; i++) { states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); } @@ -2704,7 +2704,7 @@ void AttPosEKF::ForceSymmetry() // Force symmetry on the covariance matrix to prevent ill-conditioning // of the matrix which would cause the filter to blow-up - for (unsigned i = 1; i < n_states; i++) + for (size_t i = 1; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j < i; j++) { @@ -2865,8 +2865,8 @@ bool AttPosEKF::StatesNaN() { } // delta velocities // check all states and covariance matrices - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { if (!isfinite(KH[i][j])) { current_ekf_state.KHNaN = true; @@ -3206,8 +3206,8 @@ void AttPosEKF::ZeroVariables() lastVelPosFusion = millis(); // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = 0.0f; // intermediate result used for covariance updates KHP[i][j] = 0.0f; // intermediate result used for covariance updates P[i][j] = 0.0f; // covariance matrix @@ -3231,9 +3231,9 @@ void AttPosEKF::ZeroVariables() flowStates[0] = 1.0f; flowStates[1] = 0.0f; - for (unsigned i = 0; i < data_buffer_size; i++) { + for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; i++) { - for (unsigned j = 0; j < n_states; j++) { + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { storedStates[j][i] = 0.0f; } @@ -3252,10 +3252,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) { // Copy states - for (unsigned i = 0; i < n_states; i++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { current_ekf_state.states[i] = states[i]; } - current_ekf_state.n_states = n_states; + current_ekf_state.n_states = EKF_STATE_ESTIMATES; current_ekf_state.onGround = onGround; current_ekf_state.staticMode = staticMode; current_ekf_state.useCompass = useCompass; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index b1d71bd74..f8cd743cc 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -1,9 +1,10 @@ #pragma once #include "estimator_utilities.h" +#include -const unsigned int n_states = 22; -const unsigned int data_buffer_size = 50; +constexpr size_t EKF_STATE_ESTIMATES = 22; +constexpr size_t EKF_DATA_BUFFER_SIZE = 50; class AttPosEKF { @@ -108,25 +109,25 @@ public: // Global variables - float KH[n_states][n_states]; // intermediate result used for covariance updates - float KHP[n_states][n_states]; // intermediate result used for covariance updates - float P[n_states][n_states]; // covariance matrix - float Kfusion[n_states]; // Kalman gains - float states[n_states]; // state matrix - float resetStates[n_states]; - float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps - uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + float KH[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates + float KHP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates + float P[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // covariance matrix + float Kfusion[EKF_STATE_ESTIMATES]; // Kalman gains + float states[EKF_STATE_ESTIMATES]; // state matrix + float resetStates[EKF_STATE_ESTIMATES]; + float storedStates[EKF_STATE_ESTIMATES][EKF_DATA_BUFFER_SIZE]; // state vectors stored for the last 50 time steps + uint32_t statetimeStamp[EKF_DATA_BUFFER_SIZE]; // time stamp for each state vector stored // Times uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter - float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement - float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time - float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time - float statesAtRngTime[n_states]; // filter states at the effective measurement time - float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time + float statesAtVelTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements + float statesAtPosTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements + float statesAtHgtTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for the hgtMea measurement + float statesAtMagMeasTime[EKF_STATE_ESTIMATES]; // filter satates at the effective measurement time + float statesAtVtasMeasTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time + float statesAtRngTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time + float statesAtFlowTime[EKF_STATE_ESTIMATES]; // States at the effective optical flow measurement time float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) @@ -227,7 +228,7 @@ public: unsigned storeIndex; // Optical Flow error estimation - float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators + float storedOmega[3][EKF_DATA_BUFFER_SIZE]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators // Two state EKF used to estimate focal length scale factor and terrain position float Popt[2][2]; // state covariance matrix @@ -265,9 +266,9 @@ void FuseOptFlow(); void OpticalFlowEKF(); -void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); +void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); -void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); +void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); void quatNorm(float (&quatOut)[4], const float quatIn[4]); -- cgit v1.2.3 From ce5a9929ff3748e1c0541388c41e5866d307d1c2 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 13:13:36 +0100 Subject: AttPosEKF: Fix coding style --- .../ekf_att_pos_estimator/estimator_22states.cpp | 64 +++++------ .../ekf_att_pos_estimator/estimator_22states.h | 120 ++++++++++----------- 2 files changed, 92 insertions(+), 92 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 6e654f496..d3f3fde7b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -177,7 +177,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float deltaQuat[4]; const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS); -// Remove sensor bias errors + // Remove sensor bias errors correctedDelAng.x = dAngIMU.x - states[10]; correctedDelAng.y = dAngIMU.y - states[11]; correctedDelAng.z = dAngIMU.z - states[12]; @@ -192,14 +192,14 @@ void AttPosEKF::UpdateStrapdownEquationsNED() delAngTotal.y += correctedDelAng.y; delAngTotal.z += correctedDelAng.z; -// Save current measurements + // Save current measurements Vector3f prevDelAng = correctedDelAng; -// Apply corrections for earths rotation rate and coning errors -// * and + operators have been overloaded + // Apply corrections for earths rotation rate and coning errors + // * and + operators have been overloaded correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); -// Convert the rotation vector to its equivalent quaternion + // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f) { @@ -221,14 +221,14 @@ void AttPosEKF::UpdateStrapdownEquationsNED() deltaQuat[3] = correctedDelAng.z*rotScaler; } -// Update the quaternions by rotating from the previous attitude through -// the delta angle rotation quaternion + // Update the quaternions by rotating from the previous attitude through + // the delta angle rotation quaternion qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; -// Normalise the quaternions and update the quaternion states + // Normalise the quaternions and update the quaternion states quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); if (quatMag > 1e-16f) { @@ -239,7 +239,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() states[3] = quatMagInv*qUpdated[3]; } -// Calculate the body to nav cosine matrix + // Calculate the body to nav cosine matrix q00 = sq(states[0]); q11 = sq(states[1]); q22 = sq(states[2]); @@ -263,29 +263,29 @@ void AttPosEKF::UpdateStrapdownEquationsNED() Tnb = Tbn.transpose(); -// transform body delta velocities to delta velocities in the nav frame -// * and + operators have been overloaded + // transform body delta velocities to delta velocities in the nav frame + // * and + operators have been overloaded //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU; delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU; delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU; -// calculate the magnitude of the nav acceleration (required for GPS -// variance estimation) + // calculate the magnitude of the nav acceleration (required for GPS + // variance estimation) accNavMag = delVelNav.length()/dtIMU; -// If calculating position save previous velocity + // If calculating position save previous velocity float lastVelocity[3]; lastVelocity[0] = states[4]; lastVelocity[1] = states[5]; lastVelocity[2] = states[6]; -// Sum delta velocities to get velocity + // Sum delta velocities to get velocity states[4] = states[4] + delVelNav.x; states[5] = states[5] + delVelNav.y; states[6] = states[6] + delVelNav.z; -// If calculating postions, do a trapezoidal integration for position + // If calculating postions, do a trapezoidal integration for position states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; @@ -965,24 +965,24 @@ void AttPosEKF::updateDtVelPosFilt(float dt) void AttPosEKF::FuseVelposNED() { -// declare variables used by fault isolation logic + // declare variables used by fault isolation logic uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available uint32_t hgtRetryTime = 500; // height measurement retry time uint32_t horizRetryTime; -// declare variables used to check measurement errors + // declare variables used to check measurement errors float velInnov[3] = {0.0f,0.0f,0.0f}; float posInnov[2] = {0.0f,0.0f}; float hgtInnov = 0.0f; -// declare variables used to control access to arrays + // declare variables used to control access to arrays bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; uint8_t indexLimit = 21; -// declare variables used by state and covariance update calculations + // declare variables used by state and covariance update calculations float velErr; float posErr; float R_OBS[6]; @@ -990,12 +990,12 @@ void AttPosEKF::FuseVelposNED() float SK; float quatMag; -// Perform sequential fusion of GPS measurements. This assumes that the -// errors in the different velocity and position components are -// uncorrelated which is not true, however in the absence of covariance -// data from the GPS receiver it is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion + // Perform sequential fusion of GPS measurements. This assumes that the + // errors in the different velocity and position components are + // uncorrelated which is not true, however in the absence of covariance + // data from the GPS receiver it is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { uint64_t tNow = getMicros(); @@ -1251,12 +1251,12 @@ void AttPosEKF::FuseMagnetometer() H_MAG[i] = 0.0f; } -// Perform sequential fusion of Magnetometer measurements. -// This assumes that the errors in the different components are -// uncorrelated which is not true, however in the absence of covariance -// data fit is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion + // Perform sequential fusion of Magnetometer measurements. + // This assumes that the errors in the different components are + // uncorrelated which is not true, however in the absence of covariance + // data fit is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion if (useCompass && fuseMagData && (obsIndex < 3)) { // Calculate observation jacobians and Kalman gains diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index f8cd743cc..a4b95f14a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -248,115 +248,115 @@ public: float minFlowRng; // minimum range over which to fuse optical flow measurements float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate -void updateDtGpsFilt(float dt); + void updateDtGpsFilt(float dt); -void updateDtHgtFilt(float dt); + void updateDtHgtFilt(float dt); -void UpdateStrapdownEquationsNED(); + void UpdateStrapdownEquationsNED(); -void CovariancePrediction(float dt); + void CovariancePrediction(float dt); -void FuseVelposNED(); + void FuseVelposNED(); -void FuseMagnetometer(); + void FuseMagnetometer(); -void FuseAirspeed(); + void FuseAirspeed(); -void FuseOptFlow(); + void FuseOptFlow(); -void OpticalFlowEKF(); + void OpticalFlowEKF(); -void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); + void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); -void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); + void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); -void quatNorm(float (&quatOut)[4], const float quatIn[4]); + void quatNorm(float (&quatOut)[4], const float quatIn[4]); -// store staes along with system time stamp in msces -void StoreStates(uint64_t timestamp_ms); + // store staes along with system time stamp in msces + void StoreStates(uint64_t timestamp_ms); -/** - * Recall the state vector. - * - * Recalls the vector stored at closest time to the one specified by msec - *FuseOptFlow - * @return zero on success, integer indicating the number of invalid states on failure. - * Does only copy valid states, if the statesForFusion vector was initialized - * correctly by the caller, the result can be safely used, but is a mixture - * time-wise where valid states were updated and invalid remained at the old - * value. - */ -int RecallStates(float *statesForFusion, uint64_t msec); + /** + * Recall the state vector. + * + * Recalls the vector stored at closest time to the one specified by msec + *FuseOptFlow + * @return zero on success, integer indicating the number of invalid states on failure. + * Does only copy valid states, if the statesForFusion vector was initialized + * correctly by the caller, the result can be safely used, but is a mixture + * time-wise where valid states were updated and invalid remained at the old + * value. + */ + int RecallStates(float *statesForFusion, uint64_t msec); -void RecallOmega(float *omegaForFusion, uint64_t msec); + void RecallOmega(float *omegaForFusion, uint64_t msec); -void ResetStoredStates(); + void ResetStoredStates(); -void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); + void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); -void calcEarthRateNED(Vector3f &omega, float latitude); + void calcEarthRateNED(Vector3f &omega, float latitude); -static void eul2quat(float (&quat)[4], const float (&eul)[3]); + static void eul2quat(float (&quat)[4], const float (&eul)[3]); -static void quat2eul(float (&eul)[3], const float (&quat)[4]); + static void quat2eul(float (&eul)[3], const float (&quat)[4]); -static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); + void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); -static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); + static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); -static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); + static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); -static float sq(float valIn); + static float sq(float valIn); -static float maxf(float valIn1, float valIn2); + static float maxf(float valIn1, float valIn2); -static float min(float valIn1, float valIn2); + static float min(float valIn1, float valIn2); -void OnGroundCheck(); + void OnGroundCheck(); -void CovarianceInit(); + void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); + void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); -float ConstrainFloat(float val, float min, float maxf); + float ConstrainFloat(float val, float min, float maxf); -void ConstrainVariances(); + void ConstrainVariances(); -void ConstrainStates(); + void ConstrainStates(); -void ForceSymmetry(); + void ForceSymmetry(); -int CheckAndBound(struct ekf_status_report *last_error); + int CheckAndBound(struct ekf_status_report *last_error); -void ResetPosition(); + void ResetPosition(); -void ResetVelocity(); + void ResetVelocity(); -void ZeroVariables(); + void ZeroVariables(); -void GetFilterState(struct ekf_status_report *state); + void GetFilterState(struct ekf_status_report *state); -void GetLastErrorState(struct ekf_status_report *last_error); + void GetLastErrorState(struct ekf_status_report *last_error); -bool StatesNaN(); + bool StatesNaN(); -void InitializeDynamic(float (&initvelNED)[3], float declination); + void InitializeDynamic(float (&initvelNED)[3], float declination); protected: -void updateDtVelPosFilt(float dt); + void updateDtVelPosFilt(float dt); -bool FilterHealthy(); + bool FilterHealthy(); -bool GyroOffsetsDiverged(); + bool GyroOffsetsDiverged(); -bool VelNEDDiverged(); + bool VelNEDDiverged(); -void ResetHeight(void); + void ResetHeight(void); -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); + void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); }; -- cgit v1.2.3 From 022208e998ec6c1594b7493c35d15d5358587d86 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 13:15:30 +0100 Subject: AttPosEKF: Move initializeParameters() from header to implementation file --- .../ekf_att_pos_estimator/estimator_22states.cpp | 34 ++++++++++++++++++ .../ekf_att_pos_estimator/estimator_22states.h | 41 ++++------------------ 2 files changed, 40 insertions(+), 35 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index d3f3fde7b..44a9f4fb7 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -158,6 +158,40 @@ AttPosEKF::~AttPosEKF() { } +void AttPosEKF::InitialiseParameters() +{ + covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions + covDelAngMax = 0.02f; // maximum delta angle between covariance predictions + rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + EAS2TAS = 1.0f; + + yawVarScale = 1.0f; + windVelSigma = 0.1f; + dAngBiasSigma = 5.0e-7f; + dVelBiasSigma = 1e-4f; + magEarthSigma = 3.0e-4f; + magBodySigma = 3.0e-4f; + + vneSigma = 0.2f; + vdSigma = 0.3f; + posNeSigma = 2.0f; + posDSigma = 2.0f; + + magMeasurementSigma = 0.05; + airspeedMeasurementSigma = 1.4f; + gyroProcessNoise = 1.4544411e-2f; + accelProcessNoise = 0.5f; + + gndHgtSigma = 0.1f; // terrain gradient 1-sigma + R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2 + flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check + auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter + rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check + minFlowRng = 0.3f; //minimum range between ground and flow sensor + moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate +} + + void AttPosEKF::UpdateStrapdownEquationsNED() { Vector3f delVelNav; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index a4b95f14a..221a17ba7 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -23,7 +23,7 @@ public: /* * parameters are defined here and initialised in - * the InitialiseParameters() (which is just 20 lines down) + * the InitialiseParameters() */ float covTimeStepMax; // maximum time allowed between covariance predictions @@ -50,40 +50,6 @@ public: float EAS2TAS; // ratio f true to equivalent airspeed - void InitialiseParameters() - { - covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions - covDelAngMax = 0.02f; // maximum delta angle between covariance predictions - rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - EAS2TAS = 1.0f; - - yawVarScale = 1.0f; - windVelSigma = 0.1f; - dAngBiasSigma = 5.0e-7f; - dVelBiasSigma = 1e-4f; - magEarthSigma = 3.0e-4f; - magBodySigma = 3.0e-4f; - - vneSigma = 0.2f; - vdSigma = 0.3f; - posNeSigma = 2.0f; - posDSigma = 2.0f; - - magMeasurementSigma = 0.05; - airspeedMeasurementSigma = 1.4f; - gyroProcessNoise = 1.4544411e-2f; - accelProcessNoise = 0.5f; - - gndHgtSigma = 0.1f; // terrain gradient 1-sigma - R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2 - flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check - auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter - rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check - minFlowRng = 0.3f; //minimum range between ground and flow sensor - moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate - - } - struct mag_state_struct { unsigned obsIndex; float MagPred[3]; @@ -346,6 +312,11 @@ public: protected: + /** + * @brief Initializes algorithm parameters to safe default values + **/ + void InitialiseParameters(); + void updateDtVelPosFilt(float dt); bool FilterHealthy(); -- cgit v1.2.3 From 7287dc3c4ccd92b8bfb1f6d990d42c661d9f4b96 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 13:18:32 +0100 Subject: AttPosEKF: Replace custom min/max functions with c++ standard --- .../ekf_att_pos_estimator/estimator_22states.cpp | 44 ++++++---------------- .../ekf_att_pos_estimator/estimator_22states.h | 6 +-- 2 files changed, 12 insertions(+), 38 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 44a9f4fb7..adf6961ef 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #ifndef M_PI_F #define M_PI_F ((float)M_PI) @@ -1805,7 +1806,7 @@ void AttPosEKF::FuseOptFlow() Vector3f relVelSensor; // Perform sequential fusion of optical flow measurements only with valid tilt and height - flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9]; bool validTilt = Tnb.z.z > 0.71f; if (validTilt) @@ -2070,7 +2071,7 @@ void AttPosEKF::OpticalFlowEKF() } else { return; } - distanceTravelledSq = min(distanceTravelledSq, 100.0f); + distanceTravelledSq = std::min(distanceTravelledSq, 100.0f); Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma)); } @@ -2110,7 +2111,7 @@ void AttPosEKF::OpticalFlowEKF() varInnovRng = 1.0f/SK_RNG[1]; // constrain terrain height to be below the vehicle - flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); + flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng); // estimate range to centre of image range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2]; @@ -2130,7 +2131,7 @@ void AttPosEKF::OpticalFlowEKF() } // constrain the states flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); + flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng); // correct the covariance matrix float nextPopt[2][2]; @@ -2139,8 +2140,8 @@ void AttPosEKF::OpticalFlowEKF() nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); // prevent the state variances from becoming negative and maintain symmetry - Popt[0][0] = maxf(nextPopt[0][0],0.0f); - Popt[1][1] = maxf(nextPopt[1][1],0.0f); + Popt[0][0] = std::max(nextPopt[0][0],0.0f); + Popt[1][1] = std::max(nextPopt[1][1],0.0f); Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); Popt[1][0] = Popt[0][1]; } @@ -2179,7 +2180,7 @@ void AttPosEKF::OpticalFlowEKF() vel.z = statesAtFlowTime[6]; // constrain terrain height to be below the vehicle - flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); // estimate range to centre of image range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z; @@ -2247,7 +2248,7 @@ void AttPosEKF::OpticalFlowEKF() } // constrain the states flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); // correct the covariance matrix for (uint8_t i = 0; i < 2 ; i++) { @@ -2263,8 +2264,8 @@ void AttPosEKF::OpticalFlowEKF() } // prevent the state variances from becoming negative and maintain symmetry - Popt[0][0] = maxf(nextPopt[0][0],0.0f); - Popt[1][1] = maxf(nextPopt[1][1],0.0f); + Popt[0][0] = std::max(nextPopt[0][0],0.0f); + Popt[1][1] = std::max(nextPopt[1][1],0.0f); Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); Popt[1][0] = Popt[0][1]; } @@ -2286,29 +2287,6 @@ void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATE } } -float AttPosEKF::sq(float valIn) -{ - return valIn*valIn; -} - -float AttPosEKF::maxf(float valIn1, float valIn2) -{ - if (valIn1 >= valIn2) { - return valIn1; - } else { - return valIn2; - } -} - -float AttPosEKF::min(float valIn1, float valIn2) -{ - if (valIn1 <= valIn2) { - return valIn1; - } else { - return valIn2; - } -} - // Store states in a history array along with time stamp void AttPosEKF::StoreStates(uint64_t timestamp_ms) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 221a17ba7..de21a75ca 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -274,11 +274,7 @@ public: static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); - static float sq(float valIn); - - static float maxf(float valIn1, float valIn2); - - static float min(float valIn1, float valIn2); + static inline float sq(float valIn) {return valIn * valIn;} void OnGroundCheck(); -- cgit v1.2.3 From 83298110c1dcb37a734d0140b7067ad7dd267e26 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 13:20:10 +0100 Subject: AttPosEKF: Disable unused function --- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 6 ++++-- src/modules/ekf_att_pos_estimator/estimator_22states.h | 4 ++-- 2 files changed, 6 insertions(+), 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index adf6961ef..4e4ef3b92 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -6,10 +6,10 @@ #include #ifndef M_PI_F -#define M_PI_F ((float)M_PI) +#define M_PI_F static_cast(M_PI) #endif -#define EKF_COVARIANCE_DIVERGED 1.0e8f +constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : covTimeStepMax(0.0f), @@ -2401,6 +2401,7 @@ void AttPosEKF::RecallOmega(float* omegaForFusion, uint64_t msec) } } +#if 0 void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) { // Calculate the nav to body cosine matrix @@ -2425,6 +2426,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) Tnb.x.z = 2*(q13 - q02); Tnb.y.z = 2*(q23 + q01); } +#endif void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4]) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index de21a75ca..898105db4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -218,7 +218,7 @@ public: void updateDtHgtFilt(float dt); - void UpdateStrapdownEquationsNED(); + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -272,7 +272,7 @@ public: static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); - static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); + //static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); static inline float sq(float valIn) {return valIn * valIn;} -- cgit v1.2.3 From 723c138b09663128fe2f5309bbe77b4ce22ef5fa Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 13:25:05 +0100 Subject: AttPosEKF: Move documentation to header file --- .../ekf_att_pos_estimator_main.cpp | 36 ++++++++------------ .../ekf_att_pos_estimator/estimator_22states.cpp | 28 ---------------- .../ekf_att_pos_estimator/estimator_22states.h | 38 ++++++++++++++++++++-- 3 files changed, 50 insertions(+), 52 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index eabef2798..fb88fa8e8 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -332,13 +332,13 @@ private: namespace estimator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; + /* oddly, ERROR is not defined for c++ */ + #ifdef ERROR + # undef ERROR + #endif + static const int ERROR = -1; -AttitudePositionEstimatorEKF *g_estimator = nullptr; + AttitudePositionEstimatorEKF *g_estimator = nullptr; } AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : @@ -525,16 +525,14 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() estimator::g_estimator = nullptr; } -int -AttitudePositionEstimatorEKF::enable_logging(bool logging) +int AttitudePositionEstimatorEKF::enable_logging(bool logging) { _ekf_logging = logging; return 0; } -int -AttitudePositionEstimatorEKF::parameters_update() +int AttitudePositionEstimatorEKF::parameters_update() { param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); @@ -578,8 +576,7 @@ AttitudePositionEstimatorEKF::parameters_update() return OK; } -void -AttitudePositionEstimatorEKF::vehicle_status_poll() +void AttitudePositionEstimatorEKF::vehicle_status_poll() { bool vstatus_updated; @@ -592,8 +589,7 @@ AttitudePositionEstimatorEKF::vehicle_status_poll() } } -int -AttitudePositionEstimatorEKF::check_filter_state() +int AttitudePositionEstimatorEKF::check_filter_state() { /* * CHECK IF THE INPUT DATA IS SANE @@ -709,14 +705,12 @@ AttitudePositionEstimatorEKF::check_filter_state() return check; } -void -AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[]) +void AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[]) { estimator::g_estimator->task_main(); } -void -AttitudePositionEstimatorEKF::task_main() +void AttitudePositionEstimatorEKF::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -1637,8 +1631,7 @@ AttitudePositionEstimatorEKF::task_main() _exit(0); } -int -AttitudePositionEstimatorEKF::start() +int AttitudePositionEstimatorEKF::start() { ASSERT(_estimator_task == -1); @@ -1658,8 +1651,7 @@ AttitudePositionEstimatorEKF::start() return OK; } -void -AttitudePositionEstimatorEKF::print_status() +void AttitudePositionEstimatorEKF::print_status() { math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 4e4ef3b92..28d0fde34 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2049,11 +2049,6 @@ void AttPosEKF::FuseOptFlow() } } -/* -Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF -This fiter requires optical flow rates that are not motion compensated -Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser -*/ void AttPosEKF::OpticalFlowEKF() { // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption @@ -2806,12 +2801,6 @@ bool AttPosEKF::FilterHealthy() return true; } -/** - * Reset the filter position states - * - * This resets the position to the last GPS measurement - * or to zero in case of static position. - */ void AttPosEKF::ResetPosition(void) { if (staticMode) { @@ -2825,20 +2814,12 @@ void AttPosEKF::ResetPosition(void) } } -/** - * Reset the height state. - * - * This resets the height state with the last altitude measurements - */ void AttPosEKF::ResetHeight(void) { // write to the state vector states[9] = -hgtMea; } -/** - * Reset the velocity state. - */ void AttPosEKF::ResetVelocity(void) { if (staticMode) { @@ -2931,15 +2912,6 @@ out: } -/** - * Check the filter inputs and bound its operational state - * - * This check will reset the filter states if required - * due to a failure of consistency or timeout checks. - * it should be run after the measurement data has been - * updated, but before any of the fusion steps are - * executed. - */ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 898105db4..be382127b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -230,6 +230,12 @@ public: void FuseOptFlow(); + /** + * @brief + * Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF + * This fiter requires optical flow rates that are not motion compensated + * Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser + **/ void OpticalFlowEKF(); void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); @@ -290,10 +296,31 @@ public: void ForceSymmetry(); + /** + * @brief + * Check the filter inputs and bound its operational state + * + * This check will reset the filter states if required + * due to a failure of consistency or timeout checks. + * it should be run after the measurement data has been + * updated, but before any of the fusion steps are + * executed. + */ int CheckAndBound(struct ekf_status_report *last_error); + /** + * @brief + * Reset the filter position states + * + * This resets the position to the last GPS measurement + * or to zero in case of static position. + */ void ResetPosition(); + /** + * @brief + * Reset the velocity state. + */ void ResetVelocity(); void ZeroVariables(); @@ -309,7 +336,8 @@ public: protected: /** - * @brief Initializes algorithm parameters to safe default values + * @brief + * Initializes algorithm parameters to safe default values **/ void InitialiseParameters(); @@ -321,7 +349,13 @@ protected: bool VelNEDDiverged(); - void ResetHeight(void); + /** + * @brief + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ + void ResetHeight(); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); -- cgit v1.2.3 From ce07e0de2d7cd2b476c8b8f304606f42bfd809f4 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 13:27:45 +0100 Subject: AttPosEKF: Added missing license header text --- .../ekf_att_pos_estimator/estimator_22states.cpp | 42 ++++++++++++++++++++ .../ekf_att_pos_estimator/estimator_22states.h | 42 ++++++++++++++++++++ .../ekf_att_pos_estimator/estimator_utilities.cpp | 41 ++++++++++++++++++++ .../ekf_att_pos_estimator/estimator_utilities.h | 45 +++++++++++++++++++++- 4 files changed, 168 insertions(+), 2 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 28d0fde34..5e4377f8a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -1,3 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file estimator_22states.cpp + * + * Implementation of the attitude and position estimator. + * + * @author Paul Riseborough + * @author Lorenz Meier + */ + #include "estimator_22states.h" #include #include diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index be382127b..5f35e0404 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -1,3 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file estimator_22states.h + * + * Implementation of the attitude and position estimator. + * + * @author Paul Riseborough + * @author Lorenz Meier + */ + #pragma once #include "estimator_utilities.h" diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 77cc1eeeb..855ea2c6e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -1,3 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file estimator_utilities.cpp + * + * Implementation of the attitude and position estimator. + * + * @author Paul Riseborough + * @author Lorenz Meier + */ #include "estimator_utilities.h" diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index a6b670c4d..3b9739fb5 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -1,8 +1,49 @@ -#include -#include +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file estimator_utilities.h + * + * Implementation of the attitude and position estimator. + * + * @author Paul Riseborough + * @author Lorenz Meier + */ #pragma once +#include +#include + #define GRAVITY_MSS 9.80665f #define deg2rad 0.017453292f #define rad2deg 57.295780f -- cgit v1.2.3 From ebb111dafa4929ddc3cab75a6202655c39756db0 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 13:35:13 +0100 Subject: AttPosEKF: Replace sqrt with sqrtf --- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 5e4377f8a..cb0f0e7cb 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -1267,7 +1267,7 @@ void AttPosEKF::FuseVelposNED() { states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; } - quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) // divide by 0 protection { for (uint8_t i = 0; i<=3; i++) @@ -1594,7 +1594,7 @@ void AttPosEKF::FuseMagnetometer() states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) @@ -1687,7 +1687,7 @@ void AttPosEKF::FuseAirspeed() if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { // Calculate observation jacobians - SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[0] = 1.0f / (sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; @@ -1758,7 +1758,7 @@ void AttPosEKF::FuseAirspeed() states[j] = states[j] - Kfusion[j] * innovVtas; } // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) { for (uint8_t j= 0; j <= 3; j++) @@ -2037,7 +2037,7 @@ void AttPosEKF::FuseOptFlow() states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex]; } // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) -- cgit v1.2.3 From 66cb129d181fd8c0b038161f1d84e96b4316e0c0 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 14:16:48 +0100 Subject: AttPosEKF: Fix license text for InertialNav files --- .../ekf_att_pos_estimator/estimator_22states.cpp | 62 ++++++++++------------ .../ekf_att_pos_estimator/estimator_22states.h | 60 ++++++++++----------- 2 files changed, 57 insertions(+), 65 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index cb0f0e7cb..47706c37b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -1,35 +1,32 @@ /**************************************************************************** - * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ +* Copyright (c) 2014, Paul Riseborough All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* Neither the name of the {organization} nor the names of its contributors +* may be used to endorse or promote products derived from this software without +* specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +****************************************************************************/ /** * @file estimator_22states.cpp @@ -37,7 +34,6 @@ * Implementation of the attitude and position estimator. * * @author Paul Riseborough - * @author Lorenz Meier */ #include "estimator_22states.h" @@ -1687,7 +1683,7 @@ void AttPosEKF::FuseAirspeed() if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { // Calculate observation jacobians - SH_TAS[0] = 1.0f / (sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 5f35e0404..b17f1e6ee 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -1,35 +1,32 @@ /**************************************************************************** - * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ +* Copyright (c) 2014, Paul Riseborough All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* Neither the name of the {organization} nor the names of its contributors +* may be used to endorse or promote products derived from this software without +* specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*****************************************************************************/ /** * @file estimator_22states.h @@ -37,7 +34,6 @@ * Implementation of the attitude and position estimator. * * @author Paul Riseborough - * @author Lorenz Meier */ #pragma once -- cgit v1.2.3 From 3d1e373e36f4831a9bb9a7569837a9ff9fcde1f4 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 10 Feb 2015 14:56:19 +0100 Subject: AttPosEKF: Fix initialization of AMSL estimation without GPS --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index fb88fa8e8..81b27ddbe 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1331,6 +1331,7 @@ void AttitudePositionEstimatorEKF::task_main() _local_pos.ref_alt = _baro_ref; _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; + _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); -- cgit v1.2.3 From 7f7c36b5b1e4977bb00127bbce0392f6bbd15baf Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 10 Feb 2015 14:56:50 +0100 Subject: AttPosEKF: Publish altitude position estimates without GPS --- .../ekf_att_pos_estimator_main.cpp | 168 ++++++++++----------- 1 file changed, 83 insertions(+), 85 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 81b27ddbe..e02adb305 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1514,107 +1514,105 @@ void AttitudePositionEstimatorEKF::task_main() _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } - if (_gps_initialized) { - _local_pos.timestamp = _last_sensor_timestamp; - _local_pos.x = _ekf->states[7]; - _local_pos.y = _ekf->states[8]; - // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset; - - _local_pos.vx = _ekf->states[4]; - _local_pos.vy = _ekf->states[5]; - _local_pos.vz = _ekf->states[6]; - - _local_pos.xy_valid = _gps_initialized; - _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized; - _local_pos.v_z_valid = true; - _local_pos.xy_global = true; - - _local_pos.z_global = false; - _local_pos.yaw = _att.yaw; - - /* lazily publish the local position only once available */ - if (_local_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + //Publish position estimations + _local_pos.timestamp = _last_sensor_timestamp; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; + + // XXX need to announce change of Z reference somehow elegantly + _local_pos.z = _ekf->states[9] - _baro_ref_offset; + + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; + + _local_pos.xy_valid = _gps_initialized; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = _gps_initialized; + _local_pos.v_z_valid = true; + _local_pos.xy_global = _gps_initialized; + + _local_pos.z_global = false; + _local_pos.yaw = _att.yaw; + + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); - } else { - /* advertise and publish */ - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); - } + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } - _global_pos.timestamp = _local_pos.timestamp; + _global_pos.timestamp = _local_pos.timestamp; - if (_local_pos.xy_global) { - double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); - _global_pos.lat = est_lat; - _global_pos.lon = est_lon; - _global_pos.time_utc_usec = _gps.time_utc_usec; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; - } + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_utc_usec = _gps.time_utc_usec; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + } - if (_local_pos.v_xy_valid) { - _global_pos.vel_n = _local_pos.vx; - _global_pos.vel_e = _local_pos.vy; - } else { - _global_pos.vel_n = 0.0f; - _global_pos.vel_e = 0.0f; - } + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } - /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; - if (_local_pos.v_z_valid) { - _global_pos.vel_d = _local_pos.vz; - } + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } - /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + /* terrain altitude */ + _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; + _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && + (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); - _global_pos.yaw = _local_pos.yaw; + _global_pos.yaw = _local_pos.yaw; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + + _global_pos.timestamp = _local_pos.timestamp; + + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the global position */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } - _global_pos.timestamp = _local_pos.timestamp; + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + // XXX we need to do something smart about the covariance here + // but we default to the estimate covariance for now + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; - /* lazily publish the global position only once available */ - if (_global_pos_pub > 0) { - /* publish the global position */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); } else { /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); } - - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - // XXX we need to do something smart about the covariance here - // but we default to the estimate covariance for now - _wind.covariance_north = _ekf->P[14][14]; - _wind.covariance_east = _ekf->P[15][15]; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - } - } - } } -- cgit v1.2.3 From dc522f217f5122dcbdc157d5ddb43052e9cb3d3d Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 11 Feb 2015 16:42:01 +0100 Subject: Commander: Fix GPS loss not handled properly --- src/modules/commander/commander.cpp | 35 +++++++++++++----------------- src/modules/commander/commander_helper.cpp | 5 +++++ src/modules/commander/commander_helper.h | 1 + 3 files changed, 21 insertions(+), 20 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f2e72dd0c..b2d9c9da3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1388,30 +1388,25 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } - /* update condition_global_position_valid */ - /* hysteresis for EPH/EPV */ - bool eph_good; - - if (status.condition_global_position_valid) { - if (global_position.eph > eph_threshold * 2.5f) { - eph_good = false; - - } else { - eph_good = true; + //update condition_global_position_valid + //Global positions are only published by the estimators if they are valid + if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { + //We have had no good fix for POSITION_TIMEOUT amount of time + if(status.condition_global_position_valid) { + set_tune_override(TONE_GPS_WARNING_TUNE); + status_changed = true; } - } else { - if (global_position.eph < eph_threshold) { - eph_good = true; - - } else { - eph_good = false; + status.condition_global_position_valid = false; + } + else if(hrt_absolute_time() > POSITION_TIMEOUT) { + //Got good global position estimate + if(!status.condition_global_position_valid) { + status_changed = true; + status.condition_global_position_valid = true; } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), - &status_changed); - /* update condition_local_position_valid and condition_local_altitude_valid */ /* hysteresis for EPH */ bool local_eph_good; @@ -2119,7 +2114,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ } else { - if (status_local->condition_local_position_valid) { + if (status_local->condition_global_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 60e8be23e..b5ec6c4e6 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -115,6 +115,11 @@ void buzzer_deinit() close(buzzer); } +void set_tune_override(int tune) +{ + ioctl(buzzer, TONE_SET_ALARM, tune); +} + void set_tune(int tune) { unsigned int new_tune_duration = tune_durations[tune]; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 4a77fe487..cd3db7324 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -55,6 +55,7 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); +void set_tune_override(int tune); void set_tune(int tune); void tune_positive(bool use_buzzer); void tune_neutral(bool use_buzzer); -- cgit v1.2.3 From c1e13e5afba251723cec51539ae08840d1fe3b29 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 11 Feb 2015 16:43:04 +0100 Subject: AttPosEKF: Fix GPS loss timeout not resetting properly --- .../ekf_att_pos_estimator_main.cpp | 39 +++++++++++----------- 1 file changed, 19 insertions(+), 20 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e02adb305..e06e174a5 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1068,6 +1068,7 @@ void AttitudePositionEstimatorEKF::task_main() perf_count(_perf_gps); if (_gps.fix_type < 3) { + //Too poor GPS fix to use newDataGps = false; } else { @@ -1075,17 +1076,11 @@ void AttitudePositionEstimatorEKF::task_main() /* store time of valid GPS measurement */ _gps_start_time = hrt_absolute_time(); - /* check if we had a GPS outage for a long time */ - float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f; - const float pos_reset_threshold = 5.0f; // seconds - /* timeout of 5 seconds */ - if (gps_elapsed > pos_reset_threshold) { - _ekf->ResetPosition(); - _ekf->ResetVelocity(); - _ekf->ResetStoredStates(); - } + //Calculate time since last good GPS fix + float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f; + _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold)); /* fuse GPS updates */ @@ -1102,6 +1097,11 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + //Convert from global frame to local frame + _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; + // update LPF _gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt); @@ -1121,6 +1121,14 @@ void AttitudePositionEstimatorEKF::task_main() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + /* check if we had a GPS outage for a long time */ + /* timeout of 5 seconds */ + if (gps_elapsed > pos_reset_threshold) { + _ekf->ResetPosition(); + _ekf->ResetVelocity(); + _ekf->ResetStoredStates(); + } + newDataGps = true; last_gps = _gps.timestamp_position; @@ -1275,10 +1283,6 @@ void AttitudePositionEstimatorEKF::task_main() double lon = _gps.lon / 1.0e7; float gps_alt = _gps.alt / 1e3f; - initVelNED[0] = _gps.vel_n_m_s; - initVelNED[1] = _gps.vel_e_m_s; - initVelNED[2] = _gps.vel_d_m_s; - // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame @@ -1383,19 +1387,14 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; } - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; // set fusion flags _ekf->fuseVelData = true; _ekf->fusePosData = true; + // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // run the fusion step _ekf->FuseVelposNED(); -- cgit v1.2.3 From 8d8a66607a9a2069a8533ab8893c6322db76d5f1 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 11 Feb 2015 16:43:52 +0100 Subject: AttPosEKF: Do not publish global position if we have none --- .../ekf_att_pos_estimator_main.cpp | 82 +++++++++++----------- .../ekf_att_pos_estimator/estimator_22states.h | 2 +- 2 files changed, 42 insertions(+), 42 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e06e174a5..fbd695eac 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1525,9 +1525,9 @@ void AttitudePositionEstimatorEKF::task_main() _local_pos.vy = _ekf->states[5]; _local_pos.vz = _ekf->states[6]; - _local_pos.xy_valid = _gps_initialized; + _local_pos.xy_valid = _gps_initialized && _gps.fix_type >= 3; _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized; + _local_pos.v_xy_valid = _gps_initialized && _gps.fix_type >= 3; _local_pos.v_z_valid = true; _local_pos.xy_global = _gps_initialized; @@ -1544,53 +1544,53 @@ void AttitudePositionEstimatorEKF::task_main() _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); } - _global_pos.timestamp = _local_pos.timestamp; - - if (_local_pos.xy_global) { - double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); - _global_pos.lat = est_lat; - _global_pos.lon = est_lon; - _global_pos.time_utc_usec = _gps.time_utc_usec; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; - } + //Publish Global Position, but only if it's any good + if(_gps_initialized && _gps.fix_type >= 3 && _gps.eph < _parameters.pos_stddev_threshold * 2.0f) + { + _global_pos.timestamp = _local_pos.timestamp; + + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_utc_usec = _gps.time_utc_usec; + } - if (_local_pos.v_xy_valid) { - _global_pos.vel_n = _local_pos.vx; - _global_pos.vel_e = _local_pos.vy; - } else { - _global_pos.vel_n = 0.0f; - _global_pos.vel_e = 0.0f; - } + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } - /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; - if (_local_pos.v_z_valid) { - _global_pos.vel_d = _local_pos.vz; - } - - /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } - _global_pos.yaw = _local_pos.yaw; + /* terrain altitude */ + _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; + _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && + (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + _global_pos.yaw = _local_pos.yaw; - _global_pos.timestamp = _local_pos.timestamp; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; - /* lazily publish the global position only once available */ - if (_global_pos_pub > 0) { - /* publish the global position */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the global position */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } } if (hrt_elapsed_time(&_wind.timestamp) > 99000) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index b17f1e6ee..5de9d4c5a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -312,7 +312,7 @@ public: static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); + static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); -- cgit v1.2.3 From 0cbfa65056bb4716ddaf98783a844a7bff2dd8ee Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 11 Feb 2015 17:57:33 +0100 Subject: AttPosEKF: Refactor and code cleanup --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 6 +- src/drivers/gps/gps.cpp | 159 ++++- .../ekf_att_pos_estimator_main.cpp | 705 ++++++++++++--------- 3 files changed, 527 insertions(+), 343 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 2ecc104df..66eae6255 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -4,9 +4,9 @@ # att & pos estimator, att & pos control. # -attitude_estimator_ekf start -#ekf_att_pos_estimator start -position_estimator_inav start +#attitude_estimator_ekf start +ekf_att_pos_estimator start +#position_estimator_inav start if mc_att_control start then diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index f9595a734..2bbc6c916 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -81,6 +81,12 @@ #endif static const int ERROR = -1; +//DEBUG BEGIN + #include +static int sp_man_sub = -1; +static struct manual_control_setpoint_s sp_man; +//DEBUG END + /* class for dynamic allocation of satellite info data */ class GPS_Sat_Info { @@ -162,7 +168,7 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]); namespace { -GPS *g_dev; +GPS *g_dev = nullptr; } @@ -271,6 +277,27 @@ GPS::task_main_trampoline(void *arg) g_dev->task_main(); } +static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + // check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } + + if (!newData) { + return false; + } + + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} + + void GPS::task_main() { @@ -288,31 +315,62 @@ GPS::task_main() uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; + //DEBUG BEGIN + sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + memset(&sp_man, 0, sizeof(sp_man)); + //DEBUG END + /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_fake_gps) { - _report_gps_pos.timestamp_position = hrt_absolute_time(); - _report_gps_pos.lat = (int32_t)47.378301e7f; - _report_gps_pos.lon = (int32_t)8.538777e7f; - _report_gps_pos.alt = (int32_t)1200e3f; - _report_gps_pos.timestamp_variance = hrt_absolute_time(); - _report_gps_pos.s_variance_m_s = 10.0f; - _report_gps_pos.c_variance_rad = 0.1f; - _report_gps_pos.fix_type = 3; - _report_gps_pos.eph = 0.9f; - _report_gps_pos.epv = 1.8f; - _report_gps_pos.timestamp_velocity = hrt_absolute_time(); - _report_gps_pos.vel_n_m_s = 0.0f; - _report_gps_pos.vel_e_m_s = 0.0f; - _report_gps_pos.vel_d_m_s = 0.0f; - _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); - _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.vel_ned_valid = true; + //DEBUG BEGIN: Disable GPS using aux1 + orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) { + _report_gps_pos.fix_type = 0; + _report_gps_pos.satellites_used = 0; + + //Don't modify Lat/Lon/AMSL + + _report_gps_pos.eph = (float)0xFFFF; + _report_gps_pos.epv = (float)0xFFFF; + _report_gps_pos.s_variance_m_s = (float)0xFFFF; + + _report_gps_pos.vel_m_s = 0.0f; + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_ned_valid = false; + + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.c_variance_rad = (float)0xFFFF; + } + //DEBUG END + + else { + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = (int32_t)47.378301e7f; + _report_gps_pos.lon = (int32_t)8.538777e7f; + _report_gps_pos.alt = (int32_t)1200e3f; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.s_variance_m_s = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph = 0.9f; + _report_gps_pos.epv = 1.8f; + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.vel_ned_valid = true; + } //no time and satellite information simulated + if (!(_pub_blocked)) { if (_report_gps_pos_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); @@ -364,6 +422,29 @@ GPS::task_main() if (!(_pub_blocked)) { if (helper_ret & 1) { + + //DEBUG BEGIN: Disable GPS using aux1 + orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) { + _report_gps_pos.fix_type = 0; + _report_gps_pos.satellites_used = 0; + + //Don't modify Lat/Lon/AMSL + + _report_gps_pos.eph = (float)0xFFFF; + _report_gps_pos.epv = (float)0xFFFF; + _report_gps_pos.s_variance_m_s = (float)0xFFFF; + + _report_gps_pos.vel_m_s = 0.0f; + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_ned_valid = false; + + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.c_variance_rad = (float)0xFFFF; + } + if (_report_gps_pos_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); @@ -478,25 +559,35 @@ GPS::cmd_reset() void GPS::print_info() { - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - warnx("protocol: UBX"); - break; + //GPS Mode + if(_fake_gps) { + warnx("protocol: SIMULATED"); + } - case GPS_DRIVER_MODE_MTK: - warnx("protocol: MTK"); - break; + else { + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + warnx("protocol: UBX"); + break; + + case GPS_DRIVER_MODE_MTK: + warnx("protocol: MTK"); + break; case GPS_DRIVER_MODE_ASHTECH: warnx("protocol: ASHTECH"); break; - default: - break; + default: + break; + } } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); + warnx("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, @@ -520,7 +611,7 @@ GPS::print_info() namespace gps { -GPS *g_dev; +GPS *g_dev = nullptr; void start(const char *uart_path, bool fake_gps, bool enable_sat_info); void stop(); @@ -664,6 +755,14 @@ gps_main(int argc, char *argv[]) gps::start(device_name, fake_gps, enable_sat_info); } + if (!strcmp(argv[1], "fake")) { + if(g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + gps::start(GPS_DEFAULT_UART_PORT, true, false); + } + if (!strcmp(argv[1], "stop")) gps::stop(); @@ -686,5 +785,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'fake', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]"); } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index fbd695eac..2e1750b9e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -103,6 +103,8 @@ static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds +static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure + uint32_t millis() { return IMUmsec; @@ -169,7 +171,6 @@ public: int set_debuglevel(unsigned debug) { _debug = debug; return 0; } private: - bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ int _estimator_task; /**< task handle for sensor task */ @@ -185,8 +186,8 @@ private: int _baro_sub; /**< barometer subscription */ int _gps_sub; /**< GPS subscription */ int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_control_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; int _home_sub; /**< home position as defined by commander / user */ @@ -230,26 +231,28 @@ private: perf_counter_t _perf_mag; ///= 3 && _gps.eph < ephRequired) { + _gpsIsGood = true; + } + else { + _gpsIsGood = false; + } - _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold)); + if (_gpsIsGood) { + _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - _lastGPSTimestamp) / 1e6f, 0.01f, POS_RESET_THRESHOLD)); /* fuse GPS updates */ @@ -1103,9 +1141,9 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->posNE[1] = posNED[1]; // update LPF - _gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt); + _gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); - //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); + //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); @@ -1122,18 +1160,24 @@ void AttitudePositionEstimatorEKF::task_main() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); /* check if we had a GPS outage for a long time */ - /* timeout of 5 seconds */ - if (gps_elapsed > pos_reset_threshold) { + if (dtGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); _ekf->ResetVelocity(); _ekf->ResetStoredStates(); } newDataGps = true; - last_gps = _gps.timestamp_position; - + _lastGPSTimestamp = _gps.timestamp_position; } - + else { + //Too poor GPS fix to use + newDataGps = false; + } + } + // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, + // then something is very wrong with the GPS (possibly a hardware failure or comlink error) + else if(dtGoodGPS > POS_RESET_THRESHOLD) { + _gpsIsGood = false; } bool baro_updated; @@ -1276,7 +1320,7 @@ void AttitudePositionEstimatorEKF::task_main() // } /* Initialize the filter first */ - if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { + if (!_gps_initialized && _gpsIsGood && _gps.epv < _parameters.pos_stddev_threshold) { // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; @@ -1344,289 +1388,330 @@ void AttitudePositionEstimatorEKF::task_main() // We're apparently initialized in this case now // check (and reset the filter as needed) int check = check_filter_state(); - if (check) { // Let the system re-initialize itself continue; } - // Run the strapdown INS equations every IMU update - _ekf->UpdateStrapdownEquationsNED(); - - // store the predicted states for subsequent use by measurement fusion - _ekf->StoreStates(IMUmsec); - // Check if on ground - status is used by covariance prediction - _ekf->OnGroundCheck(); - // sum delta angles and time used by covariance prediction - _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; - _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; - dt += _ekf->dtIMU; - - // perform a covariance prediction if the total delta angle has exceeded the limit - // or the time limit will be exceeded at the next IMU update - if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { - _ekf->CovariancePrediction(dt); - _ekf->summedDelAng.zero(); - _ekf->summedDelVel.zero(); - dt = 0.0f; - } + //Run EKF data fusion steps + updateSensorFusion(newDataGps, newDataMag, newRangeData, newHgtData, newAdsData); + + //Publish attitude estimations + publishAttitude(); - // Fuse GPS Measurements - if (newDataGps && _gps_initialized) { - // Convert GPS measurements to Pos NE, hgt and Vel NED - - float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; - - // Calculate acceleration predicted by GPS velocity change - if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { - - _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; - _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; - _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; - } - - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - - // run the fusion step - _ekf->FuseVelposNED(); - - } else if (!_gps_initialized) { - - // force static mode - _ekf->staticMode = true; - - // Convert GPS measurements to Pos NE, hgt and Vel NED - _ekf->velNED[0] = 0.0f; - _ekf->velNED[1] = 0.0f; - _ekf->velNED[2] = 0.0f; - - _ekf->posNE[0] = 0.0f; - _ekf->posNE[1] = 0.0f; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else { - _ekf->fuseVelData = false; - _ekf->fusePosData = false; + //Publish Local Position estimations + publishLocalPosition(); + + //Publish Global Position, but only if it's any good + if(_gps_initialized && _gpsIsGood) + { + publishGlobalPosition(); } - if (newHgtData) { - // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); - _ekf->fuseHgtData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else { - _ekf->fuseHgtData = false; + //Publish wind estimates + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + publishWindEstimate(); } + } + } - // Fuse Magnetometer Measurements - if (newDataMag) { - _ekf->fuseMagData = true; - _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + } - _ekf->magstate.obsIndex = 0; - _ekf->FuseMagnetometer(); - _ekf->FuseMagnetometer(); - _ekf->FuseMagnetometer(); + perf_end(_loop_perf); + } - } else { - _ekf->fuseMagData = false; - } + _task_running = false; - // Fuse Airspeed Measurements - if (newAdsData && _ekf->VtasMeas > 7.0f) { - _ekf->fuseVtasData = true; - _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - _ekf->FuseAirspeed(); + warnx("exiting.\n"); - } else { - _ekf->fuseVtasData = false; - } + _estimator_task = -1; + _exit(0); +} - if (newRangeData) { +void AttitudePositionEstimatorEKF::publishAttitude() +{ + // Output results + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); - if (_ekf->Tnb.z.z > 0.9f) { - // _ekf->rngMea is set in sensor readout already - _ekf->fuseRngData = true; - _ekf->fuseOptFlowData = false; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); - _ekf->OpticalFlowEKF(); - _ekf->fuseRngData = false; - } - } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + PX4_R(_att.R, i, j) = R(i, j); + } + } + _att.timestamp = _last_sensor_timestamp; + _att.q[0] = _ekf->states[0]; + _att.q[1] = _ekf->states[1]; + _att.q[2] = _ekf->states[2]; + _att.q[3] = _ekf->states[3]; + _att.q_valid = true; + _att.R_valid = true; + + _att.timestamp = _last_sensor_timestamp; + _att.roll = euler(0); + _att.pitch = euler(1); + _att.yaw = euler(2); + + _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; + + // gyro offsets + _att.rate_offsets[0] = _ekf->states[10]; + _att.rate_offsets[1] = _ekf->states[11]; + _att.rate_offsets[2] = _ekf->states[12]; + + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - // Output results - math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); - math::Matrix<3, 3> R = q.to_dcm(); - math::Vector<3> euler = R.to_euler(); - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - PX4_R(_att.R, i, j) = R(i, j); - - _att.timestamp = _last_sensor_timestamp; - _att.q[0] = _ekf->states[0]; - _att.q[1] = _ekf->states[1]; - _att.q[2] = _ekf->states[2]; - _att.q[3] = _ekf->states[3]; - _att.q_valid = true; - _att.R_valid = true; - - _att.timestamp = _last_sensor_timestamp; - _att.roll = euler(0); - _att.pitch = euler(1); - _att.yaw = euler(2); - - _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; - // gyro offsets - _att.rate_offsets[0] = _ekf->states[10]; - _att.rate_offsets[1] = _ekf->states[11]; - _att.rate_offsets[2] = _ekf->states[12]; - - /* lazily publish the attitude only once available */ - if (_att_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - - } else { - /* advertise and publish */ - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - } + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } +} - //Publish position estimations - _local_pos.timestamp = _last_sensor_timestamp; - _local_pos.x = _ekf->states[7]; - _local_pos.y = _ekf->states[8]; - - // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset; - - _local_pos.vx = _ekf->states[4]; - _local_pos.vy = _ekf->states[5]; - _local_pos.vz = _ekf->states[6]; - - _local_pos.xy_valid = _gps_initialized && _gps.fix_type >= 3; - _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized && _gps.fix_type >= 3; - _local_pos.v_z_valid = true; - _local_pos.xy_global = _gps_initialized; - - _local_pos.z_global = false; - _local_pos.yaw = _att.yaw; - - /* lazily publish the local position only once available */ - if (_local_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); - - } else { - /* advertise and publish */ - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); - } +void AttitudePositionEstimatorEKF::publishLocalPosition() +{ + _local_pos.timestamp = _last_sensor_timestamp; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; - //Publish Global Position, but only if it's any good - if(_gps_initialized && _gps.fix_type >= 3 && _gps.eph < _parameters.pos_stddev_threshold * 2.0f) - { - _global_pos.timestamp = _local_pos.timestamp; - - if (_local_pos.xy_global) { - double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); - _global_pos.lat = est_lat; - _global_pos.lon = est_lon; - _global_pos.time_utc_usec = _gps.time_utc_usec; - } - - if (_local_pos.v_xy_valid) { - _global_pos.vel_n = _local_pos.vx; - _global_pos.vel_e = _local_pos.vy; - } else { - _global_pos.vel_n = 0.0f; - _global_pos.vel_e = 0.0f; - } - - /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; - - if (_local_pos.v_z_valid) { - _global_pos.vel_d = _local_pos.vz; - } - - /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); - - _global_pos.yaw = _local_pos.yaw; - - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; - - /* lazily publish the global position only once available */ - if (_global_pos_pub > 0) { - /* publish the global position */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); - - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); - } - } + // XXX need to announce change of Z reference somehow elegantly + _local_pos.z = _ekf->states[9] - _baro_ref_offset; - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - // XXX we need to do something smart about the covariance here - // but we default to the estimate covariance for now - _wind.covariance_north = _ekf->P[14][14]; - _wind.covariance_east = _ekf->P[15][15]; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - } - } + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; - } + _local_pos.xy_valid = _gps_initialized && _gps.fix_type >= 3; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = _gps_initialized && _gps.fix_type >= 3; + _local_pos.v_z_valid = true; + _local_pos.xy_global = _gps_initialized; + + _local_pos.z_global = false; + _local_pos.yaw = _att.yaw; + + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } +} + +void AttitudePositionEstimatorEKF::publishGlobalPosition() +{ + _global_pos.timestamp = _local_pos.timestamp; + + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_utc_usec = _gps.time_utc_usec; + } + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } + + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; + + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } + + /* terrain altitude */ + _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; + _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && + (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + + _global_pos.yaw = _local_pos.yaw; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the global position */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } +} + +void AttitudePositionEstimatorEKF::publishWindEstimate() +{ + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + // XXX we need to do something smart about the covariance here + // but we default to the estimate covariance for now + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + +} + +void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, + const bool fuseBaro, const bool fuseAirSpeed) +{ + // Run the strapdown INS equations every IMU update + _ekf->UpdateStrapdownEquationsNED(); + + // store the predicted states for subsequent use by measurement fusion + _ekf->StoreStates(IMUmsec); + + // Check if on ground - status is used by covariance prediction + _ekf->OnGroundCheck(); + + // sum delta angles and time used by covariance prediction + _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; + _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; + _covariancePredictionDt += _ekf->dtIMU; + + // perform a covariance prediction if the total delta angle has exceeded the limit + // or the time limit will be exceeded at the next IMU update + if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) + || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { + _ekf->CovariancePrediction(_covariancePredictionDt); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); + _covariancePredictionDt = 0.0f; + } + + // Fuse GPS Measurements + if (fuseGPS && _gps_initialized) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + + float gps_dt = (_gps.timestamp_position - _lastGPSTimestamp) / 1e6f; + + // Calculate acceleration predicted by GPS velocity change + if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || + (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || + (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { + + _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; + _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; + _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; } - perf_end(_loop_perf); + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; + + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + + // run the fusion step + _ekf->FuseVelposNED(); + + } + else if (!_gps_initialized) { + + // force static mode + _ekf->staticMode = true; + + // Convert GPS measurements to Pos NE, hgt and Vel NED + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; + + _ekf->posNE[0] = 0.0f; + _ekf->posNE[1] = 0.0f; + + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; + + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + + // run the fusion step + _ekf->FuseVelposNED(); + + } + else { + _ekf->fuseVelData = false; + _ekf->fusePosData = false; } - _task_running = false; + if (fuseBaro) { + // Could use a blend of GPS and baro alt data if desired + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); + + } + else { + _ekf->fuseHgtData = false; + } - warnx("exiting.\n"); + // Fuse Magnetometer Measurements + if (fuseMag) { + _ekf->fuseMagData = true; + _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data - _estimator_task = -1; - _exit(0); + _ekf->magstate.obsIndex = 0; + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + + } + else { + _ekf->fuseMagData = false; + } + + // Fuse Airspeed Measurements + if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) { + _ekf->fuseVtasData = true; + _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + _ekf->FuseAirspeed(); + + } + else { + _ekf->fuseVtasData = false; + } + + // Fuse Rangefinder Measurements + if (fuseRangeSensor) { + if (_ekf->Tnb.z.z > 0.9f) { + // _ekf->rngMea is set in sensor readout already + _ekf->fuseRngData = true; + _ekf->fuseOptFlowData = false; + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->OpticalFlowEKF(); + _ekf->fuseRngData = false; + } + } } int AttitudePositionEstimatorEKF::start() -- cgit v1.2.3 From f5534dd5c195f80354d0ae24802ff4d796633623 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Feb 2015 10:58:36 +0100 Subject: AttPosEKF: Fix velNED not initialized properly on first GPS fix --- .../ekf_att_pos_estimator_main.cpp | 74 ++++++++++++---------- 1 file changed, 42 insertions(+), 32 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 2e1750b9e..bcd6e7fff 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -37,6 +37,7 @@ * * @author Paul Riseborough * @author Lorenz Meier + * @author Johan Jansen */ #include @@ -88,6 +89,13 @@ #include "estimator_22states.h" +static uint64_t IMUmsec = 0; +static uint64_t IMUusec = 0; + +//Constants +static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds +static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure + /** * estimator app start / stop handling function * @@ -99,12 +107,6 @@ __EXPORT uint32_t millis(); __EXPORT uint64_t getMicros(); -static uint64_t IMUmsec = 0; -static uint64_t IMUusec = 0; -static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds - -static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure - uint32_t millis() { return IMUmsec; @@ -137,38 +139,38 @@ public: * * @return OK on success. */ - int start(); + int start(); /** * Task status * * @return true if the mainloop is running */ - bool task_running() { return _task_running; } + bool task_running() { return _task_running; } /** * Print the current status. */ - void print_status(); + void print_status(); /** * Trip the filter by feeding it NaN values. */ - int trip_nan(); + int trip_nan(); /** * Enable logging. * * @param enable Set to true to enable logging, false to disable */ - int enable_logging(bool enable); + int enable_logging(bool enable); /** * Set debug level. * * @param debug Desired debug level - 0 to disable. */ - int set_debuglevel(unsigned debug) { _debug = debug; return 0; } + int set_debuglevel(unsigned debug) { _debug = debug; return 0; } private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -198,15 +200,15 @@ private: orb_advert_t _wind_pub; /**< wind estimate */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct gyro_report _gyro; - struct accel_report _accel; - struct mag_report _mag; - struct airspeed_s _airspeed; /**< airspeed */ - struct baro_report _baro; /**< baro readings */ + struct gyro_report _gyro; + struct accel_report _accel; + struct mag_report _mag; + struct airspeed_s _airspeed; /**< airspeed */ + struct baro_report _baro; /**< baro readings */ struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct vehicle_local_position_s _local_pos; /**< local vehicle position */ - struct vehicle_gps_position_s _gps; /**< GPS position */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_local_position_s _local_pos; /**< local vehicle position */ + struct vehicle_gps_position_s _gps; /**< GPS position */ struct wind_estimate_s _wind; /**< wind estimate */ struct range_finder_report _distance; /**< distance estimate */ @@ -252,7 +254,7 @@ private: bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 - int _mavlink_fd; + int _mavlink_fd; struct { int32_t vel_delay_ms; @@ -819,8 +821,9 @@ void AttitudePositionEstimatorEKF::task_main() _gps.vel_d_m_s = 0.0f; // init lowpass filters for baro and gps altitude - float _gps_alt_filt = 0, _baro_alt_filt = 0; - float rc = 10.0f; // RC time constant of 1st order LPF in seconds + float _gps_alt_filt = 0; + float _baro_alt_filt = 0; + const float rc = 10.0f; // RC time constant of 1st order LPF in seconds hrt_abstime baro_last = 0; _task_running = true; @@ -1105,13 +1108,13 @@ void AttitudePositionEstimatorEKF::task_main() perf_count(_perf_gps); //We are more strict for our first fix - float ephRequired = _parameters.pos_stddev_threshold; + float requiredAccuracy = _parameters.pos_stddev_threshold; if(_gpsIsGood) { - ephRequired = _parameters.pos_stddev_threshold * 2.0f; + requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f; } //Check if the GPS fix is good enough for us to use - if(_gps.fix_type >= 3 && _gps.eph < ephRequired) { + if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { _gpsIsGood = true; } else { @@ -1299,8 +1302,6 @@ void AttitudePositionEstimatorEKF::task_main() if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { - float initVelNED[3]; - // maintain filtered baro and gps altitudes to calculate weather offset // baro sample rate is ~70Hz and measurement bandwidth is high // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds @@ -1320,7 +1321,7 @@ void AttitudePositionEstimatorEKF::task_main() // } /* Initialize the filter first */ - if (!_gps_initialized && _gpsIsGood && _gps.epv < _parameters.pos_stddev_threshold) { + if (!_gps_initialized && _gpsIsGood) { // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; @@ -1348,6 +1349,11 @@ void AttitudePositionEstimatorEKF::task_main() // Look up mag declination based on current position float declination = math::radians(get_mag_declination(lat, lon)); + float initVelNED[3]; + initVelNED[0] = _gps.vel_n_m_s; + initVelNED[1] = _gps.vel_e_m_s; + initVelNED[2] = _gps.vel_d_m_s; + _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection @@ -1369,6 +1375,7 @@ void AttitudePositionEstimatorEKF::task_main() _gps_initialized = true; } else if (!_ekf->statesInitialised) { + float initVelNED[3]; initVelNED[0] = 0.0f; initVelNED[1] = 0.0f; @@ -1487,11 +1494,11 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.vy = _ekf->states[5]; _local_pos.vz = _ekf->states[6]; - _local_pos.xy_valid = _gps_initialized && _gps.fix_type >= 3; + _local_pos.xy_valid = _gps_initialized && _gpsIsGood; _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized && _gps.fix_type >= 3; + _local_pos.v_xy_valid = _gps_initialized && _gpsIsGood; _local_pos.v_z_valid = true; - _local_pos.xy_global = _gps_initialized; + _local_pos.xy_global = _gps_initialized; //TODO: Handle optical flow mode here _local_pos.z_global = false; _local_pos.yaw = _att.yaw; @@ -1559,6 +1566,7 @@ void AttitudePositionEstimatorEKF::publishWindEstimate() _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; + // XXX we need to do something smart about the covariance here // but we default to the estimate covariance for now _wind.covariance_north = _ekf->P[14][14]; @@ -1665,8 +1673,10 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // Could use a blend of GPS and baro alt data if desired _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // run the fusion step _ekf->FuseVelposNED(); -- cgit v1.2.3 From 76901c6414ac8612552546aedc194089dc45c510 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Feb 2015 11:49:45 +0100 Subject: AttPosEKF: Moved data collection to separate function --- src/modules/commander/commander.cpp | 5 +- .../ekf_att_pos_estimator_main.cpp | 942 +++++++++++---------- 2 files changed, 484 insertions(+), 463 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b2d9c9da3..242d8a486 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1395,11 +1395,10 @@ int commander_thread_main(int argc, char *argv[]) if(status.condition_global_position_valid) { set_tune_override(TONE_GPS_WARNING_TUNE); status_changed = true; + status.condition_global_position_valid = false; } - - status.condition_global_position_valid = false; } - else if(hrt_absolute_time() > POSITION_TIMEOUT) { + else if(global_position.timestamp != 0) { //Got good global position estimate if(!status.condition_global_position_valid) { status_changed = true; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index bcd6e7fff..ac9f679c2 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -93,6 +93,7 @@ static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; //Constants +static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure @@ -236,6 +237,8 @@ private: perf_counter_t _perf_airspeed; /// 1.0f || deltaT < 0.000001f) { - deltaT = 0.01f; - } - - - // Always store data, independent of init status - /* fill in last data set */ - _ekf->dtIMU = deltaT; - - if (isfinite(_gyro.x) && - isfinite(_gyro.y) && - isfinite(_gyro.z)) { - _ekf->angRate.x = _gyro.x; - _ekf->angRate.y = _gyro.y; - _ekf->angRate.z = _gyro.z; - - if (!_gyro_valid) { - lastAngRate = _ekf->angRate; - } - - _gyro_valid = true; - } - - if (accel_updated) { - _ekf->accel.x = _accel.x; - _ekf->accel.y = _accel.y; - _ekf->accel.z = _accel.z; - - if (!_accel_valid) { - lastAccel = _ekf->accel; - } - - _accel_valid = true; - } - - _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; - _ekf->lastAngRate = angRate; - _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - _ekf->lastAccel = accel; - - -#else - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - - static hrt_abstime last_accel = 0; - static hrt_abstime last_mag = 0; - - if (last_accel != _sensor_combined.accelerometer_timestamp) { - accel_updated = true; - } else { - accel_updated = false; - } - - last_accel = _sensor_combined.accelerometer_timestamp; - - - // Copy gyro and accel - _last_sensor_timestamp = _sensor_combined.timestamp; - IMUmsec = _sensor_combined.timestamp / 1e3; - IMUusec = _sensor_combined.timestamp; - - float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; - - /* guard against too large deltaT's */ - if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { - deltaT = 0.01f; - } - - _last_run = _sensor_combined.timestamp; - - // Always store data, independent of init status - /* fill in last data set */ - _ekf->dtIMU = deltaT; - - int last_gyro_main = _gyro_main; - - if (isfinite(_sensor_combined.gyro_rad_s[0]) && - isfinite(_sensor_combined.gyro_rad_s[1]) && - isfinite(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { - - _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; - _gyro_main = 0; - _gyro_valid = true; - - } else if (isfinite(_sensor_combined.gyro1_rad_s[0]) && - isfinite(_sensor_combined.gyro1_rad_s[1]) && - isfinite(_sensor_combined.gyro1_rad_s[2])) { - - _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro1_rad_s[2]; - _gyro_main = 1; - _gyro_valid = true; - - } else { - _gyro_valid = false; - } - - if (last_gyro_main != _gyro_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main); - } - - if (!_gyro_valid) { - /* keep last value if he hit an out of band value */ - lastAngRate = _ekf->angRate; - } else { - perf_count(_perf_gyro); - } - - if (accel_updated) { - - int last_accel_main = _accel_main; - - /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; - _accel_main = 0; - } else { - _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2]; - _accel_main = 1; - } - - if (!_accel_valid) { - lastAccel = _ekf->accel; - } - - if (last_accel_main != _accel_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main); - } - - _accel_valid = true; - } - - _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; - lastAngRate = _ekf->angRate; - _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; - lastAccel = _ekf->accel; - - if (last_mag != _sensor_combined.magnetometer_timestamp) { - mag_updated = true; - newDataMag = true; - - } else { - newDataMag = false; - } - - last_mag = _sensor_combined.magnetometer_timestamp; - -#endif - - //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); - - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - perf_count(_perf_airspeed); - - _ekf->VtasMeas = _airspeed.true_airspeed_m_s; - newAdsData = true; - - } else { - newAdsData = false; - } - - //Calculate time since last good GPS fix - const float dtGoodGPS = hrt_elapsed_time(&_lastGPSTimestamp) / 1e6f; - - bool gps_updated; - orb_check(_gps_sub, &gps_updated); - - if (gps_updated) { - - orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); - perf_count(_perf_gps); - - //We are more strict for our first fix - float requiredAccuracy = _parameters.pos_stddev_threshold; - if(_gpsIsGood) { - requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f; - } - - //Check if the GPS fix is good enough for us to use - if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { - _gpsIsGood = true; - } - else { - _gpsIsGood = false; - } - - if (_gpsIsGood) { - _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - _lastGPSTimestamp) / 1e6f, 0.01f, POS_RESET_THRESHOLD)); - - /* fuse GPS updates */ - - //_gps.timestamp / 1e3; - _ekf->GPSstatus = _gps.fix_type; - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; - - // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); - - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); - _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - _ekf->gpsHgt = _gps.alt / 1e3f; - - //Convert from global frame to local frame - _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; - - // update LPF - _gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); - - //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); - - // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { - // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); - // } else { - // _ekf->vneSigma = _parameters.velne_noise; - // } - - // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { - // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); - // } else { - // _ekf->posNeSigma = _parameters.posne_noise; - // } - - // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); - - /* check if we had a GPS outage for a long time */ - if (dtGoodGPS > POS_RESET_THRESHOLD) { - _ekf->ResetPosition(); - _ekf->ResetVelocity(); - _ekf->ResetStoredStates(); - } - - newDataGps = true; - _lastGPSTimestamp = _gps.timestamp_position; - } - else { - //Too poor GPS fix to use - newDataGps = false; - } - } - // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, - // then something is very wrong with the GPS (possibly a hardware failure or comlink error) - else if(dtGoodGPS > POS_RESET_THRESHOLD) { - _gpsIsGood = false; - } - - bool baro_updated; - orb_check(_baro_sub, &baro_updated); - - if (baro_updated) { - - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - - float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; - baro_last = _baro.timestamp; - - _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); - - _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); - - if (!_baro_init) { - _baro_ref = _baro.altitude; - _baro_init = true; - warnx("ALT REF INIT"); - } - - perf_count(_perf_baro); - - newHgtData = true; - } else { - newHgtData = false; - } - -#ifndef SENSOR_COMBINED_SUB - orb_check(_mag_sub, &mag_updated); -#endif - - if (mag_updated) { - - _mag_valid = true; - - perf_count(_perf_mag); - -#ifndef SENSOR_COMBINED_SUB - orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); - - // XXX we compensate the offsets upfront - should be close to zero. - // 0.001f - _ekf->magData.x = _mag.x; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - - _ekf->magData.y = _mag.y; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - - _ekf->magData.z = _mag.z; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - -#else - int last_mag_main = _mag_main; - - // XXX we compensate the offsets upfront - should be close to zero. - - /* fail over to the 2nd mag if we know the first is down */ - if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) { - _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - - _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - - _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 0; - } else { - _ekf->magData.x = _sensor_combined.magnetometer1_ga[0]; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - - _ekf->magData.y = _sensor_combined.magnetometer1_ga[1]; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - - _ekf->magData.z = _sensor_combined.magnetometer1_ga[2]; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 1; - } - - if (last_mag_main != _mag_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main); - } -#endif - - newDataMag = true; - - } else { - newDataMag = false; - } - - orb_check(_distance_sub, &newRangeData); - - if (newRangeData) { - orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); - - if (_distance.valid) { - _ekf->rngMea = _distance.distance; - _distance_last_valid = _distance.timestamp; - } else { - newRangeData = false; - } - } + pollData(); /* * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY @@ -1322,61 +949,13 @@ void AttitudePositionEstimatorEKF::task_main() /* Initialize the filter first */ if (!_gps_initialized && _gpsIsGood) { - - // GPS is in scaled integers, convert - double lat = _gps.lat / 1.0e7; - double lon = _gps.lon / 1.0e7; - float gps_alt = _gps.alt / 1e3f; - - // Set up height correctly - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame - - // init filtered gps and baro altitudes - _gps_alt_filt = gps_alt; - _baro_alt_filt = _baro.altitude; - - _ekf->baroHgt = _baro.altitude; - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); - - // Set up position variables correctly - _ekf->GPSstatus = _gps.fix_type; - - _ekf->gpsLat = math::radians(lat); - _ekf->gpsLon = math::radians(lon) - M_PI; - _ekf->gpsHgt = gps_alt; - - // Look up mag declination based on current position - float declination = math::radians(get_mag_declination(lat, lon)); - - float initVelNED[3]; - initVelNED[0] = _gps.vel_n_m_s; - initVelNED[1] = _gps.vel_e_m_s; - initVelNED[2] = _gps.vel_d_m_s; - - _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - - // Initialize projection - _local_pos.ref_lat = lat; - _local_pos.ref_lon = lon; - _local_pos.ref_alt = gps_alt; - _local_pos.ref_timestamp = _gps.timestamp_position; - - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); - - #if 0 - warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, - (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); - warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); - #endif - - _gps_initialized = true; - - } else if (!_ekf->statesInitialised) { + initializeGPS(); + } + else if (!_ekf->statesInitialised) { + // North, East Down position (m) + float posNED[3] = {0.0f, 0.0f, 0.0f}; float initVelNED[3]; - + initVelNED[0] = 0.0f; initVelNED[1] = 0.0f; initVelNED[2] = 0.0f; @@ -1390,7 +969,8 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - } else if (_ekf->statesInitialised) { + } + else if (_ekf->statesInitialised) { // We're apparently initialized in this case now // check (and reset the filter as needed) @@ -1401,7 +981,7 @@ void AttitudePositionEstimatorEKF::task_main() } //Run EKF data fusion steps - updateSensorFusion(newDataGps, newDataMag, newRangeData, newHgtData, newAdsData); + updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData); //Publish attitude estimations publishAttitude(); @@ -1435,6 +1015,60 @@ void AttitudePositionEstimatorEKF::task_main() _exit(0); } +void AttitudePositionEstimatorEKF::initializeGPS() +{ + // GPS is in scaled integers, convert + double lat = _gps.lat / 1.0e7; + double lon = _gps.lon / 1.0e7; + float gps_alt = _gps.alt / 1e3f; + + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame + + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _baro_alt_filt = _baro.altitude; + + _ekf->baroHgt = _baro.altitude; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); + + // Set up position variables correctly + _ekf->GPSstatus = _gps.fix_type; + + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; + + // Look up mag declination based on current position + float declination = math::radians(get_mag_declination(lat, lon)); + + float initVelNED[3]; + initVelNED[0] = _gps.vel_n_m_s; + initVelNED[1] = _gps.vel_e_m_s; + initVelNED[2] = _gps.vel_d_m_s; + + _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); + + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = _gps.timestamp_position; + + map_projection_init(&_pos_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + + #if 0 + warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); + warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); + #endif + + _gps_initialized = true; +} + void AttitudePositionEstimatorEKF::publishAttitude() { // Output results @@ -1798,6 +1432,394 @@ void AttitudePositionEstimatorEKF::print_status() (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } +void AttitudePositionEstimatorEKF::pollData() +{ + static Vector3f lastAngRate; + static Vector3f lastAccel; + bool accel_updated = false; + + //Update Gyro and Accelerometer +#ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + + + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + } + + _last_sensor_timestamp = _gyro.timestamp; + IMUmsec = _gyro.timestamp / 1e3; + IMUusec = _gyro.timestamp; + + float deltaT = (_gyro.timestamp - _last_run) / 1e6f; + _last_run = _gyro.timestamp; + + /* guard against too large deltaT's */ + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { + deltaT = 0.01f; + } + + + // Always store data, independent of init status + /* fill in last data set */ + _ekf->dtIMU = deltaT; + + if (isfinite(_gyro.x) && + isfinite(_gyro.y) && + isfinite(_gyro.z)) { + _ekf->angRate.x = _gyro.x; + _ekf->angRate.y = _gyro.y; + _ekf->angRate.z = _gyro.z; + + if (!_gyro_valid) { + lastAngRate = _ekf->angRate; + } + + _gyro_valid = true; + } + + if (accel_updated) { + _ekf->accel.x = _accel.x; + _ekf->accel.y = _accel.y; + _ekf->accel.z = _accel.z; + + if (!_accel_valid) { + lastAccel = _ekf->accel; + } + + _accel_valid = true; + } + + _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + _ekf->lastAngRate = angRate; + _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + _ekf->lastAccel = accel; + + +#else + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); + + static hrt_abstime last_accel = 0; + static hrt_abstime last_mag = 0; + + if (last_accel != _sensor_combined.accelerometer_timestamp) { + accel_updated = true; + } else { + accel_updated = false; + } + + last_accel = _sensor_combined.accelerometer_timestamp; + + + // Copy gyro and accel + _last_sensor_timestamp = _sensor_combined.timestamp; + IMUmsec = _sensor_combined.timestamp / 1e3; + IMUusec = _sensor_combined.timestamp; + + float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; + + /* guard against too large deltaT's */ + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { + deltaT = 0.01f; + } + + _last_run = _sensor_combined.timestamp; + + // Always store data, independent of init status + /* fill in last data set */ + _ekf->dtIMU = deltaT; + + int last_gyro_main = _gyro_main; + + if (isfinite(_sensor_combined.gyro_rad_s[0]) && + isfinite(_sensor_combined.gyro_rad_s[1]) && + isfinite(_sensor_combined.gyro_rad_s[2]) && + (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { + + _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + _gyro_main = 0; + _gyro_valid = true; + + } else if (isfinite(_sensor_combined.gyro1_rad_s[0]) && + isfinite(_sensor_combined.gyro1_rad_s[1]) && + isfinite(_sensor_combined.gyro1_rad_s[2])) { + + _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro1_rad_s[2]; + _gyro_main = 1; + _gyro_valid = true; + + } else { + _gyro_valid = false; + } + + if (last_gyro_main != _gyro_main) { + mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main); + } + + if (!_gyro_valid) { + /* keep last value if he hit an out of band value */ + lastAngRate = _ekf->angRate; + } else { + perf_count(_perf_gyro); + } + + if (accel_updated) { + + int last_accel_main = _accel_main; + + /* fail over to the 2nd accel if we know the first is down */ + if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + _accel_main = 0; + } else { + _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2]; + _accel_main = 1; + } + + if (!_accel_valid) { + lastAccel = _ekf->accel; + } + + if (last_accel_main != _accel_main) { + mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main); + } + + _accel_valid = true; + } + + _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; + lastAngRate = _ekf->angRate; + _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; + lastAccel = _ekf->accel; + + if (last_mag != _sensor_combined.magnetometer_timestamp) { + _newDataMag = true; + + } else { + _newDataMag = false; + } + + last_mag = _sensor_combined.magnetometer_timestamp; + +#endif + + //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + + //Update AirSpeed + orb_check(_airspeed_sub, &_newAdsData); + if (_newAdsData) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + perf_count(_perf_airspeed); + + _ekf->VtasMeas = _airspeed.true_airspeed_m_s; + } + + //Calculate time since last good GPS fix + const float dtGoodGPS = hrt_elapsed_time(&_lastGPSTimestamp) / 1e6f; + + orb_check(_gps_sub, &_newDataGps); + if (_newDataGps) { + + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); + perf_count(_perf_gps); + + //We are more strict for our first fix + float requiredAccuracy = _parameters.pos_stddev_threshold; + if(_gpsIsGood) { + requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f; + } + + //Check if the GPS fix is good enough for us to use + if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { + _gpsIsGood = true; + } + else { + _gpsIsGood = false; + } + + if (_gpsIsGood) { + _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - _lastGPSTimestamp) / 1e6f, 0.01f, POS_RESET_THRESHOLD)); + + /* fuse GPS updates */ + + //_gps.timestamp / 1e3; + _ekf->GPSstatus = _gps.fix_type; + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + + // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); + + _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; + + //check if we had a GPS outage for a long time + if(_gps_initialized) { + + //Convert from global frame to local frame + float posNED[3] = {0.0f, 0.0f, 0.0f}; + _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; + + if (dtGoodGPS > POS_RESET_THRESHOLD) { + _ekf->ResetPosition(); + _ekf->ResetVelocity(); + _ekf->ResetStoredStates(); + } + } + + // update LPF + _gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + + //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); + + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { + // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); + // } else { + // _ekf->vneSigma = _parameters.velne_noise; + // } + + // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // } else { + // _ekf->posNeSigma = _parameters.posne_noise; + // } + + // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + + _lastGPSTimestamp = _gps.timestamp_position; + } + else { + //Too poor GPS fix to use + _newDataGps = false; + } + } + + // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, + // then something is very wrong with the GPS (possibly a hardware failure or comlink error) + else if(dtGoodGPS > POS_RESET_THRESHOLD) { + _gpsIsGood = false; + } + + //Update barometer + orb_check(_baro_sub, &_newHgtData); + + if (_newHgtData) { + static hrt_abstime baro_last = 0; + + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + // init lowpass filters for baro and gps altitude + float baro_elapsed; + if(baro_last == 0) { + baro_elapsed = 0.0f; + } + else { + baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; + } + baro_last = _baro.timestamp; + + _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); + + _ekf->baroHgt = _baro.altitude; + _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + + if (!_baro_init) { + _baro_ref = _baro.altitude; + _baro_init = true; + warnx("ALT REF INIT"); + } + + perf_count(_perf_baro); + + _newHgtData = true; + } + + //Update Magnetometer +#ifndef SENSOR_COMBINED_SUB + orb_check(_mag_sub, &_newDataMag); +#endif + + if (_newDataMag) { + + _mag_valid = true; + + perf_count(_perf_mag); + +#ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + + // XXX we compensate the offsets upfront - should be close to zero. + // 0.001f + _ekf->magData.x = _mag.x; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + + _ekf->magData.y = _mag.y; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + + _ekf->magData.z = _mag.z; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + +#else + int last_mag_main = _mag_main; + + // XXX we compensate the offsets upfront - should be close to zero. + + /* fail over to the 2nd mag if we know the first is down */ + if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) { + _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + + _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + + _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + _mag_main = 0; + } else { + _ekf->magData.x = _sensor_combined.magnetometer1_ga[0]; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + + _ekf->magData.y = _sensor_combined.magnetometer1_ga[1]; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + + _ekf->magData.z = _sensor_combined.magnetometer1_ga[2]; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + _mag_main = 1; + } + + if (last_mag_main != _mag_main) { + mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main); + } +#endif + } + + //Update range data + orb_check(_distance_sub, &_newRangeData); + if (_newRangeData) { + orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); + + if (_distance.valid) { + _ekf->rngMea = _distance.distance; + _distance_last_valid = _distance.timestamp; + } else { + _newRangeData = false; + } + } +} + int AttitudePositionEstimatorEKF::trip_nan() { int ret = 0; -- cgit v1.2.3 From 755abccb3ea22ecf6f919e0b5279036f075702d8 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Feb 2015 13:00:20 +0100 Subject: AttPosEKF: Removed SENSOR_COMBINED_SUB macros --- .../ekf_att_pos_estimator_main.cpp | 126 +-------------------- 1 file changed, 2 insertions(+), 124 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index ac9f679c2..936092195 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -52,17 +52,13 @@ #include #include -#define SENSOR_COMBINED_SUB - #include #include #include #include #include #include -#ifdef SENSOR_COMBINED_SUB #include -#endif #include #include #include @@ -177,13 +173,8 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ int _estimator_task; /**< task handle for sensor task */ -#ifndef SENSOR_COMBINED_SUB - int _gyro_sub; /**< gyro sensor subscription */ - int _accel_sub; /**< accel sensor subscription */ - int _mag_sub; /**< mag sensor subscription */ -#else + int _sensor_combined_sub; -#endif int _distance_sub; /**< distance measurement */ int _airspeed_sub; /**< airspeed subscription */ int _baro_sub; /**< barometer subscription */ @@ -217,9 +208,7 @@ private: struct accel_scale _accel_offsets[3]; struct mag_scale _mag_offsets[3]; -#ifdef SENSOR_COMBINED_SUB struct sensor_combined_s _sensor_combined; -#endif struct map_projection_reference_s _pos_ref; @@ -405,13 +394,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _estimator_task(-1), /* subscriptions */ -#ifndef SENSOR_COMBINED_SUB - _gyro_sub(-1), - _accel_sub(-1), - _mag_sub(-1), -#else _sensor_combined_sub(-1), -#endif _distance_sub(-1), _airspeed_sub(-1), _baro_sub(-1), @@ -446,9 +429,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _accel_offsets({}), _mag_offsets({}), -#ifdef SENSOR_COMBINED_SUB _sensor_combined{}, -#endif _pos_ref{}, _baro_ref(0.0f), @@ -802,20 +783,9 @@ void AttitudePositionEstimatorEKF::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); -#ifndef SENSOR_COMBINED_SUB - - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); - - /* rate limit gyro updates to 50 Hz */ - /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_gyro_sub, 4); -#else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_sensor_combined_sub, 9); -#endif /* sets also parameters in the EKF object */ parameters_update(); @@ -826,13 +796,9 @@ void AttitudePositionEstimatorEKF::task_main() /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; -#ifndef SENSOR_COMBINED_SUB - fds[1].fd = _gyro_sub; - fds[1].events = POLLIN; -#else + fds[1].fd = _sensor_combined_sub; fds[1].events = POLLIN; -#endif _gps.vel_n_m_s = 0.0f; _gps.vel_e_m_s = 0.0f; @@ -882,14 +848,8 @@ void AttitudePositionEstimatorEKF::task_main() /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); -#ifndef SENSOR_COMBINED_SUB - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); - orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); -#else /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); -#endif /* set sensors to de-initialized state */ _gyro_valid = false; @@ -1439,66 +1399,6 @@ void AttitudePositionEstimatorEKF::pollData() bool accel_updated = false; //Update Gyro and Accelerometer -#ifndef SENSOR_COMBINED_SUB - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); - - - orb_check(_accel_sub, &accel_updated); - - if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); - } - - _last_sensor_timestamp = _gyro.timestamp; - IMUmsec = _gyro.timestamp / 1e3; - IMUusec = _gyro.timestamp; - - float deltaT = (_gyro.timestamp - _last_run) / 1e6f; - _last_run = _gyro.timestamp; - - /* guard against too large deltaT's */ - if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { - deltaT = 0.01f; - } - - - // Always store data, independent of init status - /* fill in last data set */ - _ekf->dtIMU = deltaT; - - if (isfinite(_gyro.x) && - isfinite(_gyro.y) && - isfinite(_gyro.z)) { - _ekf->angRate.x = _gyro.x; - _ekf->angRate.y = _gyro.y; - _ekf->angRate.z = _gyro.z; - - if (!_gyro_valid) { - lastAngRate = _ekf->angRate; - } - - _gyro_valid = true; - } - - if (accel_updated) { - _ekf->accel.x = _accel.x; - _ekf->accel.y = _accel.y; - _ekf->accel.z = _accel.z; - - if (!_accel_valid) { - lastAccel = _ekf->accel; - } - - _accel_valid = true; - } - - _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; - _ekf->lastAngRate = angRate; - _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - _ekf->lastAccel = accel; - - -#else orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); static hrt_abstime last_accel = 0; @@ -1611,8 +1511,6 @@ void AttitudePositionEstimatorEKF::pollData() last_mag = _sensor_combined.magnetometer_timestamp; -#endif - //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); //Update AirSpeed @@ -1748,31 +1646,12 @@ void AttitudePositionEstimatorEKF::pollData() } //Update Magnetometer -#ifndef SENSOR_COMBINED_SUB - orb_check(_mag_sub, &_newDataMag); -#endif - if (_newDataMag) { _mag_valid = true; perf_count(_perf_mag); -#ifndef SENSOR_COMBINED_SUB - orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); - - // XXX we compensate the offsets upfront - should be close to zero. - // 0.001f - _ekf->magData.x = _mag.x; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - - _ekf->magData.y = _mag.y; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - - _ekf->magData.z = _mag.z; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - -#else int last_mag_main = _mag_main; // XXX we compensate the offsets upfront - should be close to zero. @@ -1803,7 +1682,6 @@ void AttitudePositionEstimatorEKF::pollData() if (last_mag_main != _mag_main) { mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main); } -#endif } //Update range data -- cgit v1.2.3 From a8c298c77260b496b2b04ba9348ce351086c68c1 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Feb 2015 13:55:03 +0100 Subject: AttPosEKF: Remove unused gps accel estimation --- src/drivers/gps/gps.cpp | 3 ++ .../ekf_att_pos_estimator_main.cpp | 35 ++++++++++------------ .../ekf_att_pos_estimator/estimator_22states.cpp | 1 - .../ekf_att_pos_estimator/estimator_22states.h | 1 - 4 files changed, 18 insertions(+), 22 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 2bbc6c916..1a190bf40 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -328,6 +328,9 @@ GPS::task_main() //DEBUG BEGIN: Disable GPS using aux1 orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) { + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.fix_type = 0; _report_gps_pos.satellites_used = 0; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 936092195..8ec7c43a1 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -230,7 +230,7 @@ private: float _baro_alt_filt; float _covariancePredictionDt; ///< time lapsed since last covariance prediction bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use - uint64_t _lastGPSTimestamp; ///< Timestamp of last good GPS fix we have received + uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; bool _gps_initialized; hrt_abstime _filter_start_time; @@ -372,6 +372,11 @@ private: **/ void initializeGPS(); + /** + * @brief + * Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate + * flags to true (e.g newDataGps) + **/ void pollData(); }; @@ -451,7 +456,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _baro_alt_filt(0.0f), _covariancePredictionDt(0.0f), _gpsIsGood(false), - _lastGPSTimestamp(0), + _previousGPSTimestamp(0), _baro_init(false), _gps_initialized(false), _filter_start_time(0), @@ -1209,18 +1214,6 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseGPS && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED - float gps_dt = (_gps.timestamp_position - _lastGPSTimestamp) / 1e6f; - - // Calculate acceleration predicted by GPS velocity change - if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { - - _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; - _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; - _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; - } - // set fusion flags _ekf->fuseVelData = true; _ekf->fusePosData = true; @@ -1522,8 +1515,6 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->VtasMeas = _airspeed.true_airspeed_m_s; } - //Calculate time since last good GPS fix - const float dtGoodGPS = hrt_elapsed_time(&_lastGPSTimestamp) / 1e6f; orb_check(_gps_sub, &_newDataGps); if (_newDataGps) { @@ -1546,7 +1537,11 @@ void AttitudePositionEstimatorEKF::pollData() } if (_gpsIsGood) { - _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - _lastGPSTimestamp) / 1e6f, 0.01f, POS_RESET_THRESHOLD)); + + //Calculate time since last good GPS fix + const float dtGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + + _ekf->updateDtGpsFilt(math::constrain(dtGoodGPS, 0.01f, POS_RESET_THRESHOLD)); /* fuse GPS updates */ @@ -1569,7 +1564,7 @@ void AttitudePositionEstimatorEKF::pollData() float posNED[3] = {0.0f, 0.0f, 0.0f}; _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; + _ekf->posNE[1] = posNED[1]; if (dtGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); @@ -1597,7 +1592,7 @@ void AttitudePositionEstimatorEKF::pollData() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); - _lastGPSTimestamp = _gps.timestamp_position; + _previousGPSTimestamp = _gps.timestamp_position; } else { //Too poor GPS fix to use @@ -1607,7 +1602,7 @@ void AttitudePositionEstimatorEKF::pollData() // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, // then something is very wrong with the GPS (possibly a hardware failure or comlink error) - else if(dtGoodGPS > POS_RESET_THRESHOLD) { + else if( (static_cast(hrt_elapsed_time(&_gps.timestamp_position)) / 1e6f) > POS_RESET_THRESHOLD) { _gpsIsGood = false; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 47706c37b..e912e699e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -111,7 +111,6 @@ AttPosEKF::AttPosEKF() : innovVelPos{}, varInnovVelPos{}, velNED{}, - accelGPSNED{}, posNE{}, hgtMea(0.0f), baroHgtOffset(0.0f), diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 5de9d4c5a..19ef52145 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -161,7 +161,6 @@ public: float varInnovVelPos[6]; // innovation variance output float velNED[3]; // North, East, Down velocity obs (m/s) - float accelGPSNED[3]; // Acceleration predicted by GPS in earth frame float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) float baroHgtOffset; ///< the baro (weather) offset from normalized altitude -- cgit v1.2.3 From 6af2a38f3621afb57ec33e4298e4399fb567e25a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Feb 2015 14:43:23 +0100 Subject: AttPosEKF: Moved class declaration to header file --- .../AttitudePositionEstimatorEKF.h | 338 +++++++++++++++++++++ .../ekf_att_pos_estimator_main.cpp | 294 +----------------- .../ekf_att_pos_estimator/estimator_22states.cpp | 4 +- 3 files changed, 343 insertions(+), 293 deletions(-) create mode 100644 src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h new file mode 100644 index 000000000..07db924b2 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -0,0 +1,338 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file AttitudePositionEstimatorEKF.h + * Implementation of the attitude and position estimator. This is a PX4 wrapper around + * the EKF IntertialNav filter of Paul Riseborough (@see estimator_22states.cpp) + * + * @author Paul Riseborough + * @author Lorenz Meier + * @author Johan Jansen + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +//Forward declaration +class AttPosEKF; + +class AttitudePositionEstimatorEKF +{ +public: + /** + * Constructor + */ + AttitudePositionEstimatorEKF(); + + /* we do not want people ever copying this class */ + AttitudePositionEstimatorEKF(const AttitudePositionEstimatorEKF& that) = delete; + AttitudePositionEstimatorEKF operator=(const AttitudePositionEstimatorEKF&) = delete; + + /** + * Destructor, also kills the sensors task. + */ + ~AttitudePositionEstimatorEKF(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + + /** + * Print the current status. + */ + void print_status(); + + /** + * Trip the filter by feeding it NaN values. + */ + int trip_nan(); + + /** + * Enable logging. + * + * @param enable Set to true to enable logging, false to disable + */ + int enable_logging(bool enable); + + /** + * Set debug level. + * + * @param debug Desired debug level - 0 to disable. + */ + int set_debuglevel(unsigned debug) { _debug = debug; return 0; } + +private: + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _estimator_task; /**< task handle for sensor task */ + + int _sensor_combined_sub; + int _distance_sub; /**< distance measurement */ + int _airspeed_sub; /**< airspeed subscription */ + int _baro_sub; /**< barometer subscription */ + int _gps_sub; /**< GPS subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _mission_sub; + int _home_sub; /**< home position as defined by commander / user */ + + orb_advert_t _att_pub; /**< vehicle attitude */ + orb_advert_t _global_pos_pub; /**< global position */ + orb_advert_t _local_pos_pub; /**< position in local frame */ + orb_advert_t _estimator_status_pub; /**< status of the estimator */ + orb_advert_t _wind_pub; /**< wind estimate */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct gyro_report _gyro; + struct accel_report _accel; + struct mag_report _mag; + struct airspeed_s _airspeed; /**< airspeed */ + struct baro_report _baro; /**< baro readings */ + struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_local_position_s _local_pos; /**< local vehicle position */ + struct vehicle_gps_position_s _gps; /**< GPS position */ + struct wind_estimate_s _wind; /**< wind estimate */ + struct range_finder_report _distance; /**< distance estimate */ + + struct gyro_scale _gyro_offsets[3]; + struct accel_scale _accel_offsets[3]; + struct mag_scale _mag_offsets[3]; + + struct sensor_combined_s _sensor_combined; + + struct map_projection_reference_s _pos_ref; + + float _baro_ref; /**< barometer reference altitude */ + float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ + float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ + hrt_abstime _last_debug_print = 0; + + perf_counter_t _loop_perf; ///< loop performance counter + perf_counter_t _loop_intvl; ///< loop rate counter + perf_counter_t _perf_gyro; /// */ +#include "AttitudePositionEstimatorEKF.h" +#include "estimator_22states.h" + #include #include #include @@ -52,39 +55,15 @@ #include #include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include -#include -#include #include #include #include #include #include -#include "estimator_22states.h" - static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; @@ -114,273 +93,6 @@ uint64_t getMicros() return IMUusec; } -class AttitudePositionEstimatorEKF -{ -public: - /** - * Constructor - */ - AttitudePositionEstimatorEKF(); - - /* we do not want people ever copying this class */ - AttitudePositionEstimatorEKF(const AttitudePositionEstimatorEKF& that) = delete; - AttitudePositionEstimatorEKF operator=(const AttitudePositionEstimatorEKF&) = delete; - - /** - * Destructor, also kills the sensors task. - */ - ~AttitudePositionEstimatorEKF(); - - /** - * Start the sensors task. - * - * @return OK on success. - */ - int start(); - - /** - * Task status - * - * @return true if the mainloop is running - */ - bool task_running() { return _task_running; } - - /** - * Print the current status. - */ - void print_status(); - - /** - * Trip the filter by feeding it NaN values. - */ - int trip_nan(); - - /** - * Enable logging. - * - * @param enable Set to true to enable logging, false to disable - */ - int enable_logging(bool enable); - - /** - * Set debug level. - * - * @param debug Desired debug level - 0 to disable. - */ - int set_debuglevel(unsigned debug) { _debug = debug; return 0; } - -private: - bool _task_should_exit; /**< if true, sensor task should exit */ - bool _task_running; /**< if true, task is running in its mainloop */ - int _estimator_task; /**< task handle for sensor task */ - - int _sensor_combined_sub; - int _distance_sub; /**< distance measurement */ - int _airspeed_sub; /**< airspeed subscription */ - int _baro_sub; /**< barometer subscription */ - int _gps_sub; /**< GPS subscription */ - int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_control_sub; /**< notification of manual control updates */ - int _mission_sub; - int _home_sub; /**< home position as defined by commander / user */ - - orb_advert_t _att_pub; /**< vehicle attitude */ - orb_advert_t _global_pos_pub; /**< global position */ - orb_advert_t _local_pos_pub; /**< position in local frame */ - orb_advert_t _estimator_status_pub; /**< status of the estimator */ - orb_advert_t _wind_pub; /**< wind estimate */ - - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct gyro_report _gyro; - struct accel_report _accel; - struct mag_report _mag; - struct airspeed_s _airspeed; /**< airspeed */ - struct baro_report _baro; /**< baro readings */ - struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct vehicle_local_position_s _local_pos; /**< local vehicle position */ - struct vehicle_gps_position_s _gps; /**< GPS position */ - struct wind_estimate_s _wind; /**< wind estimate */ - struct range_finder_report _distance; /**< distance estimate */ - - struct gyro_scale _gyro_offsets[3]; - struct accel_scale _accel_offsets[3]; - struct mag_scale _mag_offsets[3]; - - struct sensor_combined_s _sensor_combined; - - struct map_projection_reference_s _pos_ref; - - float _baro_ref; /**< barometer reference altitude */ - float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ - float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ - hrt_abstime _last_debug_print = 0; - - perf_counter_t _loop_perf; ///< loop performance counter - perf_counter_t _loop_intvl; ///< loop rate counter - perf_counter_t _perf_gyro; /// Date: Fri, 13 Feb 2015 10:50:55 +0100 Subject: AttPosEKF: Use multiplatform land detector (was custom FixedWing only) --- .../AttitudePositionEstimatorEKF.h | 3 ++ .../ekf_att_pos_estimator_main.cpp | 18 +++++++++--- .../ekf_att_pos_estimator/estimator_22states.cpp | 34 ++++++++++++---------- .../ekf_att_pos_estimator/estimator_22states.h | 12 ++++++-- 4 files changed, 46 insertions(+), 21 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 07db924b2..228ffa853 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -141,6 +142,7 @@ private: int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; int _home_sub; /**< home position as defined by commander / user */ + int _landDetectorSub; orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -160,6 +162,7 @@ private: struct vehicle_gps_position_s _gps; /**< GPS position */ struct wind_estimate_s _wind; /**< wind estimate */ struct range_finder_report _distance; /**< distance estimate */ + struct vehicle_land_detected_s _landDetector; struct gyro_scale _gyro_offsets[3]; struct accel_scale _accel_offsets[3]; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 511492b5a..5e5208e78 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -121,6 +121,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _manual_control_sub(-1), _mission_sub(-1), _home_sub(-1), + _landDetectorSub(-1), /* publications */ _att_pub(-1), @@ -141,6 +142,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _gps({}), _wind({}), _distance{}, + _landDetector{}, _gyro_offsets({}), _accel_offsets({}), @@ -496,6 +498,7 @@ void AttitudePositionEstimatorEKF::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_sub = orb_subscribe(ORB_ID(home_position)); + _landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -649,6 +652,9 @@ void AttitudePositionEstimatorEKF::task_main() } else if (_ekf->statesInitialised) { + // Check if on ground - status is used by covariance prediction + _ekf->setOnGround(_landDetector.landed); + // We're apparently initialized in this case now // check (and reset the filter as needed) int check = check_filter_state(); @@ -904,9 +910,6 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // store the predicted states for subsequent use by measurement fusion _ekf->StoreStates(IMUmsec); - // Check if on ground - status is used by covariance prediction - _ekf->OnGroundCheck(); - // sum delta angles and time used by covariance prediction _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; @@ -1086,7 +1089,7 @@ void AttitudePositionEstimatorEKF::print_status() } printf("states: %s %s %s %s %s %s %s %s %s %s\n", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", + (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", @@ -1218,6 +1221,13 @@ void AttitudePositionEstimatorEKF::pollData() //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + //Update Land Detector + bool newLandData; + orb_check(_landDetectorSub, &newLandData); + if(newLandData) { + orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector); + } + //Update AirSpeed orb_check(_airspeed_sub, &_newAdsData); if (_newAdsData) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index f8c6510a2..700979eda 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -153,7 +153,6 @@ AttPosEKF::AttPosEKF() : inhibitGndState(true), inhibitScaleState(true), - onGround(true), staticMode(true), useGPS(false), useAirspeed(true), @@ -183,7 +182,9 @@ AttPosEKF::AttPosEKF() : auxFlowInnovGate(0.0f), rngInnovGate(0.0f), minFlowRng(0.0f), - moCompR_LOS(0.0f) + moCompR_LOS(0.0f), + + _onGround(true) { memset(&last_ekf_error, 0, sizeof(last_ekf_error)); @@ -442,7 +443,7 @@ void AttPosEKF::CovariancePrediction(float dt) daxCov = sq(dt*gyroProcessNoise); dayCov = sq(dt*gyroProcessNoise); dazCov = sq(dt*gyroProcessNoise); - if (onGround) dazCov = dazCov * sq(yawVarScale); + if (_onGround) dazCov = dazCov * sq(yawVarScale); accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); dvxCov = sq(dt*accelProcessNoise); dvyCov = sq(dt*accelProcessNoise); @@ -1132,6 +1133,7 @@ void AttPosEKF::FuseVelposNED() current_ekf_state.velHealth = false; } } + if (fusePosData) { // test horizontal position measurements @@ -1163,6 +1165,7 @@ void AttPosEKF::FuseVelposNED() current_ekf_state.posHealth = false; } } + // test height measurements if (fuseHgtData) { @@ -1608,7 +1611,7 @@ void AttPosEKF::FuseMagnetometer() KH[i][j] = Kfusion[i] * H_MAG[j]; } for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; - if (!onGround) + if (!_onGround) { for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) { @@ -1632,7 +1635,7 @@ void AttPosEKF::FuseMagnetometer() { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } - if (!onGround) + if (!_onGround) { for (uint8_t k = 16; k < EKF_STATE_ESTIMATES; k++) { @@ -2527,37 +2530,41 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d hgt = hgtRef - posNED[2]; } -void AttPosEKF::OnGroundCheck() +void AttPosEKF::setOnGround(const bool isLanded) { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + _onGround = isLanded; if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } // don't update wind states if there is no airspeed measurement - if (onGround || !useAirspeed) { + if (_onGround || !useAirspeed) { inhibitWindStates = true; } else { inhibitWindStates =false; } + // don't update magnetic field states if on ground or not using compass - if (onGround || !useCompass) { + if (_onGround || !useCompass) { inhibitMagStates = true; } else { inhibitMagStates = false; } + // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS - if ((onGround || !useGPS) && !useRangeFinder) { + if ((_onGround || !useGPS) && !useRangeFinder) { inhibitGndState = true; } else { inhibitGndState = false; } + // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable - if ((onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) { + if ((_onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) { inhibitGndState = true; } else { inhibitGndState = false; } + // Don't update focal length offset state if there is no range finder or optical flow sensor // we need both sensors to do this estimation if (!useRangeFinder || !useOpticalFlow) { @@ -2967,9 +2974,6 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) int ret = 0; - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - // Reset the filter if the states went NaN if (StatesNaN()) { ekf_debug("re-initializing dynamic"); @@ -3279,7 +3283,7 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.states[i] = states[i]; } current_ekf_state.n_states = EKF_STATE_ESTIMATES; - current_ekf_state.onGround = onGround; + current_ekf_state.onGround = _onGround; current_ekf_state.staticMode = staticMode; current_ekf_state.useCompass = useCompass; current_ekf_state.useAirspeed = useAirspeed; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 19ef52145..8e820bfd9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -212,7 +212,6 @@ public: bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant - bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused bool useGPS; // boolean true if GPS data is being used bool useAirspeed; ///< boolean true if airspeed data is being used @@ -319,7 +318,11 @@ public: static inline float sq(float valIn) {return valIn * valIn;} - void OnGroundCheck(); + /** + * @brief + * Tell the EKF if the vehicle has landed + **/ + void setOnGround(const bool isLanded); void CovarianceInit(); @@ -396,6 +399,11 @@ protected: void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); + void ResetStoredStates(); + +private: + bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + }; uint32_t millis(); -- cgit v1.2.3 From f64a8d7cb0bf922ee7cc18acc9c49fdb10e4089e Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 13 Feb 2015 10:52:10 +0100 Subject: AttPosEKF: Fix sensor loss recovery --- .../ekf_att_pos_estimator_main.cpp | 13 +++------ .../ekf_att_pos_estimator/estimator_22states.cpp | 31 +++++++++++++++------- .../ekf_att_pos_estimator/estimator_22states.h | 2 -- 3 files changed, 26 insertions(+), 20 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5e5208e78..1c79cb61d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -633,14 +633,10 @@ void AttitudePositionEstimatorEKF::task_main() } else if (!_ekf->statesInitialised) { // North, East Down position (m) - float posNED[3] = {0.0f, 0.0f, 0.0f}; - float initVelNED[3]; - - initVelNED[0] = 0.0f; - initVelNED[1] = 0.0f; - initVelNED[2] = 0.0f; - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; + float initVelNED[3] = {0.0f, 0.0f, 0.0f}; + + _ekf->posNE[0] = 0.0f; + _ekf->posNE[1] = 0.0f; _local_pos.ref_alt = _baro_ref; _baro_ref_offset = 0.0f; @@ -1291,7 +1287,6 @@ void AttitudePositionEstimatorEKF::pollData() if (dtGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); _ekf->ResetVelocity(); - _ekf->ResetStoredStates(); } } diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 700979eda..bdab0e5bc 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -1121,10 +1121,9 @@ void AttPosEKF::FuseVelposNED() current_ekf_state.velHealth = true; current_ekf_state.velFailTime = millis(); } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) { - // XXX check current_ekf_state.velHealth = true; ResetVelocity(); - ResetStoredStates(); + // do not fuse bad data fuseVelData = false; } @@ -1152,9 +1151,6 @@ void AttPosEKF::FuseVelposNED() if (current_ekf_state.posTimeout) { ResetPosition(); - // XXX cross-check the state reset - ResetStoredStates(); - // do not fuse position data on this time // step fusePosData = false; @@ -1183,7 +1179,6 @@ void AttPosEKF::FuseVelposNED() // the height data, but reset height and stored states if (current_ekf_state.hgtTimeout) { ResetHeight(); - ResetStoredStates(); fuseHgtData = false; } } @@ -2855,6 +2850,12 @@ void AttPosEKF::ResetPosition(void) // reset the states from the GPS measurements states[7] = posNE[0]; states[8] = posNE[1]; + + // stored horizontal position states to prevent subsequent GPS measurements from being rejected + for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ + storedStates[7][i] = states[7]; + storedStates[8][i] = states[8]; + } } } @@ -2862,6 +2863,11 @@ void AttPosEKF::ResetHeight(void) { // write to the state vector states[9] = -hgtMea; + + // stored horizontal position states to prevent subsequent Barometer measurements from being rejected + for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ + storedStates[9][i] = states[9]; + } } void AttPosEKF::ResetVelocity(void) @@ -2871,10 +2877,16 @@ void AttPosEKF::ResetVelocity(void) states[5] = 0.0f; states[6] = 0.0f; } else if (GPSstatus >= GPS_FIX_3D) { + //Do not use Z velocity, we trust the Barometer history more states[4] = velNED[0]; // north velocity from last reading states[5] = velNED[1]; // east velocity from last reading - states[6] = velNED[2]; // down velocity from last reading + + // stored horizontal position states to prevent subsequent GPS measurements from being rejected + for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ + storedStates[4][i] = states[4]; + storedStates[5][i] = states[5]; + } } } @@ -2992,10 +3004,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report GetFilterState(&last_ekf_error); + ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); - ResetStoredStates(); // Timeout cleared with this reset current_ekf_state.imuTimeout = false; @@ -3009,10 +3021,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report, but not setting error flag GetFilterState(&last_ekf_error); + ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); - ResetStoredStates(); ret = 0; } @@ -3182,6 +3194,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) states[20] = magBias.y; // Magnetic Field Bias Y states[21] = magBias.z; // Magnetic Field Bias Z + ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 8e820bfd9..070b6a63c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -298,8 +298,6 @@ public: void RecallOmega(float *omegaForFusion, uint64_t msec); - void ResetStoredStates(); - void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); -- cgit v1.2.3 From f4da1df23a7b3916c8217ab1566d590deacc2b10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Feb 2015 19:37:22 +0100 Subject: Style fix of copyright header --- .../ekf_att_pos_estimator/estimator_22states.cpp | 1 + .../ekf_att_pos_estimator/estimator_22states.h | 5 +- .../ekf_att_pos_estimator/estimator_utilities.cpp | 63 ++++++++++----------- .../ekf_att_pos_estimator/estimator_utilities.h | 64 +++++++++++----------- 4 files changed, 65 insertions(+), 68 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index bdab0e5bc..d8e3116b9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -34,6 +34,7 @@ * Implementation of the attitude and position estimator. * * @author Paul Riseborough + * @author Lorenz Meier */ #include "estimator_22states.h" diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 070b6a63c..e15ded977 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -26,14 +26,15 @@ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. -*****************************************************************************/ +****************************************************************************/ /** * @file estimator_22states.h * - * Implementation of the attitude and position estimator. + * Definition of the attitude and position estimator. * * @author Paul Riseborough + * @author Lorenz Meier */ #pragma once diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 855ea2c6e..e2f4c7e82 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -1,43 +1,40 @@ /**************************************************************************** - * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ +* Copyright (c) 2014, Paul Riseborough All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* Neither the name of the {organization} nor the names of its contributors +* may be used to endorse or promote products derived from this software without +* specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +****************************************************************************/ /** * @file estimator_utilities.cpp * - * Implementation of the attitude and position estimator. + * Estimator support utilities. * * @author Paul Riseborough - * @author Lorenz Meier + * @author Lorenz Meier */ #include "estimator_utilities.h" diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 3b9739fb5..95b83ead4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -1,44 +1,42 @@ /**************************************************************************** - * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ +* Copyright (c) 2014, Paul Riseborough All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* +* Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* Neither the name of the {organization} nor the names of its contributors +* may be used to endorse or promote products derived from this software without +* specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +****************************************************************************/ /** * @file estimator_utilities.h * - * Implementation of the attitude and position estimator. + * Estimator support utilities. * * @author Paul Riseborough - * @author Lorenz Meier + * @author Lorenz Meier */ + #pragma once #include -- cgit v1.2.3