Skip to content

Commit

Permalink
AttPosEKF: Remove barometer reference altitude
Browse files Browse the repository at this point in the history
  • Loading branch information
Zefz committed Mar 12, 2015
1 parent 211760e commit 435d82d
Show file tree
Hide file tree
Showing 2 changed files with 13 additions and 18 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,6 @@ class AttitudePositionEstimatorEKF

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;
Expand Down
30 changes: 13 additions & 17 deletions src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -153,7 +153,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_sensor_combined {},

_pos_ref {},
_baro_ref(0.0f),
_baro_ref_offset(0.0f),
_baro_gps_offset(0.0f),

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
}

Expand Down

0 comments on commit 435d82d

Please sign in to comment.