zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Added EPH/EPV min to LPE. (#4915)

sbg
James Goppert 9 years ago committed by Lorenz Meier
parent
04e8b40a5c
commit
5935b18581
1 changed files with 5 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      src/modules/local_position_estimator/sensors/gps.cpp

8
src/modules/local_position_estimator/sensors/gps.cpp
Unescape View File

@ -8,6 +8,8 @@ extern orb_advert_t mavlink_log_pub;
// to initialize // to initialize
static const uint32_t REQ_GPS_INIT_COUNT = 10; static const uint32_t REQ_GPS_INIT_COUNT = 10;
static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s
static const float GPS_EPH_MIN = 2.0; // m, min allowed by gps self reporting
static const float GPS_EPV_MIN = 2.0; // m, min allowed by gps self reporting
void BlockLocalPositionEstimator::gpsInit() void BlockLocalPositionEstimator::gpsInit()
{ {
@ -122,12 +124,12 @@ void BlockLocalPositionEstimator::gpsCorrect()
float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get(); float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get();
float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get(); float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
// if field is not zero, set it to the value provided // if field is not below minimum, set it to the value provided
if (_sub_gps.get().eph > 1e-3f) { if (_sub_gps.get().eph > GPS_EPH_MIN) {
var_xy = _sub_gps.get().eph * _sub_gps.get().eph; var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
} }
if (_sub_gps.get().epv > 1e-3f) { if (_sub_gps.get().epv > GPS_EPV_MIN) {
var_z = _sub_gps.get().epv * _sub_gps.get().epv; var_z = _sub_gps.get().epv * _sub_gps.get().epv;
} }

Write Preview
Loading…
Cancel
Save