dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Update gps interface

This commit is contained in:
kamilritz 2020-01-21 14:45:31 +01:00 committed by Paul Riseborough
parent a19c29e708
commit 74ec80cdc7
5 changed files with 17 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/common.h
View File

@ -67,7 +67,7 @@ struct gps_message {
float epv; ///< GPS vertical position accuracy in m
float sacc; ///< GPS speed accuracy in m/s
float vel_m_s; ///< GPS ground speed (m/sec)
float vel_ned[3]; ///< GPS ground speed NED - TODO: make Vector3f
Vector3f vel_ned; ///< GPS ground speed NED - TODO: make Vector3f
bool vel_ned_valid; ///< GPS ground speed is valid
uint8_t nsats; ///< number of satellites used
float pdop; ///< position dilution of precision

14
EKF/estimator_interface.cpp
View File

@ -182,7 +182,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
}
}
void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps)
void EstimatorInterface::setGpsData(const gps_message &gps)
{
if (!_initialised || _gps_buffer_fail) {
return;
@ -202,15 +202,17 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps)
// limit data rate to prevent data being lost
bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) {
// TODO: remove checks that are not timing related
if (((gps.time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) {
_time_last_gps = gps.time_usec;
gpsSample gps_sample_new;
gps_sample_new.time_us = gps.time_usec - _params.gps_delay_ms * 1000;
gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_gps = time_usec;
gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
gps_sample_new.vel = Vector3f(gps.vel_ned);
gps_sample_new.vel = gps.vel_ned;
_gps_speed_valid = gps.vel_ned_valid;
gps_sample_new.sacc = gps.sacc;

2
EKF/estimator_interface.h
View File

@ -179,7 +179,7 @@ public:
void setMagData(uint64_t time_usec, float (&data)[3]);
void setGpsData(uint64_t time_usec, const gps_message &gps);
void setGpsData(const gps_message &gps);
void setBaroData(uint64_t time_usec, float data);

8
EKF/gps_checks.cpp
View File

@ -175,9 +175,9 @@ bool Ekf::gps_is_good(const gps_message &gps)
_gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift);
// Check the magnitude of the filtered horizontal GPS velocity
Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned[0], gps.vel_ned[1]), // TODO: when vel_ned vector3f use .xy()
-10.0f * _params.req_hdrift,
10.0f * _params.req_hdrift);
Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()),
-10.0f * _params.req_hdrift,
10.0f * _params.req_hdrift);
_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
_gps_drift_metrics[2] = _gps_velNE_filt.norm();
_gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift);
@ -204,7 +204,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
// Check the filtered difference between GPS and EKF vertical velocity
float vz_diff_limit = 10.0f * _params.req_vdrift;
float vertVel = fminf(fmaxf((gps.vel_ned[2] - _state.vel(2)), -vz_diff_limit), vz_diff_limit);
float vertVel = fminf(fmaxf((gps.vel_ned(2) - _state.vel(2)), -vz_diff_limit), vz_diff_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);

10
test/sensor_simulator/gps.cpp
View File

@ -16,7 +16,7 @@ Gps::~Gps()
void Gps::send(uint64_t time)
{
_gps_data.time_usec = time;
_ekf->setGpsData(time, _gps_data);
_ekf->setGpsData(_gps_data);
}
void Gps::setData(const gps_message& gps)
@ -41,9 +41,7 @@ void Gps::setLongitude(int32_t lon)
void Gps::setVelocity(const Vector3f& vel)
{
_gps_data.vel_ned[0] = vel(0);
_gps_data.vel_ned[1] = vel(1);
_gps_data.vel_ned[2] = vel(2);
_gps_data.vel_ned = vel;
}
void Gps::stepHeightByMeters(float hgt_change)
@ -79,9 +77,7 @@ gps_message Gps::getDefaultGpsData()
gps_data.epv = 0.8f;
gps_data.sacc = 0.2f;
gps_data.vel_m_s = 0.0;
gps_data.vel_ned[0] = 0.0f;
gps_data.vel_ned[1] = 0.0f;
gps_data.vel_ned[2] = 0.0f;
gps_data.vel_ned.setZero();
gps_data.vel_ned_valid = 1;
gps_data.nsats = 16;
gps_data.pdop = 0.0f;