fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 21:57:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

AttPosEKF: Remove barometer reference altitude

Browse Source
This commit is contained in:
Johan Jansen
2015-03-12 18:17:39 +01:00
parent 211760e3e3
commit 435d82dee2
2 changed files with 13 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
-1
View File
@@ -174,7 +174,6 @@ private:
struct map_projection_reference_s _pos_ref;
float _baro_ref; /**< barometer reference altitude */
float _baro_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;
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+13 -17
View File
@@ -153,7 +153,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_sensor_combined {},
_pos_ref {},
_baro_ref(0.0f),
_baro_ref_offset(0.0f),
_baro_gps_offset(0.0f),
@@ -635,24 +634,26 @@ void AttitudePositionEstimatorEKF::task_main()
// }
/* Initialize the filter first */
if (!_gps_initialized && _gpsIsGood) {
initializeGPS();
} else if (!_ekf->statesInitialised) {
if (!_ekf->statesInitialised) {
// North, East Down position (m)
float initVelNED[3] = {0.0f, 0.0f, 0.0f};
_ekf->posNE[0] = 0.0f;
_ekf->posNE[1] = 0.0f;
_local_pos.ref_alt = _baro_ref;
_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);
} else if (_ekf->statesInitialised) {
} else {
if (!_gps_initialized && _gpsIsGood) {
initializeGPS();
continue;
}
// Check if on ground - status is used by covariance prediction
_ekf->setOnGround(_landDetector.landed);
@@ -716,7 +717,7 @@ void AttitudePositionEstimatorEKF::initializeGPS()
_baro_alt_filt = _baro.altitude;
_ekf->baroHgt = _baro.altitude;
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
_ekf->hgtMea = _ekf->baroHgt;
// Set up position variables correctly
_ekf->GPSstatus = _gps.fix_type;
@@ -857,7 +858,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
}
/* local pos alt is negative, change sign and add alt offsets */
_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
_global_pos.alt = (-_local_pos.z) - _baro_gps_offset;
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
@@ -976,7 +977,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
if (fuseBaro) {
// Could use a blend of GPS and baro alt data if desired
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
_ekf->hgtMea = _ekf->baroHgt;
_ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
@@ -1069,7 +1070,7 @@ 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,
printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (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);
@@ -1398,18 +1399,13 @@ void AttitudePositionEstimatorEKF::pollData()
}
baro_last = _baro.timestamp;
_baro_init = true;
_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);
}