Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

ekf2_main: extract gps mgs fill

Browse Source
This commit is contained in:
bresch
2019-11-13 21:12:47 +01:00
committed by Mathieu Bresciani
parent 5d6e0587db
commit f6a72663fa
1 changed files with 25 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+25 -36
View File
@@ -115,6 +115,7 @@ public:
private:
int getRangeSubIndex(); ///< get subscription index of first downward-facing range sensor
void fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data);
PreFlightChecker _preflt_checker;
void runPreFlightChecks(float dt, const filter_control_status_u &control_status,
@@ -928,24 +929,7 @@ void Ekf2::Run()
vehicle_gps_position_s gps;
if (_gps_subs[0].copy(&gps)) {
_gps_state[0].time_usec = gps.timestamp;
_gps_state[0].lat = gps.lat;
_gps_state[0].lon = gps.lon;
_gps_state[0].alt = gps.alt;
_gps_state[0].yaw = gps.heading;
_gps_state[0].yaw_offset = gps.heading_offset;
_gps_state[0].fix_type = gps.fix_type;
_gps_state[0].eph = gps.eph;
_gps_state[0].epv = gps.epv;
_gps_state[0].sacc = gps.s_variance_m_s;
_gps_state[0].vel_m_s = gps.vel_m_s;
_gps_state[0].vel_ned[0] = gps.vel_n_m_s;
_gps_state[0].vel_ned[1] = gps.vel_e_m_s;
_gps_state[0].vel_ned[2] = gps.vel_d_m_s;
_gps_state[0].vel_ned_valid = gps.vel_ned_valid;
_gps_state[0].nsats = gps.satellites_used;
const float tdop = 0.9f;
_gps_state[0].gdop = sqrtf(gps.hdop * gps.hdop + gps.vdop * gps.vdop + tdop * tdop);
fillGpsMsgWithVehicleGpsPosData(_gps_state[0], gps);
_gps_alttitude_ellipsoid[0] = gps.alt_ellipsoid;
ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100);
@@ -959,24 +943,7 @@ void Ekf2::Run()
vehicle_gps_position_s gps;
if (_gps_subs[1].copy(&gps)) {
_gps_state[1].time_usec = gps.timestamp;
_gps_state[1].lat = gps.lat;
_gps_state[1].lon = gps.lon;
_gps_state[1].alt = gps.alt;
_gps_state[1].yaw = gps.heading;
_gps_state[1].yaw_offset = gps.heading_offset;
_gps_state[1].fix_type = gps.fix_type;
_gps_state[1].eph = gps.eph;
_gps_state[1].epv = gps.epv;
_gps_state[1].sacc = gps.s_variance_m_s;
_gps_state[1].vel_m_s = gps.vel_m_s;
_gps_state[1].vel_ned[0] = gps.vel_n_m_s;
_gps_state[1].vel_ned[1] = gps.vel_e_m_s;
_gps_state[1].vel_ned[2] = gps.vel_d_m_s;
_gps_state[1].vel_ned_valid = gps.vel_ned_valid;
_gps_state[1].nsats = gps.satellites_used;
const float tdop = 0.9f;
_gps_state[0].gdop = sqrtf(gps.hdop * gps.hdop + gps.vdop * gps.vdop + tdop * tdop);
fillGpsMsgWithVehicleGpsPosData(_gps_state[1], gps);
_gps_alttitude_ellipsoid[1] = gps.alt_ellipsoid;
}
}
@@ -1680,6 +1647,28 @@ void Ekf2::Run()
}
}
void Ekf2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data)
{
msg.time_usec = data.timestamp;
msg.lat = data.lat;
msg.lon = data.lon;
msg.alt = data.alt;
msg.yaw = data.heading;
msg.yaw_offset = data.heading_offset;
msg.fix_type = data.fix_type;
msg.eph = data.eph;
msg.epv = data.epv;
msg.sacc = data.s_variance_m_s;
msg.vel_m_s = data.vel_m_s;
msg.vel_ned[0] = data.vel_n_m_s;
msg.vel_ned[1] = data.vel_e_m_s;
msg.vel_ned[2] = data.vel_d_m_s;
msg.vel_ned_valid = data.vel_ned_valid;
msg.nsats = data.satellites_used;
const float tdop = 0.9f; // TDOP is usually never worse than 0.9
msg.gdop = sqrtf(data.hdop * data.hdop + data.vdop * data.vdop + tdop * tdop);
}
void Ekf2::runPreFlightChecks(const float dt,
const filter_control_status_u &control_status,
const vehicle_status_s &vehicle_status,