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

ekf2: delete redundant aid src status getters

Browse Source
This commit is contained in:
Daniel Agar
2023-10-18 15:21:51 -04:00
committed by GitHub
parent bdaf0acfca
commit 408c30de13
7 changed files with 275 additions and 356 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
-80
View File
@@ -295,86 +295,6 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
}
#if defined(CONFIG_EKF2_GNSS)
void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
{
hvel[0] = _aid_src_gnss_vel.innovation[0];
hvel[1] = _aid_src_gnss_vel.innovation[1];
vvel = _aid_src_gnss_vel.innovation[2];
hpos[0] = _aid_src_gnss_pos.innovation[0];
hpos[1] = _aid_src_gnss_pos.innovation[1];
vpos = _aid_src_gnss_hgt.innovation;
}
void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
{
hvel[0] = _aid_src_gnss_vel.innovation_variance[0];
hvel[1] = _aid_src_gnss_vel.innovation_variance[1];
vvel = _aid_src_gnss_vel.innovation_variance[2];
hpos[0] = _aid_src_gnss_pos.innovation_variance[0];
hpos[1] = _aid_src_gnss_pos.innovation_variance[1];
vpos = _aid_src_gnss_hgt.innovation_variance;
}
void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
{
hvel = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]);
vvel = _aid_src_gnss_vel.test_ratio[2];
hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]);
vpos = _aid_src_gnss_hgt.test_ratio;
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
{
hvel[0] = _aid_src_ev_vel.innovation[0];
hvel[1] = _aid_src_ev_vel.innovation[1];
vvel = _aid_src_ev_vel.innovation[2];
hpos[0] = _aid_src_ev_pos.innovation[0];
hpos[1] = _aid_src_ev_pos.innovation[1];
vpos = _aid_src_ev_hgt.innovation;
}
void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
{
hvel[0] = _aid_src_ev_vel.innovation_variance[0];
hvel[1] = _aid_src_ev_vel.innovation_variance[1];
vvel = _aid_src_ev_vel.innovation_variance[2];
hpos[0] = _aid_src_ev_pos.innovation_variance[0];
hpos[1] = _aid_src_ev_pos.innovation_variance[1];
vpos = _aid_src_ev_hgt.innovation_variance;
}
void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
{
hvel = fmaxf(_aid_src_ev_vel.test_ratio[0], _aid_src_ev_vel.test_ratio[1]);
vvel = _aid_src_ev_vel.test_ratio[2];
hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]);
vpos = _aid_src_ev_hgt.test_ratio;
}
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_AUXVEL)
void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
{
aux_vel_innov[0] = _aid_src_aux_vel.innovation[0];
aux_vel_innov[1] = _aid_src_aux_vel.innovation[1];
}
void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
{
aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0];
aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
}
#endif // CONFIG_EKF2_AUXVEL
bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
{
origin_time = _pos_ref.getProjectionReferenceTimestamp();