Skip to content

Commit

Permalink
dont introduce more GPS namings, GNSS instead
Browse files Browse the repository at this point in the history
  • Loading branch information
haumarco committed Jul 9, 2024
1 parent ef8696f commit 30211ab
Show file tree
Hide file tree
Showing 4 changed files with 10 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -319,19 +319,19 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
_gps_was_fused = ekf_gps_fusion;

if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED)) {
if (!_gps_spoofed) {
_gps_spoofed = true;
if (!_gnss_spoofed) {
_gnss_spoofed = true;

if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "GPS signal spoofed\t");
mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS signal spoofed\t");
}

events::send(events::ID("check_estimator_gps_warning_spoofing"), {events::Log::Alert, events::LogInternal::Info},
"GPS signal spoofed");
events::send(events::ID("check_estimator_gnss_warning_spoofing"), {events::Log::Alert, events::LogInternal::Info},
"GNSS signal spoofed");
}

} else {
_gps_spoofed = false;
_gnss_spoofed = false;
}

if (!context.isArmed() && ekf_gps_check_fail) {
Expand Down Expand Up @@ -699,7 +699,7 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
}
}

void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position)
void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const
{
if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_CRITICAL) {
/* EVENT
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
void checkEstimatorStatusFlags(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
const vehicle_local_position_s &lpos);
void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position);
void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const;
void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position,
Expand Down Expand Up @@ -101,7 +101,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
bool _position_reliant_on_optical_flow{false};

bool _gps_was_fused{false};
bool _gps_spoofed{false};
bool _gnss_spoofed{false};

bool _nav_failure_imminent_warned{false};

Expand Down
1 change: 0 additions & 1 deletion src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,6 @@ void Ekf::collect_gps(const gnssSample &gps)

bool Ekf::runGnssChecks(const gnssSample &gps)
{
// Check GPS spoofing
_gps_check_fail_status.flags.spoofed = gps.spoofed;

// Check the fix type
Expand Down
2 changes: 1 addition & 1 deletion src/modules/ekf2/EKF/common.h
Original file line number Diff line number Diff line change
Expand Up @@ -537,7 +537,7 @@ union gps_check_fail_status_u {
uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground)
uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive
uint16_t spoofed: 1; ///< 10 - true if the GPS data is spoofed
uint16_t spoofed: 1; ///< 10 - true if the GNSS data is spoofed
} flags;
uint16_t value;
};
Expand Down

0 comments on commit 30211ab

Please sign in to comment.