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

Remove output argument from calcEarthRateNed

This commit is contained in:
kamilritz 2019-12-25 17:17:06 +01:00 committed by Mathieu Bresciani
parent 9e6d27fafb
commit a21a7cd5b9
3 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/ekf.h
View File

@ -641,7 +641,7 @@ private:
void fuse(float *K, float innovation);
// calculate the earth rotation vector from a given latitude
void calcEarthRateNED(Vector3f &omega, float lat_rad) const;
Vector3f calcEarthRateNED(float lat_rad) const;
// return true id the GPS quality is good enough to set an origin and start aiding
bool gps_is_good(const gps_message &gps);

8
EKF/ekf_helper.cpp
View File

@ -791,11 +791,11 @@ void Ekf::constrainStates()
}
// calculate the earth rotation vector
void Ekf::calcEarthRateNED(Vector3f &omega, float lat_rad) const
Vector3f Ekf::calcEarthRateNED(float lat_rad) const
{
omega(0) = CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad);
omega(1) = 0.0f;
omega(2) = -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad);
return Vector3f{CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad),
0.0f,
-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad)};
}
void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos)

2
EKF/gps_checks.cpp
View File

@ -76,7 +76,7 @@ bool Ekf::collect_gps(const gps_message &gps)
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
_NED_origin_initialised = true;
calcEarthRateNED(_earth_rate_NED, (float)_pos_ref.lat_rad);
_earth_rate_NED = calcEarthRateNED((float)_pos_ref.lat_rad);
_last_gps_origin_time_us = _time_last_imu;
// set the magnetic field data returned by the geo library using the current GPS position