forked from Archive/PX4-Autopilot
EKF: Use voting class instead of special routines to select sensor
This commit is contained in:
Lorenz Meier
parent
fcb25fd02c
commit
68666aa393
|
|
@ -70,6 +70,7 @@
|
|||
|
||||
#include <geo/geo.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <lib/ecl/validation/data_validator_group.h>
|
||||
#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
|
||||
|
||||
|
|
|
|||
|
|
@ -68,7 +68,6 @@
|
|||
#include <mavlink/mavlink_log.h>
|
||||
#include <platforms/px4_defines.h>
|
||||
|
||||
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()
|
||||
|
|
@ -140,29 +144,27 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
|
|||
_estimator_status_pub(nullptr),
|
||||
_wind_pub(nullptr),
|
||||
|
||||
_att({}),
|
||||
_gyro({}),
|
||||
_accel({}),
|
||||
_mag({}),
|
||||
_airspeed({}),
|
||||
_baro({}),
|
||||
_vstatus({}),
|
||||
_global_pos({}),
|
||||
_local_pos({}),
|
||||
_gps({}),
|
||||
_wind({}),
|
||||
_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{},
|
||||
|
||||
|
|
@ -189,15 +191,17 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
|
|||
_baro_init(false),
|
||||
_gps_initialized(false),
|
||||
_filter_start_time(0),
|
||||
_last_sensor_timestamp(0),
|
||||
_last_run(0),
|
||||
_last_sensor_timestamp(hrt_absolute_time()),
|
||||
_distance_last_valid(0),
|
||||
_gyro_valid(false),
|
||||
_accel_valid(false),
|
||||
_mag_valid(false),
|
||||
_gyro_main(0),
|
||||
_accel_main(0),
|
||||
_mag_main(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),
|
||||
|
||||
|
|
@ -215,7 +219,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
|
|||
_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);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue