fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 05:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged release into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-13 11:06:01 +02:00
parent 2fc069bd63 a0176474c7
commit c9fefe236b
47 changed files with 1036 additions and 344 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+7 -2
View File
@@ -174,7 +174,7 @@ private:
struct map_projection_reference_s _pos_ref;
float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
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;
@@ -193,7 +193,6 @@ private:
bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use
uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received
bool _baro_init;
float _baroAltRef;
bool _gps_initialized;
hrt_abstime _filter_start_time;
hrt_abstime _last_sensor_timestamp;
@@ -333,6 +332,12 @@ private:
**/
void initializeGPS();
/**
* Initialize the reference position for the local coordinate frame
*/
void initReferencePosition(hrt_abstime timestamp,
double lat, double lon, float gps_alt, float baro_alt);
/**
* @brief
* Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+104 -36
View File
@@ -75,6 +75,9 @@ static uint64_t IMUusec = 0;
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
static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds)
static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds)
static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds)
/**
* estimator app start / stop handling function
@@ -157,7 +160,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_sensor_combined {},
_pos_ref{},
_baro_ref_offset(0.0f),
_filter_ref_offset(0.0f),
_baro_gps_offset(0.0f),
/* performance counters */
@@ -177,7 +180,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_gpsIsGood(false),
_previousGPSTimestamp(0),
_baro_init(false),
_baroAltRef(0.0f),
_gps_initialized(false),
_filter_start_time(0),
_last_sensor_timestamp(0),
@@ -408,6 +410,15 @@ int AttitudePositionEstimatorEKF::check_filter_state()
// Count the reset condition
perf_count(_perf_reset);
// 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);
initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude);
} else if (_ekf_logging) {
_ekf->GetFilterState(&ekf_report);
@@ -590,6 +601,7 @@ void AttitudePositionEstimatorEKF::task_main()
_baro_init = false;
_gps_initialized = false;
_last_sensor_timestamp = hrt_absolute_time();
_last_run = _last_sensor_timestamp;
@@ -626,7 +638,10 @@ void AttitudePositionEstimatorEKF::task_main()
// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
// maintain heavily filtered values for both baro and gps altitude
// Assume the filtered output should be identical for both sensors
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
if (_gpsIsGood) {
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
}
// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
// _last_debug_print = hrt_absolute_time();
// perf_print_counter(_perf_baro);
@@ -648,12 +663,15 @@ void AttitudePositionEstimatorEKF::task_main()
_ekf->posNE[1] = 0.0f;
_local_pos.ref_alt = 0.0f;
_baro_ref_offset = 0.0f;
_baro_gps_offset = 0.0f;
_baro_alt_filt = _baro.altitude;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
_filter_ref_offset = -_baro.altitude;
warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
} else {
if (!_gps_initialized && _gpsIsGood) {
@@ -705,6 +723,32 @@ void AttitudePositionEstimatorEKF::task_main()
return;
}
void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
double lat, double lon, float gps_alt, float baro_alt)
{
// Reference altitude
if (isfinite(_ekf->states[9])) {
_filter_ref_offset = _ekf->states[9];
} else if (isfinite(-_ekf->hgtMea)) {
_filter_ref_offset = -_ekf->hgtMea;
} else {
_filter_ref_offset = -_baro.altitude;
}
// init filtered gps and baro altitudes
_gps_alt_filt = gps_alt;
_baro_alt_filt = baro_alt;
// Initialize projection
_local_pos.ref_lat = lat;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = timestamp;
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);
}
void AttitudePositionEstimatorEKF::initializeGPS()
{
// GPS is in scaled integers, convert
@@ -714,11 +758,6 @@ void AttitudePositionEstimatorEKF::initializeGPS()
// 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 = _ekf->baroHgt;
@@ -740,20 +779,13 @@ void AttitudePositionEstimatorEKF::initializeGPS()
_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);
initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude);
#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);
(double)_filter_ref_offset);
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv,
(double)math::degrees(declination));
#endif
@@ -814,7 +846,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.y = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef;
_local_pos.z = _ekf->states[9] - _filter_ref_offset;
//_local_pos.z_stable = _ekf->states[9];
_local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5];
@@ -829,6 +862,17 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.z_global = false;
_local_pos.yaw = _att.yaw;
if (!isfinite(_local_pos.x) ||
!isfinite(_local_pos.y) ||
!isfinite(_local_pos.z) ||
!isfinite(_local_pos.vx) ||
!isfinite(_local_pos.vy) ||
!isfinite(_local_pos.vz))
{
// bad data, abort publication
return;
}
/* lazily publish the local position only once available */
if (_local_pos_pub != nullptr) {
/* publish the attitude setpoint */
@@ -862,7 +906,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
}
/* local pos alt is negative, change sign and add alt offsets */
_global_pos.alt = (-_local_pos.z) - _baro_gps_offset;
_global_pos.alt = (-_local_pos.z) - _filter_ref_offset - _baro_gps_offset;
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
@@ -877,6 +921,17 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
if (!isfinite(_global_pos.lat) ||
!isfinite(_global_pos.lon) ||
!isfinite(_global_pos.alt) ||
!isfinite(_global_pos.vel_n) ||
!isfinite(_global_pos.vel_e) ||
!isfinite(_global_pos.vel_d))
{
// bad data, abort publication
return;
}
/* lazily publish the global position only once available */
if (_global_pos_pub != nullptr) {
/* publish the global position */
@@ -1073,8 +1128,9 @@ void AttitudePositionEstimatorEKF::print_status()
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec);
printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt);
printf("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)\n", (double)(_local_pos.z), (double)_global_pos.alt);
printf("filter ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_filter_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);
@@ -1173,7 +1229,7 @@ void AttitudePositionEstimatorEKF::pollData()
if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
(_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
@@ -1212,7 +1268,7 @@ void AttitudePositionEstimatorEKF::pollData()
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) {
if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) {
_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];
@@ -1320,7 +1376,11 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD));
// update LPF
_gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
if (isfinite(filter_step)) {
_gps_alt_filt += filter_step;
}
}
//check if we had a GPS outage for a long time
@@ -1403,15 +1463,19 @@ void AttitudePositionEstimatorEKF::pollData()
}
baro_last = _baro.timestamp;
if(!_baro_init) {
if (!_baro_init) {
_baro_init = true;
_baroAltRef = _baro.altitude;
_baro_alt_filt = _baro.altitude;
}
_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);
float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
if (isfinite(filter_step)) {
_baro_alt_filt += filter_step;
}
perf_count(_perf_baro);
}
@@ -1435,7 +1499,7 @@ void AttitudePositionEstimatorEKF::pollData()
/* fail over to the 2nd mag if we know the first is down */
if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount &&
_sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) &&
mag0.length() > 0.1f) {
_ekf->magData.x = mag0.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
@@ -1463,7 +1527,7 @@ void AttitudePositionEstimatorEKF::pollData()
}
if (last_mag_main != _mag_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main);
mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main);
}
}
@@ -1497,30 +1561,34 @@ int AttitudePositionEstimatorEKF::trip_nan()
float nan_val = 0.0f / 0.0f;
warnx("system not armed, tripping state vector with NaN values");
warnx("system not armed, tripping state vector with NaN");
_ekf->states[5] = nan_val;
usleep(100000);
warnx("tripping covariance #1 with NaN values");
warnx("tripping covariance #1 with NaN");
_ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates
usleep(100000);
warnx("tripping covariance #2 with NaN values");
warnx("tripping covariance #2 with NaN");
_ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates
usleep(100000);
warnx("tripping covariance #3 with NaN values");
warnx("tripping covariance #3 with NaN");
_ekf->P[3][3] = nan_val; // covariance matrix
usleep(100000);
warnx("tripping Kalman gains with NaN values");
warnx("tripping Kalman gains with NaN");
_ekf->Kfusion[0] = nan_val; // Kalman gains
usleep(100000);
warnx("tripping stored states[0] with NaN values");
warnx("tripping stored states[0] with NaN");
_ekf->storedStates[0][0] = nan_val;
usleep(100000);
warnx("tripping states[9] with NaN");
_ekf->states[9] = nan_val;
usleep(100000);
warnx("\nDONE - FILTER STATE:");
print_status();
}