From e0d1e3ca5e73a2e6a56a8b1050cba6b420e915d0 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 3 Mar 2015 13:36:18 +0100 Subject: AttPosEKF: Fix code style using AStyle script --- .../ekf_att_pos_estimator_main.cpp | 392 ++++++++++++--------- 1 file changed, 216 insertions(+), 176 deletions(-) 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 ed140a728..aad3aa43d 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 @@ -96,13 +96,13 @@ uint64_t getMicros() 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() : @@ -110,7 +110,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _task_running(false), _estimator_task(-1), - /* subscriptions */ +/* subscriptions */ _sensor_combined_sub(-1), _distance_sub(-1), _airspeed_sub(-1), @@ -124,7 +124,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _landDetectorSub(-1), _armedSub(-1), - /* publications */ +/* publications */ _att_pub(-1), _global_pos_pub(-1), _local_pos_pub(-1), @@ -132,72 +132,72 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _wind_pub(-1), _att({}), - _gyro({}), - _accel({}), - _mag({}), - _airspeed({}), - _baro({}), - _vstatus({}), - _global_pos({}), - _local_pos({}), - _gps({}), - _wind({}), - _distance{}, - _landDetector{}, - _armed{}, - - _gyro_offsets({}), - _accel_offsets({}), - _mag_offsets({}), - - _sensor_combined{}, - - _pos_ref{}, - _baro_ref(0.0f), - _baro_ref_offset(0.0f), - _baro_gps_offset(0.0f), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), - _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), - _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), - _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), - _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - - /* states */ - _gps_alt_filt(0.0f), - _baro_alt_filt(0.0f), - _covariancePredictionDt(0.0f), - _gpsIsGood(false), - _previousGPSTimestamp(0), - _baro_init(false), - _gps_initialized(false), - _filter_start_time(0), - _last_sensor_timestamp(0), - _last_run(0), - _distance_last_valid(0), - _gyro_valid(false), - _accel_valid(false), - _mag_valid(false), - _gyro_main(0), - _accel_main(0), - _mag_main(0), - _ekf_logging(true), - _debug(0), - - _newDataGps(false), - _newHgtData(false), - _newAdsData(false), - _newDataMag(false), - _newRangeData(false), - - _mavlink_fd(-1), - _parameters{}, - _parameter_handles{}, - _ekf(nullptr) + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + _wind({}), + _distance {}, + _landDetector {}, + _armed {}, + + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), + + _sensor_combined {}, + + _pos_ref {}, + _baro_ref(0.0f), + _baro_ref_offset(0.0f), + _baro_gps_offset(0.0f), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), + _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), + + /* states */ + _gps_alt_filt(0.0f), + _baro_alt_filt(0.0f), + _covariancePredictionDt(0.0f), + _gpsIsGood(false), + _previousGPSTimestamp(0), + _baro_init(false), + _gps_initialized(false), + _filter_start_time(0), + _last_sensor_timestamp(0), + _last_run(0), + _distance_last_valid(0), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), + _gyro_main(0), + _accel_main(0), + _mag_main(0), + _ekf_logging(true), + _debug(0), + + _newDataGps(false), + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), + + _mavlink_fd(-1), + _parameters {}, + _parameter_handles {}, + _ekf(nullptr) { _last_run = hrt_absolute_time(); @@ -336,7 +336,7 @@ int AttitudePositionEstimatorEKF::parameters_update() _ekf->posDSigma = _parameters.posd_noise; _ekf->magMeasurementSigma = _parameters.mag_noise; _ekf->gyroProcessNoise = _parameters.gyro_pnoise; - _ekf->accelProcessNoise = _parameters.acc_pnoise; + _ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise; _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF } @@ -367,14 +367,15 @@ int AttitudePositionEstimatorEKF::check_filter_state() int check = _ekf->CheckAndBound(&ekf_report); - const char* const feedback[] = { 0, - "NaN in states, resetting", - "stale sensor data, resetting", - "got initial position lock", - "excessive gyro offsets", - "velocity diverted, check accel config", - "excessive covariances", - "unknown condition, resetting"}; + const char *const feedback[] = { 0, + "NaN in states, resetting", + "stale sensor data, resetting", + "got initial position lock", + "excessive gyro offsets", + "velocity diverted, check accel config", + "excessive covariances", + "unknown condition, resetting" + }; // Print out error condition if (check) { @@ -393,6 +394,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() } struct estimator_status_report rep; + memset(&rep, 0, sizeof(rep)); // If error flag is set, we got a filter reset @@ -435,18 +437,18 @@ int AttitudePositionEstimatorEKF::check_filter_state() if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", - ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); + ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); } if (rep.timeout_flags) { warnx("timeout: %s%s%s%s", - ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), - ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), - ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), - ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); + ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), + ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), + ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), + ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); } } @@ -463,6 +465,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() if (_estimator_status_pub > 0) { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); + } else { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } @@ -533,8 +536,9 @@ void AttitudePositionEstimatorEKF::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -593,7 +597,7 @@ void AttitudePositionEstimatorEKF::task_main() /** * PART ONE: COLLECT ALL DATA **/ - pollData(); + pollData(); /* * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY @@ -631,10 +635,10 @@ void AttitudePositionEstimatorEKF::task_main() /* Initialize the filter first */ if (!_gps_initialized && _gpsIsGood) { initializeGPS(); - } - else if (!_ekf->statesInitialised) { + + } else if (!_ekf->statesInitialised) { // North, East Down position (m) - float initVelNED[3] = {0.0f, 0.0f, 0.0f}; + float initVelNED[3] = {0.0f, 0.0f, 0.0f}; _ekf->posNE[0] = 0.0f; _ekf->posNE[1] = 0.0f; @@ -646,8 +650,7 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - } - else if (_ekf->statesInitialised) { + } else if (_ekf->statesInitialised) { // Check if on ground - status is used by covariance prediction _ekf->setOnGround(_landDetector.landed); @@ -655,6 +658,7 @@ 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; @@ -670,8 +674,7 @@ void AttitudePositionEstimatorEKF::task_main() publishLocalPosition(); //Publish Global Position, but only if it's any good - if(_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) - { + if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) { publishGlobalPosition(); } @@ -739,12 +742,14 @@ void AttitudePositionEstimatorEKF::initializeGPS() 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 +#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 + (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; } @@ -843,6 +848,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() 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; @@ -858,7 +864,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() /* 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); + (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); _global_pos.yaw = _local_pos.yaw; _global_pos.eph = _gps.eph; @@ -898,8 +904,9 @@ void AttitudePositionEstimatorEKF::publishWindEstimate() } -void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, - const bool fuseBaro, const bool fuseAirSpeed) +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(); @@ -914,8 +921,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // 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)) { + if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) + || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(_covariancePredictionDt); _ekf->summedDelAng.zero(); _ekf->summedDelVel.zero(); @@ -937,8 +944,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // run the fusion step _ekf->FuseVelposNED(); - } - else if (!_gps_initialized) { + } else if (!_gps_initialized) { // force static mode _ekf->staticMode = true; @@ -962,8 +968,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // run the fusion step _ekf->FuseVelposNED(); - } - else { + } else { _ekf->fuseVelData = false; _ekf->fusePosData = false; } @@ -979,34 +984,33 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // run the fusion step _ekf->FuseVelposNED(); - } - else { + } else { _ekf->fuseHgtData = false; } // 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 + _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(); - } - else { + } 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->RecallStates(_ekf->statesAtVtasMeasTime, + (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); - } - else { + } else { _ekf->fuseVtasData = false; } @@ -1064,37 +1068,50 @@ void AttitudePositionEstimatorEKF::print_status() printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - 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]); + printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, + (double)_baro_gps_offset); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, + (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, + (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, + (double)_ekf->correctedDelAng.z); + printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], + (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], + (double)_ekf->states[6]); + 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 (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]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + 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]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], + (double)_ekf->states[21]); printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); } else { printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); - printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], + (double)_ekf->states[17]); + printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], + (double)_ekf->states[20]); } + printf("states: %s %s %s %s %s %s %s %s %s %s\n", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } void AttitudePositionEstimatorEKF::pollData() @@ -1102,7 +1119,8 @@ void AttitudePositionEstimatorEKF::pollData() //Update arming status bool armedUpdate; orb_check(_armedSub, &armedUpdate); - if(armedUpdate) { + + if (armedUpdate) { orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed); } @@ -1118,6 +1136,7 @@ void AttitudePositionEstimatorEKF::pollData() if (last_accel != _sensor_combined.accelerometer_timestamp) { accel_updated = true; + } else { accel_updated = false; } @@ -1146,9 +1165,9 @@ void AttitudePositionEstimatorEKF::pollData() 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)) { + 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]; @@ -1157,8 +1176,8 @@ void AttitudePositionEstimatorEKF::pollData() _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])) { + 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]; @@ -1177,6 +1196,7 @@ void AttitudePositionEstimatorEKF::pollData() if (!_gyro_valid) { /* keep last value if he hit an out of band value */ lastAngRate = _ekf->angRate; + } else { perf_count(_perf_gyro); } @@ -1191,6 +1211,7 @@ void AttitudePositionEstimatorEKF::pollData() _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]; @@ -1228,12 +1249,14 @@ void AttitudePositionEstimatorEKF::pollData() //Update Land Detector bool newLandData; orb_check(_landDetectorSub, &newLandData); - if(newLandData) { + + if (newLandData) { orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector); } //Update AirSpeed orb_check(_airspeed_sub, &_newAdsData); + if (_newAdsData) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); @@ -1243,6 +1266,7 @@ void AttitudePositionEstimatorEKF::pollData() orb_check(_gps_sub, &_newDataGps); + if (_newDataGps) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); @@ -1250,15 +1274,16 @@ void AttitudePositionEstimatorEKF::pollData() //We are more strict for our first fix float requiredAccuracy = _parameters.pos_stddev_threshold; - if(_gpsIsGood) { + + 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) { + if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { _gpsIsGood = true; - } - else { + + } else { _gpsIsGood = false; } @@ -1268,9 +1293,10 @@ void AttitudePositionEstimatorEKF::pollData() const float dtLastGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; //Stop dead-reckoning mode - if(_global_pos.dead_reckoning) { + if (_global_pos.dead_reckoning) { mavlink_log_info(_mavlink_fd, "[ekf] stop dead-reckoning"); } + _global_pos.dead_reckoning = false; //Fetch new GPS data @@ -1283,7 +1309,7 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; - if(_previousGPSTimestamp != 0) { + if (_previousGPSTimestamp != 0) { //Calculate average time between GPS updates _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); @@ -1292,10 +1318,10 @@ void AttitudePositionEstimatorEKF::pollData() } //check if we had a GPS outage for a long time - if(_gps_initialized) { + if (_gps_initialized) { //Convert from global frame to local frame - float posNED[3] = {0.0f, 0.0f, 0.0f}; + 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]; @@ -1323,8 +1349,8 @@ void AttitudePositionEstimatorEKF::pollData() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); _previousGPSTimestamp = _gps.timestamp_position; - } - else { + + } else { //Too poor GPS fix to use _newDataGps = false; } @@ -1333,9 +1359,10 @@ 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) const float dtLastGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; - if(dtLastGoodGPS >= POS_RESET_THRESHOLD) { - if(_global_pos.dead_reckoning) { + if (dtLastGoodGPS >= POS_RESET_THRESHOLD) { + + if (_global_pos.dead_reckoning) { mavlink_log_info(_mavlink_fd, "[ekf] gave up dead-reckoning after long timeout"); } @@ -1344,14 +1371,15 @@ void AttitudePositionEstimatorEKF::pollData() } //If we have no good GPS fix for half a second, then enable dead-reckoning mode while armed (for up to POS_RESET_THRESHOLD seconds) - else if(dtLastGoodGPS >= 0.5f) { - if(_armed.armed) { - if(!_global_pos.dead_reckoning) { + else if (dtLastGoodGPS >= 0.5f) { + if (_armed.armed) { + if (!_global_pos.dead_reckoning) { mavlink_log_info(_mavlink_fd, "[ekf] dead-reckoning enabled"); } + _global_pos.dead_reckoning = true; - } - else { + + } else { _global_pos.dead_reckoning = false; } } @@ -1366,18 +1394,20 @@ void AttitudePositionEstimatorEKF::pollData() // init lowpass filters for baro and gps altitude float baro_elapsed; - if(baro_last == 0) { + + if (baro_last == 0) { baro_elapsed = 0.0f; - } - else { + + } 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); + _baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); if (!_baro_init) { _baro_ref = _baro.altitude; @@ -1412,6 +1442,7 @@ void AttitudePositionEstimatorEKF::pollData() _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 @@ -1431,19 +1462,22 @@ void AttitudePositionEstimatorEKF::pollData() //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 AttitudePositionEstimatorEKF::trip_nan() +{ int ret = 0; @@ -1451,6 +1485,7 @@ int AttitudePositionEstimatorEKF::trip_nan() { if (_armed.armed) { warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); ret = 1; + } else { float nan_val = 0.0f / 0.0f; @@ -1488,18 +1523,21 @@ int AttitudePositionEstimatorEKF::trip_nan() { int ekf_att_pos_estimator_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}"); + } if (!strcmp(argv[1], "start")) { - if (estimator::g_estimator != nullptr) + if (estimator::g_estimator != nullptr) { errx(1, "already running"); + } estimator::g_estimator = new AttitudePositionEstimatorEKF(); - if (estimator::g_estimator == nullptr) + if (estimator::g_estimator == nullptr) { errx(1, "alloc failed"); + } if (OK != estimator::g_estimator->start()) { delete estimator::g_estimator; @@ -1513,14 +1551,16 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); exit(0); } if (!strcmp(argv[1], "stop")) { - if (estimator::g_estimator == nullptr) + if (estimator::g_estimator == nullptr) { errx(1, "not running"); + } delete estimator::g_estimator; estimator::g_estimator = nullptr; -- cgit v1.2.3