From 68666aa393aac7cd4fc742e39393731c7090df70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 30 Aug 2015 12:53:22 +0200 Subject: [PATCH] EKF: Use voting class instead of special routines to select sensor --- .../AttitudePositionEstimatorEKF.h | 19 +- .../ekf_att_pos_estimator_main.cpp | 408 ++++++++---------- 2 files changed, 192 insertions(+), 235 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 28abb0818b..6251f8f5cb 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -70,6 +70,7 @@ #include #include +#include #include "estimator_22states.h" //Forward declaration @@ -169,10 +170,8 @@ private: struct vehicle_land_detected_s _landDetector; struct actuator_armed_s _armed; - Vector3f lastAngRate; - Vector3f lastAccel; - hrt_abstime last_accel; - hrt_abstime last_mag; + hrt_abstime _last_accel; + hrt_abstime _last_mag; struct gyro_scale _gyro_offsets[3]; struct accel_scale _accel_offsets[3]; @@ -185,6 +184,8 @@ private: float _filter_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; + float _vibration_warning_threshold = 1.0f; + hrt_abstime _vibration_warning_timestamp = 0; perf_counter_t _loop_perf; ///< loop performance counter perf_counter_t _loop_intvl; ///< loop rate counter @@ -204,14 +205,16 @@ private: bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; - hrt_abstime _last_run; hrt_abstime _distance_last_valid; - bool _gyro_valid; - bool _accel_valid; - bool _mag_valid; + DataValidatorGroup _voter_gyro; + DataValidatorGroup _voter_accel; + DataValidatorGroup _voter_mag; int _gyro_main; ///< index of the main gyroscope int _accel_main; ///< index of the main accelerometer int _mag_main; ///< index of the main magnetometer + bool _data_good; ///< all required filter data is ok + bool _failsafe; ///< failsafe on one of the sensors + bool _vibration_warning; ///< high vibration levels detected bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 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 7de7e04613..95b4a90251 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 @@ -68,7 +68,6 @@ #include #include -static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; //Constants @@ -88,13 +87,18 @@ static constexpr float EPV_LARGE_VALUE = 1000.0f; */ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); -__EXPORT uint32_t millis(); - -__EXPORT uint64_t getMicros(); +uint32_t millis(); +uint64_t getMicros(); +uint32_t getMillis(); uint32_t millis() { - return IMUmsec; + return getMillis(); +} + +uint32_t getMillis() +{ + return getMicros() / 1000; } uint64_t getMicros() @@ -133,89 +137,88 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _landDetectorSub(-1), _armedSub(-1), -/* publications */ + /* publications */ _att_pub(nullptr), _global_pos_pub(nullptr), _local_pos_pub(nullptr), _estimator_status_pub(nullptr), _wind_pub(nullptr), - _att({}), - _gyro({}), - _accel({}), - _mag({}), - _airspeed({}), - _baro({}), - _vstatus({}), - _global_pos({}), - _local_pos({}), - _gps({}), - _wind({}), - _distance {}, - _landDetector {}, - _armed {}, + _att{}, + _gyro{}, + _accel{}, + _mag{}, + _airspeed{}, + _baro{}, + _vstatus{}, + _global_pos{}, + _local_pos{}, + _gps{}, + _wind{}, + _distance{}, + _landDetector{}, + _armed{}, - lastAngRate{}, - lastAccel{}, - last_accel(0), - last_mag(0), + _last_accel(0), + _last_mag(0), - _gyro_offsets({}), - _accel_offsets({}), - _mag_offsets({}), + _gyro_offsets{}, + _accel_offsets{}, + _mag_offsets{}, - _sensor_combined {}, + _sensor_combined{}, - _pos_ref{}, - _filter_ref_offset(0.0f), - _baro_gps_offset(0.0f), + _pos_ref{}, + _filter_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")), + /* 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), + /* 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(hrt_absolute_time()), + _distance_last_valid(0), + _voter_gyro(3), + _voter_accel(3), + _voter_mag(3), + _gyro_main(-1), + _accel_main(-1), + _mag_main(-1), + _data_good(false), + _failsafe(false), + _vibration_warning(false), + _ekf_logging(true), + _debug(0), - _newHgtData(false), - _newAdsData(false), - _newDataMag(false), - _newRangeData(false), + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), - _mavlink_fd(-1), - _parameters{}, - _parameter_handles{}, - _ekf(nullptr), + _mavlink_fd(-1), + _parameters{}, + _parameter_handles{}, + _ekf(nullptr), - _LP_att_P(100.0f, 10.0f), - _LP_att_Q(100.0f, 10.0f), - _LP_att_R(100.0f, 10.0f) + _LP_att_P(100.0f, 10.0f), + _LP_att_Q(100.0f, 10.0f), + _LP_att_R(100.0f, 10.0f) { - _last_run = hrt_absolute_time(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); @@ -613,15 +616,14 @@ void AttitudePositionEstimatorEKF::task_main() orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); /* set sensors to de-initialized state */ - _gyro_valid = false; - _accel_valid = false; - _mag_valid = false; + _gyro_main = -1; + _accel_main = -1; + _mag_main = -1; _baro_init = false; _gps_initialized = false; _last_sensor_timestamp = hrt_absolute_time(); - _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; @@ -649,7 +651,7 @@ void AttitudePositionEstimatorEKF::task_main() * We run the filter only once all data has been fetched **/ - if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) { // maintain filtered baro and gps altitudes to calculate weather offset // baro sample rate is ~70Hz and measurement bandwidth is high @@ -1012,7 +1014,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->UpdateStrapdownEquationsNED(); // store the predicted states for subsequent use by measurement fusion - _ekf->StoreStates(IMUmsec); + _ekf->StoreStates(getMillis()); // sum delta angles and time used by covariance prediction _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; @@ -1038,8 +1040,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _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)); + _ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -1062,8 +1064,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _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)); + _ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -1079,7 +1081,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + _ekf->RecallStates(_ekf->statesAtHgtTime, (getMillis() - _parameters.height_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -1092,7 +1094,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseMag) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, - (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + (getMillis() - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data _ekf->magstate.obsIndex = 0; _ekf->FuseMagnetometer(); @@ -1107,7 +1109,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, - (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + (getMillis() - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); } else { @@ -1120,7 +1122,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // _ekf->rngMea is set in sensor readout already _ekf->fuseRngData = true; _ekf->fuseOptFlowData = false; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->RecallStates(_ekf->statesAtRngTime, (getMillis() - 100.0f)); _ekf->OpticalFlowEKF(); _ekf->fuseRngData = false; } @@ -1166,7 +1168,7 @@ void AttitudePositionEstimatorEKF::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - PX4_INFO("dtIMU: %8.6f filt: %8.6f IMUmsec: %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); + PX4_INFO("dtIMU: %8.6f filt: %8.6f IMU (ms): %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)getMillis()); PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt); PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt); PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset, @@ -1213,6 +1215,13 @@ void AttitudePositionEstimatorEKF::print_status() (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + + PX4_INFO("gyro status:"); + _voter_gyro.print(); + PX4_INFO("accel status:"); + _voter_accel.print(); + PX4_INFO("mag status:"); + _voter_mag.print(); } void AttitudePositionEstimatorEKF::pollData() @@ -1225,27 +1234,99 @@ void AttitudePositionEstimatorEKF::pollData() orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed); } - //Update Gyro and Accelerometer - bool accel_updated = false; - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - if (last_accel != _sensor_combined.accelerometer_timestamp[_accel_main]) { - accel_updated = true; + // Feed validator with recent sensor data - } else { - accel_updated = false; + for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { + _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], _sensor_combined.gyro_errcount[i]); + _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], _sensor_combined.accelerometer_errcount[i]); + _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], _sensor_combined.magnetometer_errcount[i]); } - last_accel = _sensor_combined.accelerometer_timestamp[_accel_main]; + // Get best measurement values + hrt_abstime curr_time = hrt_absolute_time(); + (void)_voter_gyro.get_best(curr_time, &_gyro_main); + if (_gyro_main >= 0) { + _ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]; + _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; + _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; + _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; + perf_count(_perf_gyro); + } + (void)_voter_accel.get_best(curr_time, &_accel_main); + if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) { + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]; + _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; + _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; + _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; + _last_accel = _sensor_combined.accelerometer_timestamp[_accel_main]; + } + + (void)_voter_mag.get_best(curr_time, &_mag_main); + if (_mag_main >= 0) { + Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1], + _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); + + /* fail over to the 2nd mag if we know the first is down */ + if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) { + _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 + _newDataMag = true; + _last_mag = _sensor_combined.magnetometer_timestamp[_mag_main]; + perf_count(_perf_mag); + } + } + + if (!_failsafe && (_voter_gyro.failover_count() > 0 || + _voter_accel.failover_count() > 0 || + _voter_mag.failover_count() > 0)) { + + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } + + if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { + + if (_vibration_warning_timestamp == 0) { + _vibration_warning_timestamp = curr_time; + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { + _vibration_warning = true; + mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d", + (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), + (int)(100 * _voter_accel.get_vibration_factor(curr_time)), + (int)(100 * _voter_mag.get_vibration_factor(curr_time))); + } + } else { + _vibration_warning_timestamp = 0; + } - // 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; + float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f; + _last_sensor_timestamp = _sensor_combined.timestamp; + + /* guard against too large deltaT's */ + if (!PX4_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; // XXX this is for assessing the filter performance // leave this in as long as larger improvements are still being made. @@ -1283,93 +1364,7 @@ void AttitudePositionEstimatorEKF::pollData() } #endif - /* guard against too large deltaT's */ - if (!PX4_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; - unsigned best_gyro_err = UINT32_MAX - GYRO_SWITCH_HYSTERESIS - 1; - _gyro_valid = false; - - for (unsigned i = 0; i < 3; i++) { - - if (PX4_ISFINITE(_sensor_combined.gyro_integral_rad[i * 3 + 0]) && - PX4_ISFINITE(_sensor_combined.gyro_integral_rad[i * 3 + 1]) && - PX4_ISFINITE(_sensor_combined.gyro_integral_rad[i * 3 + 2]) && - (_sensor_combined.gyro_errcount[i] < (best_gyro_err + GYRO_SWITCH_HYSTERESIS))) { - - _ekf->angRate.x = _sensor_combined.gyro_rad_s[i * 3 + 0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[i * 3 + 1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[i * 3 + 2]; - _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[i * 3 + 0]; - _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[i * 3 + 1]; - _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[i * 3 + 2]; - _gyro_main = i; - best_gyro_err = _sensor_combined.gyro_errcount[i]; - _gyro_valid = true; - - } - } - - 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 || hrt_elapsed_time(&last_accel) > 5000) { - - int last_accel_main = _accel_main; - - unsigned best_accel_err = UINT32_MAX - ACCEL_SWITCH_HYSTERESIS - 1; - _accel_valid = false; - - for (unsigned i = 0; i < 3; i++) { - - /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount[i] < (best_accel_err + ACCEL_SWITCH_HYSTERESIS)) { - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[i * 3 + 0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[i * 3 + 1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[i * 3 + 2]; - _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[i * 3 + 0]; - _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[i * 3 + 1]; - _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[i * 3 + 2]; - _accel_main = i; - best_accel_err = _sensor_combined.accelerometer_errcount[i]; - _accel_valid = true; - } - } - - 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); - } - } - - if (last_mag != _sensor_combined.magnetometer_timestamp[_mag_main]) { - _newDataMag = true; - - } else { - _newDataMag = false; - } - - last_mag = _sensor_combined.magnetometer_timestamp[_mag_main]; + _data_good = true; //PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); @@ -1391,7 +1386,6 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s; } - bool gps_update; orb_check(_gps_sub, &gps_update); @@ -1545,46 +1539,6 @@ void AttitudePositionEstimatorEKF::pollData() perf_count(_perf_baro); } - //Update Magnetometer - if (_newDataMag || hrt_elapsed_time(&last_mag) > 500000) { - - perf_count(_perf_mag); - - int last_mag_main = _mag_main; - - unsigned best_mag_err = UINT32_MAX - MAG_SWITCH_HYSTERESIS - 1; - _mag_valid = false; - - for (unsigned i = 0; i < 3; i++) { - - Vector3f mag(_sensor_combined.magnetometer_ga[i * 3 + 0], _sensor_combined.magnetometer_ga[i * 3 + 1], - _sensor_combined.magnetometer_ga[i * 3 + 2]); - - const unsigned mag_timeout_us = 200000; - - /* fail over to the 2nd mag if we know the first is down */ - if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp[i]) < mag_timeout_us && - _sensor_combined.magnetometer_errcount[i] < (best_mag_err + MAG_SWITCH_HYSTERESIS) && - mag.length() > 0.1f) { - _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 - _mag_main = i; - best_mag_err = _sensor_combined.magnetometer_errcount[i]; - _mag_valid = true; - } - } - - if (last_mag_main != _mag_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main); - } - } - //Update range data orb_check(_distance_sub, &_newRangeData);