fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Changes required to enter POSCTL mode

Browse Source
This commit is contained in:
Paul Riseborough 2016-01-01 11:56:38 +11:00
parent eda69e727f
commit 0cb210d045
2 changed files with 33 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
EKF/estimator_base.cpp
View File

@ -170,6 +170,8 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_gps = gps_sample_new.time_us;
_last_valid_gps_time_us = hrt_absolute_time();
gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
@ -261,8 +263,8 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
_params.baro_delay_ms = 0;
_params.gps_delay_ms = 200;
_params.airspeed_delay_ms = 0;
_params.requiredEph = 200;
_params.requiredEpv = 200;
_params.requiredEph = 500;
_params.requiredEpv = 800;
_params.gyro_noise = 1e-3f;
_params.accel_noise = 1e-1f;
_params.gyro_bias_p_noise = 1e-5f;
@ -335,20 +337,38 @@ void EstimatorBase::initialiseGPS(struct gps_message *gps)
map_projection_init(&_posRef, lat, lon);
_gps_alt_ref = gps->alt / 1e3f;
_gps_initialised = true;
_last_gps_origin_time_us = hrt_absolute_time();
}
}
bool EstimatorBase::gps_is_good(struct gps_message *gps)
{
// go through apm implementation of calcGpsGoodToAlign for fancier checks
if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) {
return true;
// Use a stricter check for initialisation than during flight to avoid complete loss of GPS
if (_gps_initialised) {
if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph * 2) && (gps->epv < _params.requiredEpv * 2)) {
return true;
} else {
return false;
}
} else {
return false;
if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) {
return true;
} else {
return false;
}
}
}
bool EstimatorBase::position_is_valid()
{
// return true if the position estimate is valid
// TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status
return _gps_initialised && (hrt_absolute_time() - _last_valid_gps_time_us) < 5e6;
}
void EstimatorBase::printStoredIMU()
{
printf("---------Printing IMU data buffer------------\n");

11
EKF/estimator_base.h
View File

@ -190,6 +190,8 @@ protected:
float _dt_imu_avg;
uint64_t _imu_time_last;
uint64_t _last_valid_gps_time_us;
imuSample _imu_sample_delayed;
imuSample _imu_down_sampled;
Quaternion
@ -206,9 +208,6 @@ protected:
outputSample _output_new;
imuSample _imu_sample_new;
struct map_projection_reference_s _posRef;
float _gps_alt_ref;
uint64_t _imu_ticks;
@ -266,6 +265,8 @@ public:
void printGps(struct gpsSample *data);
void printStoredGps();
bool position_is_valid();
void copy_quaternion(float *quat) {
for (unsigned i = 0; i < 4; i++) {
quat[i] = _output_new.quat_nominal(i);
@ -284,4 +285,8 @@ public:
void copy_timestamp(uint64_t *time_us) {
*time_us = _imu_time_last;
}
uint64_t _last_gps_origin_time_us;
struct map_projection_reference_s _posRef;
float _gps_alt_ref;
};