fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 17:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: scope GnssCtrl enum

Browse Source
This commit is contained in:
bresch
2023-12-05 11:22:59 -05:00
committed by Daniel Agar
parent 97423136d1
commit 1df52df27d
4 changed files with 13 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/common.h
+2 -2
View File
@@ -134,7 +134,7 @@ enum class ImuCtrl : uint8_t {
GravityVector = (1<<2), GravityVector = (1<<2),
}; };
enum GnssCtrl : uint8_t { enum class GnssCtrl : uint8_t {
HPOS = (1<<0), HPOS = (1<<0),
VPOS = (1<<1), VPOS = (1<<1),
VEL = (1<<2), VEL = (1<<2),
@@ -325,7 +325,7 @@ struct parameters {
#endif // CONFIG_EKF2_BAROMETER #endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS) #if defined(CONFIG_EKF2_GNSS)
int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL}; int32_t gnss_ctrl{static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL)};
float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m) Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m)
src/modules/ekf2/EKF/gnss_height_control.cpp
+2 -2
View File
@@ -86,7 +86,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
} }
// determine if we should use height aiding // determine if we should use height aiding
const bool continuing_conditions_passing = (_params.gnss_ctrl & GnssCtrl::VPOS) const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
&& measurement_valid && measurement_valid
&& _NED_origin_initialised && _NED_origin_initialised
&& _gps_checks_passed; && _gps_checks_passed;
@@ -113,7 +113,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
bias_est.setBias(_state.pos(2) + measurement); bias_est.setBias(_state.pos(2) + measurement);
// reset vertical velocity // reset vertical velocity
if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & GnssCtrl::VEL)) { if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))) {
// use 1.5 as a typical ratio of vacc/hacc // use 1.5 as a typical ratio of vacc/hacc
resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise))); resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise)));
src/modules/ekf2/EKF/gps_control.cpp
+3 -3
View File
@@ -99,7 +99,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
vel_obs_var, // observation variance vel_obs_var, // observation variance
math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate
_aid_src_gnss_vel); _aid_src_gnss_vel);
const bool gnss_vel_enabled = (_params.gnss_ctrl & GnssCtrl::VEL); const bool gnss_vel_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL));
// GNSS position // GNSS position
const Vector2f position{gps_sample.pos}; const Vector2f position{gps_sample.pos};
@@ -121,7 +121,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
pos_obs_var, // observation variance pos_obs_var, // observation variance
math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
_aid_src_gnss_pos); _aid_src_gnss_pos);
const bool gnss_pos_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
// Determine if we should use GPS aiding for velocity and horizontal position // Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
@@ -310,7 +310,7 @@ bool Ekf::shouldResetGpsFusion() const
#if defined(CONFIG_EKF2_GNSS_YAW) #if defined(CONFIG_EKF2_GNSS_YAW)
void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing) void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
{ {
if (!(_params.gnss_ctrl & GnssCtrl::YAW) if (!(_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
|| _control_status.flags.gps_yaw_fault) { || _control_status.flags.gps_yaw_fault) {
stopGpsYawFusion(); stopGpsYawFusion();
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+6 -6
View File
@@ -37,12 +37,12 @@ void EkfWrapper::setGpsHeightRef()
void EkfWrapper::enableGpsHeightFusion() void EkfWrapper::enableGpsHeightFusion()
{ {
_ekf_params->gnss_ctrl |= GnssCtrl::VPOS; _ekf_params->gnss_ctrl |= static_cast<int32_t>(GnssCtrl::VPOS);
} }
void EkfWrapper::disableGpsHeightFusion() void EkfWrapper::disableGpsHeightFusion()
{ {
_ekf_params->gnss_ctrl &= ~GnssCtrl::VPOS; _ekf_params->gnss_ctrl &= ~static_cast<int32_t>(GnssCtrl::VPOS);
} }
bool EkfWrapper::isIntendingGpsHeightFusion() const bool EkfWrapper::isIntendingGpsHeightFusion() const
@@ -102,12 +102,12 @@ bool EkfWrapper::isIntendingBetaFusion() const
void EkfWrapper::enableGpsFusion() void EkfWrapper::enableGpsFusion()
{ {
_ekf_params->gnss_ctrl |= GnssCtrl::HPOS | GnssCtrl::VEL; _ekf_params->gnss_ctrl |= static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL);
} }
void EkfWrapper::disableGpsFusion() void EkfWrapper::disableGpsFusion()
{ {
_ekf_params->gnss_ctrl &= ~(GnssCtrl::HPOS | GnssCtrl::VEL); _ekf_params->gnss_ctrl &= ~(static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL));
} }
bool EkfWrapper::isIntendingGpsFusion() const bool EkfWrapper::isIntendingGpsFusion() const
@@ -117,12 +117,12 @@ bool EkfWrapper::isIntendingGpsFusion() const
void EkfWrapper::enableGpsHeadingFusion() void EkfWrapper::enableGpsHeadingFusion()
{ {
_ekf_params->gnss_ctrl |= GnssCtrl::YAW; _ekf_params->gnss_ctrl |= static_cast<int32_t>(GnssCtrl::YAW);
} }
void EkfWrapper::disableGpsHeadingFusion() void EkfWrapper::disableGpsHeadingFusion()
{ {
_ekf_params->gnss_ctrl &= ~GnssCtrl::YAW; _ekf_params->gnss_ctrl &= ~static_cast<int32_t>(GnssCtrl::YAW);
} }
bool EkfWrapper::isIntendingGpsHeadingFusion() const bool EkfWrapper::isIntendingGpsHeadingFusion() const