forked from Archive/PX4-Autopilot
AttPosEKF: Make local_pos output Z ref pos relative
This commit is contained in:
parent
435d82dee2
commit
4a8f799e9e
|
@ -193,6 +193,7 @@ private:
|
||||||
bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use
|
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
|
uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received
|
||||||
bool _baro_init;
|
bool _baro_init;
|
||||||
|
float _baroAltRef;
|
||||||
bool _gps_initialized;
|
bool _gps_initialized;
|
||||||
hrt_abstime _filter_start_time;
|
hrt_abstime _filter_start_time;
|
||||||
hrt_abstime _last_sensor_timestamp;
|
hrt_abstime _last_sensor_timestamp;
|
||||||
|
|
|
@ -173,6 +173,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
|
||||||
_gpsIsGood(false),
|
_gpsIsGood(false),
|
||||||
_previousGPSTimestamp(0),
|
_previousGPSTimestamp(0),
|
||||||
_baro_init(false),
|
_baro_init(false),
|
||||||
|
_baroAltRef(0.0f),
|
||||||
_gps_initialized(false),
|
_gps_initialized(false),
|
||||||
_filter_start_time(0),
|
_filter_start_time(0),
|
||||||
_last_sensor_timestamp(0),
|
_last_sensor_timestamp(0),
|
||||||
|
@ -810,7 +811,7 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
|
||||||
_local_pos.y = _ekf->states[8];
|
_local_pos.y = _ekf->states[8];
|
||||||
|
|
||||||
// XXX need to announce change of Z reference somehow elegantly
|
// XXX need to announce change of Z reference somehow elegantly
|
||||||
_local_pos.z = _ekf->states[9] - _baro_ref_offset;
|
_local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef;
|
||||||
|
|
||||||
_local_pos.vx = _ekf->states[4];
|
_local_pos.vx = _ekf->states[4];
|
||||||
_local_pos.vy = _ekf->states[5];
|
_local_pos.vy = _ekf->states[5];
|
||||||
|
@ -1399,7 +1400,10 @@ void AttitudePositionEstimatorEKF::pollData()
|
||||||
}
|
}
|
||||||
|
|
||||||
baro_last = _baro.timestamp;
|
baro_last = _baro.timestamp;
|
||||||
|
if(!_baro_init) {
|
||||||
_baro_init = true;
|
_baro_init = true;
|
||||||
|
_baroAltRef = _baro.altitude;
|
||||||
|
}
|
||||||
|
|
||||||
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
|
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue