fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 09:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF drop unused timestamp from collect_gps() and pass data by const reference

Browse Source
This commit is contained in:
Daniel Agar
2019-03-05 17:26:19 -05:00
parent 78b899c72d
commit c66f7f4a09
7 changed files with 49 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+14 -14
View File
@@ -190,7 +190,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
}
}
void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps)
{
if (!_initialised || _gps_buffer_fail) {
return;
@@ -210,35 +210,35 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct 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) {
if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) {
gpsSample gps_sample_new;
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
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 = Vector3f(gps.vel_ned);
_gps_speed_valid = gps->vel_ned_valid;
gps_sample_new.sacc = gps->sacc;
gps_sample_new.hacc = gps->eph;
gps_sample_new.vacc = gps->epv;
_gps_speed_valid = gps.vel_ned_valid;
gps_sample_new.sacc = gps.sacc;
gps_sample_new.hacc = gps.eph;
gps_sample_new.vacc = gps.epv;
gps_sample_new.hgt = (float)gps->alt * 1e-3f;
gps_sample_new.hgt = (float)gps.alt * 1e-3f;
gps_sample_new.yaw = gps->yaw;
if (ISFINITE(gps->yaw_offset)) {
_gps_yaw_offset = gps->yaw_offset;
gps_sample_new.yaw = gps.yaw;
if (ISFINITE(gps.yaw_offset)) {
_gps_yaw_offset = gps.yaw_offset;
} else {
_gps_yaw_offset = 0.0f;
}
// Only calculate the relative position if the WGS-84 location of the origin is set
if (collect_gps(time_usec, gps)) {
if (collect_gps(gps)) {
float lpos_x = 0.0f;
float lpos_y = 0.0f;
map_projection_project(&_pos_ref, (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;