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
|
||||
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;
|
||||
|
|
|
@ -132,70 +132,71 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
|
|||
_wind_pub(-1),
|
||||
|
||||
_att({}),
|
||||
_gyro({}),
|
||||
_accel({}),
|
||||
_mag({}),
|
||||
_airspeed({}),
|
||||
_baro({}),
|
||||
_vstatus({}),
|
||||
_global_pos({}),
|
||||
_local_pos({}),
|
||||
_gps({}),
|
||||
_wind({}),
|
||||
_distance {},
|
||||
_landDetector {},
|
||||
_armed {},
|
||||
_gyro({}),
|
||||
_accel({}),
|
||||
_mag({}),
|
||||
_airspeed({}),
|
||||
_baro({}),
|
||||
_vstatus({}),
|
||||
_global_pos({}),
|
||||
_local_pos({}),
|
||||
_gps({}),
|
||||
_wind({}),
|
||||
_distance {},
|
||||
_landDetector {},
|
||||
_armed {},
|
||||
|
||||
_gyro_offsets({}),
|
||||
_accel_offsets({}),
|
||||
_mag_offsets({}),
|
||||
_gyro_offsets({}),
|
||||
_accel_offsets({}),
|
||||
_mag_offsets({}),
|
||||
|
||||
_sensor_combined {},
|
||||
_sensor_combined {},
|
||||
|
||||
_pos_ref {},
|
||||
_baro_ref_offset(0.0f),
|
||||
_baro_gps_offset(0.0f),
|
||||
_pos_ref{},
|
||||
_baro_ref_offset(0.0f),
|
||||
_baro_gps_offset(0.0f),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
|
||||
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
|
||||
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
|
||||
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
|
||||
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
|
||||
_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
|
||||
_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
|
||||
_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
|
||||
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
|
||||
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
|
||||
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
|
||||
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
|
||||
_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
|
||||
_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
|
||||
_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
|
||||
|
||||
/* states */
|
||||
_gps_alt_filt(0.0f),
|
||||
_baro_alt_filt(0.0f),
|
||||
_covariancePredictionDt(0.0f),
|
||||
_gpsIsGood(false),
|
||||
_previousGPSTimestamp(0),
|
||||
_baro_init(false),
|
||||
_gps_initialized(false),
|
||||
_filter_start_time(0),
|
||||
_last_sensor_timestamp(0),
|
||||
_last_run(0),
|
||||
_distance_last_valid(0),
|
||||
_gyro_valid(false),
|
||||
_accel_valid(false),
|
||||
_mag_valid(false),
|
||||
_gyro_main(0),
|
||||
_accel_main(0),
|
||||
_mag_main(0),
|
||||
_ekf_logging(true),
|
||||
_debug(0),
|
||||
/* states */
|
||||
_gps_alt_filt(0.0f),
|
||||
_baro_alt_filt(0.0f),
|
||||
_covariancePredictionDt(0.0f),
|
||||
_gpsIsGood(false),
|
||||
_previousGPSTimestamp(0),
|
||||
_baro_init(false),
|
||||
_baroAltRef(0.0f),
|
||||
_gps_initialized(false),
|
||||
_filter_start_time(0),
|
||||
_last_sensor_timestamp(0),
|
||||
_last_run(0),
|
||||
_distance_last_valid(0),
|
||||
_gyro_valid(false),
|
||||
_accel_valid(false),
|
||||
_mag_valid(false),
|
||||
_gyro_main(0),
|
||||
_accel_main(0),
|
||||
_mag_main(0),
|
||||
_ekf_logging(true),
|
||||
_debug(0),
|
||||
|
||||
_newHgtData(false),
|
||||
_newAdsData(false),
|
||||
_newDataMag(false),
|
||||
_newRangeData(false),
|
||||
_newHgtData(false),
|
||||
_newAdsData(false),
|
||||
_newDataMag(false),
|
||||
_newRangeData(false),
|
||||
|
||||
_mavlink_fd(-1),
|
||||
_parameters {},
|
||||
_parameter_handles {},
|
||||
_ekf(nullptr)
|
||||
_mavlink_fd(-1),
|
||||
_parameters {},
|
||||
_parameter_handles {},
|
||||
_ekf(nullptr)
|
||||
{
|
||||
_last_run = hrt_absolute_time();
|
||||
|
||||
|
@ -810,7 +811,7 @@ 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;
|
||||
_local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef;
|
||||
|
||||
_local_pos.vx = _ekf->states[4];
|
||||
_local_pos.vy = _ekf->states[5];
|
||||
|
@ -1399,7 +1400,10 @@ void AttitudePositionEstimatorEKF::pollData()
|
|||
}
|
||||
|
||||
baro_last = _baro.timestamp;
|
||||
_baro_init = true;
|
||||
if(!_baro_init) {
|
||||
_baro_init = true;
|
||||
_baroAltRef = _baro.altitude;
|
||||
}
|
||||
|
||||
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
|
||||
|
||||
|
|
Loading…
Reference in New Issue