diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-11 16:43:52 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-11 16:43:52 +0100 |
commit | 8d8a66607a9a2069a8533ab8893c6322db76d5f1 (patch) | |
tree | 84f3bcc6f8138a92c95bcac8a654827b9daf813b /src/modules | |
parent | c1e13e5afba251723cec51539ae08840d1fe3b29 (diff) | |
download | px4-firmware-8d8a66607a9a2069a8533ab8893c6322db76d5f1.tar.gz px4-firmware-8d8a66607a9a2069a8533ab8893c6322db76d5f1.tar.bz2 px4-firmware-8d8a66607a9a2069a8533ab8893c6322db76d5f1.zip |
AttPosEKF: Do not publish global position if we have none
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 82 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.h | 2 |
2 files changed, 42 insertions, 42 deletions
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e06e174a5..fbd695eac 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1525,9 +1525,9 @@ void AttitudePositionEstimatorEKF::task_main() _local_pos.vy = _ekf->states[5]; _local_pos.vz = _ekf->states[6]; - _local_pos.xy_valid = _gps_initialized; + _local_pos.xy_valid = _gps_initialized && _gps.fix_type >= 3; _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized; + _local_pos.v_xy_valid = _gps_initialized && _gps.fix_type >= 3; _local_pos.v_z_valid = true; _local_pos.xy_global = _gps_initialized; @@ -1544,53 +1544,53 @@ void AttitudePositionEstimatorEKF::task_main() _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); } - _global_pos.timestamp = _local_pos.timestamp; - - if (_local_pos.xy_global) { - double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); - _global_pos.lat = est_lat; - _global_pos.lon = est_lon; - _global_pos.time_utc_usec = _gps.time_utc_usec; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; - } + //Publish Global Position, but only if it's any good + if(_gps_initialized && _gps.fix_type >= 3 && _gps.eph < _parameters.pos_stddev_threshold * 2.0f) + { + _global_pos.timestamp = _local_pos.timestamp; + + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_utc_usec = _gps.time_utc_usec; + } - if (_local_pos.v_xy_valid) { - _global_pos.vel_n = _local_pos.vx; - _global_pos.vel_e = _local_pos.vy; - } else { - _global_pos.vel_n = 0.0f; - _global_pos.vel_e = 0.0f; - } + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } - /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; - if (_local_pos.v_z_valid) { - _global_pos.vel_d = _local_pos.vz; - } - - /* 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 (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } - _global_pos.yaw = _local_pos.yaw; + /* 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); - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + _global_pos.yaw = _local_pos.yaw; - _global_pos.timestamp = _local_pos.timestamp; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; - /* lazily publish the global position only once available */ - if (_global_pos_pub > 0) { - /* publish the global position */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the global position */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } } if (hrt_elapsed_time(&_wind.timestamp) > 99000) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index b17f1e6ee..5de9d4c5a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -312,7 +312,7 @@ public: static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); + static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); |