dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AttPosEKF: Make local_pos output Z ref pos relative

This commit is contained in:
Johan Jansen 2015-03-13 15:27:02 +01:00
parent 435d82dee2
commit 4a8f799e9e
2 changed files with 64 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -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;

122
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -132,70 +132,71 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_wind_pub(-1), _wind_pub(-1),
_att({}), _att({}),
_gyro({}), _gyro({}),
_accel({}), _accel({}),
_mag({}), _mag({}),
_airspeed({}), _airspeed({}),
_baro({}), _baro({}),
_vstatus({}), _vstatus({}),
_global_pos({}), _global_pos({}),
_local_pos({}), _local_pos({}),
_gps({}), _gps({}),
_wind({}), _wind({}),
_distance {}, _distance {},
_landDetector {}, _landDetector {},
_armed {}, _armed {},
_gyro_offsets({}), _gyro_offsets({}),
_accel_offsets({}), _accel_offsets({}),
_mag_offsets({}), _mag_offsets({}),
_sensor_combined {}, _sensor_combined {},
_pos_ref {}, _pos_ref{},
_baro_ref_offset(0.0f), _baro_ref_offset(0.0f),
_baro_gps_offset(0.0f), _baro_gps_offset(0.0f),
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
/* states */ /* states */
_gps_alt_filt(0.0f), _gps_alt_filt(0.0f),
_baro_alt_filt(0.0f), _baro_alt_filt(0.0f),
_covariancePredictionDt(0.0f), _covariancePredictionDt(0.0f),
_gpsIsGood(false), _gpsIsGood(false),
_previousGPSTimestamp(0), _previousGPSTimestamp(0),
_baro_init(false), _baro_init(false),
_gps_initialized(false), _baroAltRef(0.0f),
_filter_start_time(0), _gps_initialized(false),
_last_sensor_timestamp(0), _filter_start_time(0),
_last_run(0), _last_sensor_timestamp(0),
_distance_last_valid(0), _last_run(0),
_gyro_valid(false), _distance_last_valid(0),
_accel_valid(false), _gyro_valid(false),
_mag_valid(false), _accel_valid(false),
_gyro_main(0), _mag_valid(false),
_accel_main(0), _gyro_main(0),
_mag_main(0), _accel_main(0),
_ekf_logging(true), _mag_main(0),
_debug(0), _ekf_logging(true),
_debug(0),
_newHgtData(false), _newHgtData(false),
_newAdsData(false), _newAdsData(false),
_newDataMag(false), _newDataMag(false),
_newRangeData(false), _newRangeData(false),
_mavlink_fd(-1), _mavlink_fd(-1),
_parameters {}, _parameters {},
_parameter_handles {}, _parameter_handles {},
_ekf(nullptr) _ekf(nullptr)
{ {
_last_run = hrt_absolute_time(); _last_run = hrt_absolute_time();
@ -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;
_baro_init = true; if(!_baro_init) {
_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));