fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 21:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

added library for terrain_estimation

Browse Source 
Conflicts:
	makefiles/nuttx/config_aerocore_default.mk
	makefiles/nuttx/config_px4fmu-v1_default.mk
	makefiles/nuttx/config_px4fmu-v2_default.mk
	makefiles/nuttx/config_px4fmu-v2_multiplatform.mk
	makefiles/posix/config_posix_sitl.mk
This commit is contained in:
tumbili
2015-11-17 22:28:08 +01:00
committed by Roman
parent e0cdf65fb4
commit 3b865624f1
5 changed files with 336 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+3
View File
@@ -69,6 +69,7 @@
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <geo/geo.h>
#include <terrain_estimation/terrain_estimator.h>
#include <systemlib/perf_counter.h>
#include <lib/ecl/validation/data_validator_group.h>
#include "estimator_22states.h"
@@ -278,6 +279,8 @@ private:
AttPosEKF *_ekf;
TerrainEstimator *_terrain_estimator;
/* Low pass filter for attitude rates */
math::LowPassFilter2p _LP_att_P;
math::LowPassFilter2p _LP_att_Q;
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+13 -3
View File
@@ -212,6 +212,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_parameters{},
_parameter_handles{},
_ekf(nullptr),
_terrain_estimator(nullptr),
_LP_att_P(250.0f, 20.0f),
_LP_att_Q(250.0f, 20.0f),
@@ -219,6 +220,8 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
{
_voter_mag.set_timeout(200000);
_terrain_estimator = new TerrainEstimator();
_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
_parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS");
@@ -697,6 +700,10 @@ void AttitudePositionEstimatorEKF::task_main()
// Run EKF data fusion steps
updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
// Run separate terrain estimator
_terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance);
_terrain_estimator->measurement_update(&_gps, &_distance, &_att);
// Publish attitude estimations
publishAttitude();
@@ -997,9 +1004,12 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
}
/* terrain altitude */
_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
if (_terrain_estimator->is_valid()) {
_global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground();
_global_pos.terrain_alt_valid = true;
} else {
_global_pos.terrain_alt_valid = false;
}
_global_pos.yaw = _local_pos.yaw;