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: Remove use of camel case variable names

Browse Source 
Also fixes bug in GPS speed accuracy check that was using horizontal position accuracy variable by mistake.
This commit is contained in:
Paul Riseborough 2016-01-28 08:37:45 +11:00
parent cc50d26601
commit 39eef3a2d7
4 changed files with 75 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/ekf.cpp
View File

@ -206,7 +206,7 @@ void Ekf::predictState()
{
if (!_earth_rate_initialised) {
if (_gps_initialised) {
calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad);
calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad);
_earth_rate_initialised = true;
}
}

4
EKF/estimator_base.cpp
View File

@ -181,7 +181,7 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
float lpos_x = 0.0f;
float lpos_y = 0.0f;
map_projection_project(&_posRef, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
gps_sample_new.pos(0) = lpos_x;
gps_sample_new.pos(1) = lpos_y;
gps_sample_new.hgt = gps->alt / 1e3f;
@ -312,7 +312,7 @@ void EstimatorBase::initialiseGPS(struct gps_message *gps)
// Initialise projection
double lat = gps->lat / 1.0e7;
double lon = gps->lon / 1.0e7;
map_projection_init(&_posRef, lat, lon);
map_projection_init(&_pos_ref, lat, lon);
_gps_alt_ref = gps->alt / 1e3f;
_gps_initialised = true;
_last_gps_origin_time_us = _time_last_imu;

34
EKF/estimator_base.h
View File

@ -88,14 +88,14 @@ struct parameters {
// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
// good enough to set a local origin and commence aiding
int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used
float requiredEph = 5.0f; // maximum acceptable horizontal position error
float requiredEpv = 8.0f; // maximum acceptable vertical position error
float requiredSacc = 1.0f; // maximum acceptable speed error
int requiredNsats = 6; // minimum acceptable satellite count
float requiredGDoP = 2.0f; // maximum acceptable geometric dilution of precision
float requiredHdrift = 0.3f; // maximum acceptable horizontal drift speed
float requiredVdrift = 0.5f; // maximum acceptable vertical drift speed
int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used
float req_hacc = 5.0f; // maximum acceptable horizontal position error
float req_vacc = 8.0f; // maximum acceptable vertical position error
float req_sacc = 1.0f; // maximum acceptable speed error
int req_nsats = 6; // minimum acceptable satellite count
float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision
float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed
float req_vdrift = 0.5f; // maximum acceptable vertical drift speed
};
class EstimatorBase
@ -294,11 +294,11 @@ protected:
// variables used for the GPS quality checks
float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s)
float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s)
float _gpsDriftVelD = 0.0f; // GPS down position derivative (m/s)
float _gpsVertVelFilt = 0.0f; // GPS filtered Down velocity (m/s)
float _gpsVelNorthFilt = 0.0f; // GPS filtered North velocity (m/s)
float _gpsVelEastFilt = 0.0f; // GPS filtered East velocity (m/s)
uint64_t _lastGpsFail_us = 0; // last system time in usec that the GPS failed it's checks
float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s)
float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s)
float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s)
float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s)
uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks
public:
void printIMU(struct imuSample *data);
@ -339,13 +339,13 @@ public:
// Variables used to publish the WGS-84 location of the EKF local NED origin
uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec)
struct map_projection_reference_s _posRef = {}; // Contains WGS-84 position latitude and longitude (radians)
struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
float _gps_alt_ref = 0.0f; // WGS-84 height (m)
bool _vehicleArmed = false; // vehicle arm status used to turn off funtionality used on the ground
bool _vehicle_armed = false; // vehicle arm status used to turn off funtionality used on the ground
// publish the status of various GPS quality checks
union gpsCheckFailStatus_u {
union gps_check_fail_status_u {
struct {
uint16_t nsats : 1; // 0 - true if number of satellites used is insufficient
uint16_t gdop : 1; // 1 - true if geometric dilution of precision is insufficient
@ -359,6 +359,6 @@ public:
} flags;
uint16_t value;
};
gpsCheckFailStatus_u _gpsCheckFailStatus;
gps_check_fail_status_u _gps_check_fail_status;
};

110
EKF/gps_checks.cpp
View File

@ -62,119 +62,119 @@
bool EstimatorBase::gps_is_good(struct gps_message *gps)
{
// Check the number of satellites
_gpsCheckFailStatus.flags.nsats = (gps->nsats < _params.requiredNsats);
_gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats);
// Check the geometric dilution of precision
_gpsCheckFailStatus.flags.gdop = (gps->gdop > _params.requiredGDoP);
_gps_check_fail_status.flags.gdop = (gps->gdop > _params.req_gdop);
// Check the reported horizontal position accuracy
_gpsCheckFailStatus.flags.hacc = (gps->eph > _params.requiredEph);
_gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc);
// Check the reported vertical position accuracy
_gpsCheckFailStatus.flags.vacc = (gps->epv > _params.requiredEpv);
_gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc);
// Check the reported speed accuracy
_gpsCheckFailStatus.flags.sacc = (gps->eph > _params.requiredEph);
_gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc);
// Calculate position movement since last measurement
float deltaPosN = 0.0f;
float deltaPosE = 0.0f;
float delta_posN = 0.0f;
float delta_PosE = 0.0f;
double lat = gps->lat * 1.0e-7;
double lon = gps->lon * 1.0e-7;
if (_posRef.init_done) {
map_projection_project(&_posRef, lat, lon, &deltaPosN, &deltaPosE);
if (_pos_ref.init_done) {
map_projection_project(&_pos_ref, lat, lon, &delta_posN, &delta_PosE);
} else {
map_projection_init(&_posRef, lat, lon);
map_projection_init(&_pos_ref, lat, lon);
_gps_alt_ref = gps->alt * 1e-3f;
}
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient
const float filtTimeConst = 10.0f;
float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us)*1e-6f,0.001f),filtTimeConst);
float filterCoef = dt/filtTimeConst;
const float filt_time_const = 10.0f;
float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us)*1e-6f,0.001f),filt_time_const);
float filter_coef = dt/filt_time_const;
// Calculate the horizontal drift velocity components and limit to 10x the threshold
float velLimit = 10.0f * _params.requiredHdrift;
float velN = fminf(fmaxf(deltaPosN / dt, -velLimit), velLimit);
float velE = fminf(fmaxf(deltaPosE / dt, -velLimit), velLimit);
float vel_limit = 10.0f * _params.req_hdrift;
float velN = fminf(fmaxf(delta_posN / dt, -vel_limit), vel_limit);
float velE = fminf(fmaxf(delta_PosE / dt, -vel_limit), vel_limit);
// Apply a low pass filter
_gpsDriftVelN = velN * filterCoef + _gpsDriftVelN * (1.0f - filterCoef);
_gpsDriftVelE = velE * filterCoef + _gpsDriftVelE * (1.0f - filterCoef);
_gpsDriftVelN = velN * filter_coef + _gpsDriftVelN * (1.0f - filter_coef);
_gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef);
// Calculate the horizontal drift speed and fail if too high
// This check can only be used if the vehicle is stationary during alignment
if(_vehicleArmed) {
float driftSpeed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
_gpsCheckFailStatus.flags.hdrift = (driftSpeed > _params.requiredHdrift);
if(_vehicle_armed) {
float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
_gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift);
} else {
_gpsCheckFailStatus.flags.hdrift = false;
_gps_check_fail_status.flags.hdrift = false;
}
// Save current position as the reference for next time
map_projection_init(&_posRef, lat, lon);
map_projection_init(&_pos_ref, lat, lon);
_last_gps_origin_time_us = _time_last_imu;
// Calculate the vertical drift velocity and limit to 10x the threshold
velLimit = 10.0f * _params.requiredVdrift;
float velD = fminf(fmaxf((_gps_alt_ref - gps->alt * 1e-3f) / dt, -velLimit), velLimit);
vel_limit = 10.0f * _params.req_vdrift;
float velD = fminf(fmaxf((_gps_alt_ref - gps->alt * 1e-3f) / dt, -vel_limit), vel_limit);
// Save the current height as the reference for next time
_gps_alt_ref = gps->alt * 1e-3f;
// Apply a low pass filter to the vertical velocity
_gpsDriftVelD = velD * filterCoef + _gpsDriftVelD * (1.0f - filterCoef);
_gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef);
// Fail if the vertical drift speed is too high
// This check can only be used if the vehicle is stationary during alignment
if(_vehicleArmed) {
_gpsCheckFailStatus.flags.vdrift = (fabsf(_gpsDriftVelD) > _params.requiredVdrift);
if(_vehicle_armed) {
_gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift);
} else {
_gpsCheckFailStatus.flags.vdrift = false;
_gps_check_fail_status.flags.vdrift = false;
}
// Check the magnitude of the filtered horizontal GPS velocity
// This check can only be used if the vehicle is stationary during alignment
if (_vehicleArmed) {
velLimit = 10.0f * _params.requiredHdrift;
float velN = fminf(fmaxf(gps->vel_ned[0],-velLimit),velLimit);
float velE = fminf(fmaxf(gps->vel_ned[1],-velLimit),velLimit);
_gpsVelNorthFilt = velN * filterCoef + _gpsVelNorthFilt * (1.0f - filterCoef);
_gpsVelEastFilt = velE * filterCoef + _gpsVelEastFilt * (1.0f - filterCoef);
float horizSpeed = sqrtf(_gpsVelNorthFilt * _gpsVelNorthFilt + _gpsVelEastFilt * _gpsVelEastFilt);
_gpsCheckFailStatus.flags.hspeed = (horizSpeed > _params.requiredHdrift);
if (_vehicle_armed) {
vel_limit = 10.0f * _params.req_hdrift;
float velN = fminf(fmaxf(gps->vel_ned[0],-vel_limit),vel_limit);
float velE = fminf(fmaxf(gps->vel_ned[1],-vel_limit),vel_limit);
_gps_velN_filt = velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef);
_gps_velE_filt = velE * filter_coef + _gps_velE_filt * (1.0f - filter_coef);
float horiz_speed = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt);
_gps_check_fail_status.flags.hspeed = (horiz_speed > _params.req_hdrift);
} else {
_gpsCheckFailStatus.flags.hspeed = false;
_gps_check_fail_status.flags.hspeed = false;
}
// Check the filtered difference between GPS and EKF vertical velocity
velLimit = 10.0f * _params.requiredVdrift;
float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -velLimit), velLimit);
_gpsVertVelFilt = vertVel * filterCoef + _gpsVertVelFilt * (1.0f - filterCoef);
_gpsCheckFailStatus.flags.vspeed = (fabsf(_gpsVertVelFilt) > _params.requiredVdrift);
vel_limit = 10.0f * _params.req_vdrift;
float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -vel_limit), vel_limit);
_gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef);
_gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift);
// assume failed first time through
if (_lastGpsFail_us == 0) {
_lastGpsFail_us = _time_last_imu;
if (_last_gps_fail_us == 0) {
_last_gps_fail_us = _time_last_imu;
}
// if any user selected checks have failed, record the fail time
if (
(_gpsCheckFailStatus.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) ||
(_gpsCheckFailStatus.flags.gdop && (_params.gps_check_mask & MASK_GPS_GDOP)) ||
(_gpsCheckFailStatus.flags.hacc && (_params.gps_check_mask & MASK_GPS_HACC)) ||
(_gpsCheckFailStatus.flags.vacc && (_params.gps_check_mask & MASK_GPS_VACC)) ||
(_gpsCheckFailStatus.flags.sacc && (_params.gps_check_mask & MASK_GPS_SACC)) ||
(_gpsCheckFailStatus.flags.hdrift && (_params.gps_check_mask & MASK_GPS_HDRIFT)) ||
(_gpsCheckFailStatus.flags.vdrift && (_params.gps_check_mask & MASK_GPS_VDRIFT)) ||
(_gpsCheckFailStatus.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
(_gpsCheckFailStatus.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD))
(_gps_check_fail_status.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) ||
(_gps_check_fail_status.flags.gdop && (_params.gps_check_mask & MASK_GPS_GDOP)) ||
(_gps_check_fail_status.flags.hacc && (_params.gps_check_mask & MASK_GPS_HACC)) ||
(_gps_check_fail_status.flags.vacc && (_params.gps_check_mask & MASK_GPS_VACC)) ||
(_gps_check_fail_status.flags.sacc && (_params.gps_check_mask & MASK_GPS_SACC)) ||
(_gps_check_fail_status.flags.hdrift && (_params.gps_check_mask & MASK_GPS_HDRIFT)) ||
(_gps_check_fail_status.flags.vdrift && (_params.gps_check_mask & MASK_GPS_VDRIFT)) ||
(_gps_check_fail_status.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
(_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD))
) {
_lastGpsFail_us = _time_last_imu;
_last_gps_fail_us = _time_last_imu;
}
// continuous period without fail of 10 seconds required to return a healthy status
if (_time_last_imu - _lastGpsFail_us > 1e7) {
if (_time_last_imu - _last_gps_fail_us > 1e7) {
return true;
}
return false;