fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 03:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: move gps_message update to UpdateGpsSample()

Browse Source
This commit is contained in:
Daniel Agar
2020-10-31 15:29:37 -04:00
parent 0db706011a
commit 68f49619b7
2 changed files with 39 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+38 -35
View File
@@ -423,20 +423,7 @@ void EKF2::Run()
UpdateBaroSample(ekf2_timestamps);
if (_vehicle_gps_position_sub.updated()) {
vehicle_gps_position_s gps;
if (_vehicle_gps_position_sub.copy(&gps)) {
gps_message gps_msg{};
fillGpsMsgWithVehicleGpsPosData(gps_msg, gps);
_ekf.setGpsData(gps_msg);
_gps_time_usec = gps_msg.time_usec;
_gps_alttitude_ellipsoid = gps.alt_ellipsoid;
}
}
UpdateGpsSample(ekf2_timestamps);
UpdateAirspeedSample(ekf2_timestamps);
@@ -538,27 +525,6 @@ 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;
msg.pdop = sqrtf(data.hdop * data.hdop + data.vdop * data.vdop);
}
void EKF2::runPreFlightChecks(const float dt,
const filter_control_status_u &control_status,
const estimator_innovations_s &innov,
@@ -1401,6 +1367,43 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &
return new_optical_flow;
}
void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF GPS message
if (_param_ekf2_aid_mask.get() & MASK_USE_GPS) {
vehicle_gps_position_s vehicle_gps_position;
if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
gps_message gps_msg{
.time_usec = vehicle_gps_position.timestamp,
.lat = vehicle_gps_position.lat,
.lon = vehicle_gps_position.lon,
.alt = vehicle_gps_position.alt,
.yaw = vehicle_gps_position.heading,
.yaw_offset = vehicle_gps_position.heading_offset,
.fix_type = vehicle_gps_position.fix_type,
.eph = vehicle_gps_position.eph,
.epv = vehicle_gps_position.epv,
.sacc = vehicle_gps_position.s_variance_m_s,
.vel_m_s = vehicle_gps_position.vel_m_s,
.vel_ned = Vector3f{
vehicle_gps_position.vel_n_m_s,
vehicle_gps_position.vel_e_m_s,
vehicle_gps_position.vel_d_m_s
},
.vel_ned_valid = vehicle_gps_position.vel_ned_valid,
.nsats = vehicle_gps_position.satellites_used,
.pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
+ vehicle_gps_position.vdop * vehicle_gps_position.vdop),
};
_ekf.setGpsData(gps_msg);
_gps_time_usec = gps_msg.time_usec;
_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
}
}
}
void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
{
/* Check and save learned magnetometer bias estimates */
src/modules/ekf2/EKF2.hpp
+1 -1
View File
@@ -121,7 +121,6 @@ private:
void Run() override;
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,
@@ -152,6 +151,7 @@ private:
void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom);
bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow);
void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateMagCalibration(const hrt_abstime &timestamp);