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