Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Enable use of GPS delay value from returned by the driver 

Use the time delay returned by the GPS driver.
Wait long enough for the GPS configuration to be determined, but time out after 30 seconds and warn the user that a default value for time delay will be used.
This commit is contained in:
priseborough 2016-12-19 11:43:11 +11:00 committed by Randy Mackay
parent bdc0630ccf
commit b862f0d7ad
5 changed files with 57 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -182,14 +182,9 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m
AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
// @Param: GPS_DELAY
// @DisplayName: GPS measurement delay (msec)
// @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements. The autpilot should be restarted after adjusting this parameter.
// @Range: 0 250
// @Increment: 10
// @User: Advanced
// @Units: msec
AP_GROUPINFO("GPS_DELAY", 8, NavEKF3, _gpsDelay_ms, 150),
// 8 previously used for EKF3_GPS_DELAY parameter that has been deprecated.
// The EKF now takes its GPs delay form the GPS library with the default delays
// specified by the GPS_DELAY and GPS_DELAY2 parameters.
// Height measurement parameters
@ -605,33 +600,43 @@ bool NavEKF3::InitialiseFilter(void)
imuSampleTime_us = AP_HAL::micros64();
// see if we will be doing logging
DataFlash_Class *dataflash = DataFlash_Class::instance();
if (dataflash != nullptr) {
logging.enabled = dataflash->log_replay();
}
if (core == nullptr) {
// see if we will be doing logging
DataFlash_Class *dataflash = DataFlash_Class::instance();
if (dataflash != nullptr) {
logging.enabled = dataflash->log_replay();
}
// don't run multiple filters for 1 IMU
const AP_InertialSensor &ins = _ahrs->get_ins();
uint8_t mask = (1U<<ins.get_accel_count())-1;
_imuMask.set(_imuMask.get() & mask);
// count IMUs from mask
// initialise the setup variables
for (uint8_t i=0; i<7; i++) {
coreSetupRequired[i] = false;
coreImuIndex[i] = 0;
}
num_cores = 0;
// count IMUs from mask
for (uint8_t i=0; i<7; i++) {
if (_imuMask & (1U<<i)) {
coreSetupRequired[num_cores] = true;
coreImuIndex[num_cores] = i;
num_cores++;
}
}
// check if there is enough memory to create the EKF cores
if (hal.util->available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF3: not enough memory");
_enable.set(0);
return false;
}
// create the cores
core = new NavEKF3_core[num_cores];
if (core == nullptr) {
_enable.set(0);
@ -639,22 +644,24 @@ bool NavEKF3::InitialiseFilter(void)
return false;
}
// set the IMU index for the cores
num_cores = 0;
for (uint8_t i=0; i<7; i++) {
if (_imuMask & (1U<<i)) {
if(!core[num_cores].setup_core(this, i, num_cores)) {
return false;
}
num_cores++;
}
}
// Set the primary initially to be the lowest index
primary = 0;
}
// initialse the cores. We return success only if all cores
// Set up any cores that have been created
// This specifies the IMU to be used and creates the data buffers
// If we are unble to set up a core, return false and try again next time the function is called
for (uint8_t core_index=0; core_index<num_cores; core_index++) {
if (coreSetupRequired[core_index]) {
coreSetupRequired[core_index] = !core[core_index].setup_core(this, coreImuIndex[core_index], core_index);
if(coreSetupRequired[core_index]) {
return false;
}
}
}
// Set the primary initially to be the lowest index
primary = 0;
// initialise the cores. We return success only if all cores
// initialise successfully
bool ret = true;
for (uint8_t i=0; i<num_cores; i++) {

4
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -327,7 +327,6 @@ private:
AP_Float _accNoise; // accelerometer process noise : m/s^2
AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s
AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2
AP_Int16 _gpsDelay_ms; // effective average delay of GPS measurements relative to inertial measurement (msec)
AP_Int16 _hgtDelay_ms; // effective average delay of Height measurements relative to inertial measurements (msec)
AP_Int8 _fusionModeGPS; // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity
AP_Int16 _gpsVelInnovGate; // Percentage number of standard deviations applied to GPS velocity innovation consistency check
@ -423,6 +422,9 @@ private:
bool runCoreSelection; // true when the primary core has stabilised and the core selection logic can be started
bool coreSetupRequired[7]; // true when this core index needs to be setup
uint8_t coreImuIndex[7]; // IMU index used by this core
// update the yaw reset data to capture changes due to a lane switch
// new_primary - index of the ekf instance that we are about to switch to as the primary
// old_primary - index of the ekf instance that we are currently using as the primary

3
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -411,7 +411,8 @@ void NavEKF3_core::readGpsData()
// 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
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms;
// Use the driver specified delay
gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(_ahrs->get_gps().get_lag() * 1000.0f);
// Correct for the average intersampling delay due to the filter updaterate
gpsDataNew.time_ms -= localFilterTimeStep_ms/2;

16
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -36,6 +36,7 @@ NavEKF3_core::NavEKF3_core(void) :
_perf_test[8] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test8");
_perf_test[9] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test9");
firstInitTime_ms = 0;
lastInitFailReport_ms = 0;
}
// setup this core backend
@ -60,8 +61,21 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
return false;
}
// Wait up to 30 seconds for all GPS units to finish their configuration.
// Until this has occurred we will not know what type of GPS is being used
// and what its time delay is
if (!_ahrs->get_gps().all_configured() && (AP_HAL::millis() < 30E3)) {
if (AP_HAL::millis() - lastInitFailReport_ms > 5000) {
lastInitFailReport_ms = AP_HAL::millis();
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 waiting for GPS config data");
}
return false;
} else if (!_ahrs->get_gps().all_configured()) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 GPS config unavailable - using default time delay");
}
// find the maximum time delay for all potential sensors
uint16_t maxTimeDelay_ms = MAX(_frontend->_gpsDelay_ms ,
uint16_t maxTimeDelay_ms = MAX((uint16_t)(_ahrs->get_gps().get_lag() * 1000.0f) ,
MAX(_frontend->_hgtDelay_ms ,
MAX(_frontend->_flowDelay_ms ,
MAX(_frontend->_rngBcnDelay_ms ,

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -897,6 +897,7 @@ private:
Vector3f velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
Vector3f posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
uint32_t firstInitTime_ms; // First time the initialise function was called (msec)
uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report wass sent (msec)
// Specify preferred source of data to be used for a state reset
enum resetDataSource {