fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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:34:58 +01:00 committed by Daniel Agar
parent 97423136d1
commit 1df52df27d
4 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/common.h
View File

@ -134,7 +134,7 @@ enum class ImuCtrl : uint8_t {
GravityVector = (1<<2),
};
enum GnssCtrl : uint8_t {
enum class GnssCtrl : uint8_t {
HPOS = (1<<0),
VPOS = (1<<1),
VEL = (1<<2),
@ -325,7 +325,7 @@ struct parameters {
#endif // CONFIG_EKF2_BAROMETER
#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)
Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m)

4
src/modules/ekf2/EKF/gnss_height_control.cpp
View File

@ -86,7 +86,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
}
// 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
&& _NED_origin_initialised
&& _gps_checks_passed;
@ -113,7 +113,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
bias_est.setBias(_state.pos(2) + measurement);
// 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
resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise)));

6
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -99,7 +99,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
vel_obs_var, // observation variance
math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate
_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
const Vector2f position{gps_sample.pos};
@ -121,7 +121,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
pos_obs_var, // observation variance
math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
_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
// 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)
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) {
stopGpsYawFusion();

12
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
View File

@ -37,12 +37,12 @@ void EkfWrapper::setGpsHeightRef()
void EkfWrapper::enableGpsHeightFusion()
{
_ekf_params->gnss_ctrl |= GnssCtrl::VPOS;
_ekf_params->gnss_ctrl |= static_cast<int32_t>(GnssCtrl::VPOS);
}
void EkfWrapper::disableGpsHeightFusion()
{
_ekf_params->gnss_ctrl &= ~GnssCtrl::VPOS;
_ekf_params->gnss_ctrl &= ~static_cast<int32_t>(GnssCtrl::VPOS);
}
bool EkfWrapper::isIntendingGpsHeightFusion() const
@ -102,12 +102,12 @@ bool EkfWrapper::isIntendingBetaFusion() const
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()
{
_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
@ -117,12 +117,12 @@ bool EkfWrapper::isIntendingGpsFusion() const
void EkfWrapper::enableGpsHeadingFusion()
{
_ekf_params->gnss_ctrl |= GnssCtrl::YAW;
_ekf_params->gnss_ctrl |= static_cast<int32_t>(GnssCtrl::YAW);
}
void EkfWrapper::disableGpsHeadingFusion()
{
_ekf_params->gnss_ctrl &= ~GnssCtrl::YAW;
_ekf_params->gnss_ctrl &= ~static_cast<int32_t>(GnssCtrl::YAW);
}
bool EkfWrapper::isIntendingGpsHeadingFusion() const