Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_NavEKF3: added two more EK3_OPTION bits #28750

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions Tools/Replay/LR_MsgHandler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,9 @@ void LR_MsgHandler_REV2::process_message(uint8_t *msgbytes)
break;
case AP_DAL::Event::setSourceSet0 ... AP_DAL::Event::setSourceSet2:
break;
case AP_DAL::Event::EK3GPSDisable:
case AP_DAL::Event::EK3GPSEnable:
break;
}
if (replay_force_ekf3) {
LR_MsgHandler_REV3 h{f, ekf2, ekf3};
Expand Down Expand Up @@ -133,6 +136,12 @@ void LR_MsgHandler_REV3::process_message(uint8_t *msgbytes)
case AP_DAL::Event::setSourceSet0 ... AP_DAL::Event::setSourceSet2:
ekf3.setPosVelYawSourceSet(uint8_t(msg.event)-uint8_t(AP_DAL::Event::setSourceSet0));
break;
case AP_DAL::Event::EK3GPSDisable:
ekf3.force_gps_disable(true);
break;
case AP_DAL::Event::EK3GPSEnable:
ekf3.force_gps_disable(false);
break;
}

if (replay_force_ekf2) {
Expand Down
8 changes: 8 additions & 0 deletions libraries/AP_AHRS/AP_AHRS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -3629,6 +3629,14 @@ float AP_AHRS::get_air_density_ratio(void) const
return 1.0 / sq(eas2tas);
}

#if HAL_NAVEKF3_AVAILABLE
// AUX switch support for disabling GPS only in EKF3
void AP_AHRS::gps_disable_ek3(bool gps_disable)
{
EKF3.force_gps_disable(gps_disable);
}
#endif

// singleton instance
AP_AHRS *AP_AHRS::_singleton;

Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_AHRS/AP_AHRS.h
Original file line number Diff line number Diff line change
Expand Up @@ -683,6 +683,9 @@ class AP_AHRS {
// get access to an EKFGSF_yaw estimator
const EKFGSF_yaw *get_yaw_estimator(void) const;

// AUX switch support for disabling GPS only in EKF3
void gps_disable_ek3(bool gps_disable);

private:

// roll/pitch/yaw euler angles, all in radians
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_DAL/AP_DAL.h
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@ class AP_DAL {
setSourceSet0 = 13,
setSourceSet1 = 14,
setSourceSet2 = 15,
EK3GPSDisable = 16,
EK3GPSEnable = 17,
};

// must remain the same as AP_AHRS_VehicleClass numbers-wise
Expand Down
23 changes: 21 additions & 2 deletions libraries/AP_NavEKF3/AP_NavEKF3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -738,8 +738,8 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {

// @Param: OPTIONS
// @DisplayName: Optional EKF behaviour
// @Description: This controls optional EKF behaviour. Setting JammingExpected will change the EKF nehaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds to prevent bad
// @Bitmask: 0:JammingExpected
// @Description: This controls optional EKF behaviour. Setting JammingExpected will change the EKF nehaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds to prevent likely bad GPS data from being fused. The DisableSetLatLng option disables processing of SetLatLng commands provided by an external estimator. The DisableGPSAtStartup option allows for easier GPS denied testing to ensure that no GPS data is processed before an AUX function to disable GPS can be processed.
// @Bitmask: 0:JammingExpected,2:DisableSetLatLng,5:DisableGPSAtStartup
// @User: Advanced
AP_GROUPINFO("OPTIONS", 11, NavEKF3, _options, 0),

Expand Down Expand Up @@ -787,6 +787,14 @@ bool NavEKF3::InitialiseFilter(void)
}
#endif

if (option_is_enabled(Option::DisableGPSAtStartup)) {
// this is equivalent to the auxillary function for GPS
// disable being active at startup. Useful for GPS-denied
// testing to ensure no use is made of the GPS while still
// allowing GPS on EKF2
_gps_disabled = true;
}

if (core == nullptr) {

// don't run multiple filters for 1 IMU
Expand Down Expand Up @@ -1421,6 +1429,10 @@ bool NavEKF3::setLatLng(const Location &loc, float posAccuracy, uint32_t timesta
#if EK3_FEATURE_POSITION_RESET
dal.log_SetLatLng(loc, posAccuracy, timestamp_ms);

if (option_is_enabled(Option::DisableSetLatLng)) {
return false;
}

if (!core) {
return false;
}
Expand Down Expand Up @@ -2086,3 +2098,10 @@ const EKFGSF_yaw *NavEKF3::get_yawEstimator(void) const
}
return nullptr;
}

// force GPS disable on EKF3 only
void NavEKF3::force_gps_disable(bool gps_disable)
{
AP::dal().log_event3(gps_disable?AP_DAL::Event::EK3GPSDisable:AP_DAL::Event::EK3GPSEnable);
_gps_disabled = gps_disable;
}
16 changes: 16 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3.h
Original file line number Diff line number Diff line change
Expand Up @@ -365,6 +365,14 @@ class NavEKF3 {
// get a yaw estimator instance
const EKFGSF_yaw *get_yawEstimator(void) const;

// force GPS disable on EKF3 only
void force_gps_disable(bool gps_disable);

// true when we will not use GPS due to AUX switch disable
bool gps_is_disabled(void) const {
return _gps_disabled;
}

private:
class AP_DAL &dal;

Expand Down Expand Up @@ -455,6 +463,11 @@ class NavEKF3 {
// enum for processing options
enum class Option {
JammingExpected = (1<<0),
// 1 reserved for DisableRangeFusion
DisableSetLatLng = (1<<2),
// 3 reserved for RangeToLocHgtOffset
// 4 reserved for LimitRngToLocUpdate
DisableGPSAtStartup = (1<<5),
};
bool option_is_enabled(Option option) const {
return (_options & (uint32_t)option) != 0;
Expand Down Expand Up @@ -575,4 +588,7 @@ class NavEKF3 {

// position, velocity and yaw source control
AP_NavEKF_Source sources;

// true when we should not use GPS due to AUX switch disable
bool _gps_disabled;
};
18 changes: 16 additions & 2 deletions libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -547,12 +547,25 @@ void NavEKF3_core::readGpsData()
// check for new GPS data
const auto &gps = dal.gps();

if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS) {
return;
}

if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms > 1000 ||
gps.status(selected_gps) < AP_DAL_GPS::GPS_OK_FIX_3D || frontend->_gps_disabled) {
// GPS has dropped lock or data so reset good to align status and force checks to restart
gpsGoodToAlign = false;
lastGpsVelFail_ms = imuSampleTime_ms;
lastGpsVelPass_ms = 0;
waitingForGpsChecks = true;
}

// limit update rate to avoid overflowing the FIFO buffer
if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
return;
}

if (gps.status(selected_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
if (gps.status(selected_gps) < AP_DAL_GPS::GPS_OK_FIX_3D || frontend->_gps_disabled) {
// report GPS fix status
gpsCheckStatus.bad_fix = true;
dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
Expand Down Expand Up @@ -725,6 +738,7 @@ void NavEKF3_core::readGpsYawData()
float yaw_deg, yaw_accuracy_deg;
uint32_t yaw_time_ms;
if (gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D &&
!frontend->_gps_disabled &&
dal.gps().gps_yaw_deg(selected_gps, yaw_deg, yaw_accuracy_deg, yaw_time_ms) &&
yaw_time_ms != yawMeasTime_ms) {
// GPS modules are rather too optimistic about their
Expand Down Expand Up @@ -1156,7 +1170,7 @@ void NavEKF3_core::update_gps_selection(void)
// many GPS sensors available
preferred_gps = core_index;
}
if (gps.status(preferred_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) {
if (gps.status(preferred_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D && !frontend->_gps_disabled) {
// select our preferred_gps if it has a 3D fix, otherwise
// use the primary GPS
selected_gps = preferred_gps;
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -234,7 +234,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
// In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
if(validOrigin) {
auto &gps = dal.gps();
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D) && !frontend->_gps_disabled) {
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
const Location &gpsloc = gps.location(selected_gps);
posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat();
Expand Down Expand Up @@ -348,7 +348,7 @@ bool NavEKF3_core::getLLH(Location &loc) const
bool NavEKF3_core::getGPSLLH(Location &loc) const
{
const auto &gps = dal.gps();
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D)) {
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) && !frontend->_gps_disabled) {
loc = gps.location(selected_gps);
return true;
}
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -473,7 +473,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
update_sensor_selection();

// If we are a plane and don't have GPS lock then don't initialise
if (assume_zero_sideslip() && dal.gps().status(preferred_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
if (assume_zero_sideslip() && (dal.gps().status(preferred_gps) < AP_DAL_GPS::GPS_OK_FIX_3D || frontend->_gps_disabled)) {
dal.snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"EKF3 init failure: No GPS lock");
Expand Down
12 changes: 12 additions & 0 deletions libraries/RC_Channel/RC_Channel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -731,6 +731,9 @@ void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos
#if AP_GRIPPER_ENABLED
case AUX_FUNC::GRIPPER:
#endif
#if HAL_NAVEKF3_AVAILABLE
case AUX_FUNC::GPS_DISABLE_EK3:
#endif
#if AP_INERTIALSENSOR_KILL_IMU_ENABLED
case AUX_FUNC::KILL_IMU1:
case AUX_FUNC::KILL_IMU2:
Expand Down Expand Up @@ -892,6 +895,9 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = {
#if HAL_MOUNT_ENABLED
{ AUX_FUNC::MOUNT_LRF_ENABLE, "Mount LRF Enable"},
#endif
#if HAL_NAVEKF3_AVAILABLE
{ AUX_FUNC::GPS_DISABLE_EK3,"GPSDisableEK3"},
#endif
};

/* lookup the announcement for switch change */
Expand Down Expand Up @@ -1574,6 +1580,12 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch
#endif
break;

#if HAL_NAVEKF3_AVAILABLE
case AUX_FUNC::GPS_DISABLE_EK3:
AP::ahrs().gps_disable_ek3(ch_flag == AuxSwitchPos::HIGH);
break;
#endif

case AUX_FUNC::GPS_DISABLE_YAW:
AP::gps().set_force_disable_yaw(ch_flag == AuxSwitchPos::HIGH);
break;
Expand Down
2 changes: 2 additions & 0 deletions libraries/RC_Channel/RC_Channel.h
Original file line number Diff line number Diff line change
Expand Up @@ -262,6 +262,8 @@ class RC_Channel {
AUTOTUNE_TEST_GAINS = 180, // auto tune tuning switch to test or revert gains


GPS_DISABLE_EK3 = 190, // disable GPS only for EKF3

// inputs from 200 will eventually used to replace RCMAP
ROLL = 201, // roll input
PITCH = 202, // pitch input
Expand Down
Loading