aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: implement sensor affinity using EK3_AFFINITY parameter

Browse Source 
this allows the EKF core index to be used to select a GPS/baro/mag
instance. This is an alternative to GPS blending that allows EKF lane
switching to be used to select the right combination of GPS and IMU
add logging to XKFS message
This commit is contained in:
Harshit Kumar Sankhla 2020-06-23 22:05:34 +05:30 committed by Andrew Tridgell
parent 9588a68e1b
commit edc3709653
8 changed files with 267 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -647,6 +647,14 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @User: Advanced
AP_GROUPINFO("ERR_THRESH", 61, NavEKF3, _err_thresh, 0.2),
// @Param: AFFINITY
// @DisplayName: EKF3 Sensor Affinity Options
// @Description: These options control the affinity between sensor instances and EKF cores
// @User: Advanced
// @Bitmask: 0:EnableGPSAffinity,1:EnableBaroAffinity,2:EnableCompassAffinity,3:EnableAirspeedAffinity
// @RebootRequired: True
AP_GROUPINFO("AFFINITY", 62, NavEKF3, _affinity, 0),
AP_GROUPEND
};
@ -1202,6 +1210,39 @@ uint8_t NavEKF3::getActiveMag(int8_t instance) const
}
}
// return the baro in use for the specified instance
uint8_t NavEKF3::getActiveBaro(int8_t instance) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
return core[instance].getActiveBaro();
} else {
return 255;
}
}
// return the GPS in use for the specified instance
uint8_t NavEKF3::getActiveGPS(int8_t instance) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
return core[instance].getActiveGPS();
} else {
return 255;
}
}
// return the airspeed sensor in use for the specified instance
uint8_t NavEKF3::getActiveAirspeed(int8_t instance) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
return core[instance].getActiveAirspeed();
} else {
return 255;
}
}
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
bool NavEKF3::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const

7
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -152,9 +152,12 @@ public:
// An out of range instance (eg -1) returns data for the primary instance
void getMagXYZ(int8_t instance, Vector3f &magXYZ) const;
// return the magnetometer in use for the specified instance
// return the sensor in use for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
uint8_t getActiveMag(int8_t instance) const;
uint8_t getActiveBaro(int8_t instance) const;
uint8_t getActiveGPS(int8_t instance) const;
uint8_t getActiveAirspeed(int8_t instance) const;
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
@ -506,6 +509,7 @@ private:
AP_Int16 _gsfResetDelay; // number of mSec from loss of navigation to requesting a reset using EKF-GSF yaw estimator data
AP_Int8 _gsfResetMaxCount; // maximum number of times the EKF3 is allowed to reset it's yaw to the EKF-GSF estimate
AP_Float _err_thresh; // lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError
AP_Int32 _affinity; // bitmask of sensor affinity options
// Possible values for _flowUse
#define FLOW_USE_NONE 0
@ -616,6 +620,7 @@ private:
void Log_Write_XKF3(uint8_t core, uint64_t time_us) const;
void Log_Write_XKF4(uint8_t core, uint64_t time_us) const;
void Log_Write_XKF5(uint64_t time_us) const;
void Log_Write_XKFS(uint8_t core, uint64_t time_us) const;
void Log_Write_Quaternion(uint8_t core, uint64_t time_us) const;
void Log_Write_Beacon(uint64_t time_us) const;
void Log_Write_BodyOdom(uint64_t time_us) const;

24
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -51,7 +51,6 @@ void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const
Vector3f wind;
Vector3f magNED;
Vector3f magXYZ;
uint8_t magIndex = getActiveMag(_core);
getAccelBias(_core,accelBias);
getWind(_core,wind);
getMagNED(_core,magNED);
@ -70,12 +69,30 @@ void NavEKF3::Log_Write_XKF2(uint8_t _core, uint64_t time_us) const
magD : (int16_t)(magNED.z),
magX : (int16_t)(magXYZ.x),
magY : (int16_t)(magXYZ.y),
magZ : (int16_t)(magXYZ.z),
index : (uint8_t)(magIndex)
magZ : (int16_t)(magXYZ.z)
};
AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
}
void NavEKF3::Log_Write_XKFS(uint8_t _core, uint64_t time_us) const
{
// Write sensor selection EKF packet
uint8_t magIndex = getActiveMag(_core);
uint8_t baroIndex = getActiveBaro(_core);
uint8_t GPSIndex = getActiveGPS(_core);
uint8_t airspeedIndex = getActiveAirspeed(_core);
const struct log_EKFS pkt {
LOG_PACKET_HEADER_INIT(LOG_XKFS_MSG),
time_us : time_us,
core : _core,
mag_index : (uint8_t)(magIndex),
baro_index : (uint8_t)(baroIndex),
gps_index : (uint8_t)(GPSIndex),
airspeed_index : (uint8_t)(airspeedIndex),
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
void NavEKF3::Log_Write_XKF3(uint8_t _core, uint64_t time_us) const
{
// Write third EKF packet
@ -318,6 +335,7 @@ void NavEKF3::Log_Write()
Log_Write_XKF2(i, time_us);
Log_Write_XKF3(i, time_us);
Log_Write_XKF4(i, time_us);
Log_Write_XKFS(i, time_us);
Log_Write_Quaternion(i, time_us);
Log_Write_GSF(i, time_us);
}

147
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -285,7 +285,10 @@ void NavEKF3_core::readMagData()
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
if (magTimeout && (maxCount > 1) &&
!onGround &&
imuSampleTime_ms - ekfStartTime_ms > 30000 &&
!(frontend->_affinity & EKF_AFFINITY_MAG)) {
// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
@ -523,8 +526,8 @@ void NavEKF3_core::readGpsData()
// limit update rate to avoid overflowing the FIFO buffer
const AP_GPS &gps = AP::gps();
if (gps.last_message_time_ms() - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) {
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) {
if (gps.status(selected_gps) >= AP_GPS::GPS_OK_FIX_3D) {
// report GPS fix status
gpsCheckStatus.bad_fix = false;
@ -532,13 +535,13 @@ void NavEKF3_core::readGpsData()
secondLastGpsTime_ms = lastTimeGpsReceived_ms;
// get current fix time
lastTimeGpsReceived_ms = gps.last_message_time_ms();
lastTimeGpsReceived_ms = gps.last_message_time_ms(selected_gps);
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
// ideally we should be using a timing signal from the GPS receiver to set this time
// Use the driver specified delay
float gps_delay_sec = 0;
gps.get_lag(gps_delay_sec);
gps.get_lag(selected_gps, gps_delay_sec);
gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f);
// Correct for the average intersampling delay due to the filter updaterate
@ -548,17 +551,17 @@ void NavEKF3_core::readGpsData()
gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.time_ms);
// Get which GPS we are using for position information
gpsDataNew.sensor_idx = gps.primary_sensor();
gpsDataNew.sensor_idx = selected_gps;
// read the NED velocity from the GPS
gpsDataNew.vel = gps.velocity();
gpsDataNew.vel = gps.velocity(selected_gps);
// Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
// Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw;
if (!gps.speed_accuracy(gpsSpdAccRaw)) {
if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) {
gpsSpdAccuracy = 0.0f;
} else {
gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
@ -567,7 +570,7 @@ void NavEKF3_core::readGpsData()
}
gpsPosAccuracy *= (1.0f - alpha);
float gpsPosAccRaw;
if (!gps.horizontal_accuracy(gpsPosAccRaw)) {
if (!gps.horizontal_accuracy(selected_gps, gpsPosAccRaw)) {
gpsPosAccuracy = 0.0f;
} else {
gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
@ -576,7 +579,7 @@ void NavEKF3_core::readGpsData()
}
gpsHgtAccuracy *= (1.0f - alpha);
float gpsHgtAccRaw;
if (!gps.vertical_accuracy(gpsHgtAccRaw)) {
if (!gps.vertical_accuracy(selected_gps, gpsHgtAccRaw)) {
gpsHgtAccuracy = 0.0f;
} else {
gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw);
@ -585,16 +588,16 @@ void NavEKF3_core::readGpsData()
}
// check if we have enough GPS satellites and increase the gps noise scaler if we don't
if (gps.num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
if (gps.num_sats(selected_gps) >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.0f;
} else if (gps.num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
} else if (gps.num_sats(selected_gps) == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.4f;
} else { // <= 4 satellites or in constant position mode
gpsNoiseScaler = 2.0f;
}
// Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly
if (gps.have_vertical_velocity() && (frontend->_fusionModeGPS == 0) && !frontend->inhibitGpsVertVelUse) {
if (gps.have_vertical_velocity(selected_gps) && (frontend->_fusionModeGPS == 0) && !frontend->inhibitGpsVertVelUse) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;
@ -612,7 +615,7 @@ void NavEKF3_core::readGpsData()
}
// Read the GPS location in WGS-84 lat,long,height coordinates
const struct Location &gpsloc = gps.location();
const struct Location &gpsloc = gps.location(selected_gps);
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
if (gpsGoodToAlign && !validOrigin) {
@ -701,10 +704,10 @@ void NavEKF3_core::readBaroData()
// check to see if baro measurement has changed so we know if a new measurement has arrived
// limit update rate to avoid overflowing the FIFO buffer
const AP_Baro &baro = AP::baro();
if (baro.get_last_update() - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) {
if (baro.get_last_update(selected_baro) - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) {
frontend->logging.log_baro = true;
baroDataNew.hgt = baro.get_altitude();
baroDataNew.hgt = baro.get_altitude(selected_baro);
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
// This prevents negative baro disturbances due to rotor wash ground interaction corrupting the EKF altitude during initial ascent
@ -713,7 +716,7 @@ void NavEKF3_core::readBaroData()
}
// time stamp used to check for new measurement
lastBaroReceived_ms = baro.get_last_update();
lastBaroReceived_ms = baro.get_last_update(selected_baro);
// estimate of time height measurement was taken, allowing for delays
baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms;
@ -789,12 +792,12 @@ void NavEKF3_core::readAirSpdData()
// if airspeed reading is valid and is set by the user to be used and has been updated then
// we take a new reading, convert from EAS to TAS and set the flag letting other functions
// know a new measurement is available
const AP_Airspeed *aspeed = _ahrs->get_airspeed();
if (aspeed &&
aspeed->use() &&
(aspeed->last_update_ms() - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
tasDataNew.tas = aspeed->get_raw_airspeed() * AP::ahrs().get_EAS2TAS();
timeTasReceived_ms = aspeed->last_update_ms();
const auto *airspeed = AP::airspeed();
if (airspeed &&
airspeed->use(selected_airspeed) &&
(airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * _ahrs->get_EAS2TAS();
timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed);
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
// Correct for the average intersampling delay due to the filter update rate
@ -1028,6 +1031,104 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t
storedExtNavVel.push(extNavVelNew);
}
/*
update the GPS selection
*/
void NavEKF3_core::update_gps_selection(void)
{
auto &gps = AP::gps();
// in normal operation use the primary GPS
selected_gps = gps.primary_sensor();
preferred_gps = selected_gps;
if (frontend->_affinity & EKF_AFFINITY_GPS) {
if (core_index < gps.num_sensors() ) {
// always prefer our core_index, unless we don't have that
// many GPS sensors available
preferred_gps = core_index;
}
if (gps.status(preferred_gps) >= AP_GPS::GPS_OK_FIX_3D) {
// select our preferred_gps if it has a 3D fix, otherwise
// use the primary GPS
selected_gps = preferred_gps;
}
}
}
/*
update the mag selection
*/
void NavEKF3_core::update_mag_selection(void)
{
const auto *compass = _ahrs->get_compass();
if (compass == nullptr) {
return;
}
if (frontend->_affinity & EKF_AFFINITY_MAG) {
if (core_index < compass->get_count() &&
compass->healthy(core_index) &&
compass->use_for_yaw(core_index)) {
// use core_index compass if it is healthy
magSelectIndex = core_index;
}
}
}
/*
update the baro selection
*/
void NavEKF3_core::update_baro_selection(void)
{
auto &baro = AP::baro();
// in normal operation use the primary baro
selected_baro = baro.get_primary();
if (frontend->_affinity & EKF_AFFINITY_BARO) {
if (core_index < baro.num_instances() &&
baro.healthy(core_index)) {
// use core_index baro if it is healthy
selected_baro = core_index;
}
}
}
/*
update the airspeed selection
*/
void NavEKF3_core::update_airspeed_selection(void)
{
const auto *arsp = AP::airspeed();
if (arsp == nullptr) {
return;
}
// in normal operation use the primary airspeed sensor
selected_airspeed = arsp->get_primary();
if (frontend->_affinity & EKF_AFFINITY_ARSP) {
if (core_index < arsp->get_num_sensors() &&
arsp->healthy(core_index) &&
arsp->use(core_index)) {
// use core_index airspeed if it is healthy
selected_airspeed = core_index;
}
}
}
/*
update sensor selections
*/
void NavEKF3_core::update_sensor_selection(void)
{
update_gps_selection();
update_mag_selection();
update_baro_selection();
update_airspeed_selection();
}
/*
update timing statistics structure
*/

31
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -266,9 +266,10 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
} else {
// In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
if(validOrigin) {
if ((AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D)) {
auto &gps = AP::gps();
if ((gps.status(selected_gps) >= AP_GPS::GPS_OK_FIX_2D)) {
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
const struct Location &gpsloc = AP::gps().location();
const struct Location &gpsloc = gps.location(selected_gps);
const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc);
posNE.x = tempPosNE.x;
posNE.y = tempPosNE.y;
@ -349,9 +350,9 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
} else {
// we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS
// in this mode we cannot use the EKF states to estimate position so will return the best available data
if ((gps.status() >= AP_GPS::GPS_OK_FIX_2D)) {
if ((gps.status(selected_gps) >= AP_GPS::GPS_OK_FIX_2D)) {
// we have a GPS position fix to return
const struct Location &gpsloc = gps.location();
const struct Location &gpsloc = gps.location(selected_gps);
loc.lat = gpsloc.lat;
loc.lng = gpsloc.lng;
return true;
@ -370,8 +371,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
} else {
// If no origin has been defined for the EKF, then we cannot use its position states so return a raw
// GPS reading if available and return false
if ((gps.status() >= AP_GPS::GPS_OK_FIX_3D)) {
const struct Location &gpsloc = gps.location();
if ((gps.status(selected_gps) >= AP_GPS::GPS_OK_FIX_3D)) {
const struct Location &gpsloc = gps.location(selected_gps);
loc = gpsloc;
loc.relative_alt = 0;
loc.terrain_alt = 0;
@ -458,6 +459,24 @@ uint8_t NavEKF3_core::getActiveMag() const
return (uint8_t)magSelectIndex;
}
// return the index for the active barometer
uint8_t NavEKF3_core::getActiveBaro() const
{
return (uint8_t)selected_baro;
}
// return the index for the active GPS
uint8_t NavEKF3_core::getActiveGPS() const
{
return (uint8_t)selected_gps;
}
// return the index for the active airspeed
uint8_t NavEKF3_core::getActiveAirspeed() const
{
return (uint8_t)selected_airspeed;
}
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
void NavEKF3_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
{

28
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -54,7 +54,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
// This check can only be used when the vehicle is stationary
const AP_GPS &gps = AP::gps();
const struct Location &gpsloc = gps.location(); // Current location
const struct Location &gpsloc = gps.location(preferred_gps); // Current location
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
// calculate time lapsed since last update and limit to prevent numerical errors
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
@ -82,18 +82,18 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
// Check that the vertical GPS vertical velocity is reasonable after noise filtering
bool gpsVertVelFail;
if (gps.have_vertical_velocity() && onGround) {
if (gps.have_vertical_velocity(preferred_gps) && onGround) {
// check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else if ((frontend->_fusionModeGPS == 0) && !gps.have_vertical_velocity()) {
} else if ((frontend->_fusionModeGPS == 0) && !gps.have_vertical_velocity(preferred_gps)) {
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true;
// if we have a 3D fix with no vertical velocity and
// EK3_GPS_TYPE=0 then change it to 1. It means the GPS is not
// capable of giving a vertical velocity
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
if (gps.status(preferred_gps) >= AP_GPS::GPS_OK_FIX_3D) {
frontend->_fusionModeGPS.set(1);
gcs().send_text(MAV_SEVERITY_WARNING, "EK3: Changed EK3_GPS_TYPE to 1");
}
@ -135,7 +135,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
// fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f;
bool hAccFail;
if (gps.horizontal_accuracy(hAcc)) {
if (gps.horizontal_accuracy(preferred_gps, hAcc)) {
hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
} else {
hAccFail = false;
@ -155,7 +155,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
// Check for vertical GPS accuracy
float vAcc = 0.0f;
bool vAccFail = false;
if (gps.vertical_accuracy(vAcc)) {
if (gps.vertical_accuracy(preferred_gps, vAcc)) {
vAccFail = (vAcc > 7.5f * checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
}
// Report check result as a text string and bitmask
@ -182,24 +182,24 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
}
// fail if satellite geometry is poor
bool hdopFail = (gps.get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP);
bool hdopFail = (gps.get_hdop(preferred_gps) > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP);
// Report check result as a text string and bitmask
if (hdopFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop()));
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop(preferred_gps)));
gpsCheckStatus.bad_hdop = true;
} else {
gpsCheckStatus.bad_hdop = false;
}
// fail if not enough sats
bool numSatsFail = (gps.num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS);
bool numSatsFail = (gps.num_sats(preferred_gps) < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS);
// Report check result as a text string and bitmask
if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS numsats %u (needs 6)", gps.num_sats());
"GPS numsats %u (needs 6)", gps.num_sats(preferred_gps));
gpsCheckStatus.bad_sats = true;
} else {
gpsCheckStatus.bad_sats = false;
@ -265,7 +265,7 @@ void NavEKF3_core::calcGpsGoodForFlight(void)
// get the receivers reported speed accuracy
float gpsSpdAccRaw;
if (!AP::gps().speed_accuracy(gpsSpdAccRaw)) {
if (!AP::gps().speed_accuracy(preferred_gps, gpsSpdAccRaw)) {
gpsSpdAccRaw = 0.0f;
}
@ -325,9 +325,9 @@ void NavEKF3_core::detectFlight()
bool largeHgtChange = false;
// trigger at 8 m/s airspeed
if (_ahrs->airspeed_sensor_enabled()) {
const AP_Airspeed *airspeed = _ahrs->get_airspeed();
if (airspeed->get_airspeed() * AP::ahrs().get_EAS2TAS() > 10.0f) {
const auto *arsp = AP::airspeed();
if (arsp && arsp->healthy(selected_airspeed) && arsp->use(selected_airspeed)) {
if (arsp->get_airspeed(selected_airspeed) * _ahrs->get_EAS2TAS() > 10.0f) {
highAirSpd = true;
}
}

10
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -70,7 +70,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
if (frontend->_fusionModeGPS != 3) {
// Wait for the configuration of all GPS units to be confirmed. Until this has occurred the GPS driver cannot provide a correct time delay
float gps_delay_sec = 0;
if (!AP::gps().get_lag(gps_delay_sec)) {
if (!AP::gps().get_lag(selected_gps, gps_delay_sec)) {
if (AP_HAL::millis() - lastInitFailReport_ms > 10000) {
lastInitFailReport_ms = AP_HAL::millis();
// provide an escalating series of messages
@ -483,8 +483,11 @@ after the tilt has stabilised.
*/
bool NavEKF3_core::InitialiseFilterBootstrap(void)
{
// update sensor selection (for affinity)
update_sensor_selection();
// If we are a plane and don't have GPS lock then don't initialise
if (assume_zero_sideslip() && AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) {
if (assume_zero_sideslip() && AP::gps().status(preferred_gps) < AP_GPS::GPS_OK_FIX_3D) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"EKF3 init failure: No GPS lock");
@ -646,6 +649,9 @@ void NavEKF3_core::UpdateFilter(bool predict)
fill_scratch_variables();
// update sensor selection (for affinity)
update_sensor_selection();
// TODO - in-flight restart method
// Check arm status and perform required checks and mode changes

29
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -171,8 +171,11 @@ public:
// return body magnetic field estimates in measurement units / 1000
void getMagXYZ(Vector3f &magXYZ) const;
// return the index for the active magnetometer
// return the index for the active sensors
uint8_t getActiveMag() const;
uint8_t getActiveBaro() const;
uint8_t getActiveGPS() const;
uint8_t getActiveAirspeed() const;
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
@ -1482,4 +1485,28 @@ private:
uint8_t EKFGSF_yaw_reset_count; // number of emergency yaw resets performed
bool EKFGSF_run_filterbank; // true when the filter bank is active
uint8_t EKFGSF_yaw_valid_count; // number of updates since the last invalid yaw estimate
// bits in EK3_AFFINITY
enum ekf_affinity {
EKF_AFFINITY_GPS = (1U<<0),
EKF_AFFINITY_BARO = (1U<<1),
EKF_AFFINITY_MAG = (1U<<2),
EKF_AFFINITY_ARSP = (1U<<3),
};
// update selected_sensors for this core
void update_sensor_selection(void);
void update_gps_selection(void);
void update_mag_selection(void);
void update_baro_selection(void);
void update_airspeed_selection(void);
// selected and preferred sensor instances. We separate selected
// from preferred so that calcGpsGoodToAlign() can ensure the
// preferred sensor is ready. Note that magSelectIndex is used for
// compass selection
uint8_t selected_gps;
uint8_t preferred_gps;
uint8_t selected_baro;
uint8_t selected_airspeed;
};