forked from Archive/PX4-Autopilot
Fixed re-initialization of estimator, re-initializes in air now reliably. Does give useful HIL results.
This commit is contained in:
parent
21edf72779
commit
23937150bc
|
@ -138,35 +138,15 @@ void swap_var(float &d1, float &d2)
|
|||
d2 = tmp;
|
||||
}
|
||||
|
||||
AttPosEKF::AttPosEKF() :
|
||||
fusionModeGPS(0),
|
||||
covSkipCount(0),
|
||||
statesInitialised(false),
|
||||
fuseVelData(false),
|
||||
fusePosData(false),
|
||||
fuseHgtData(false),
|
||||
fuseMagData(false),
|
||||
fuseVtasData(false),
|
||||
onGround(true),
|
||||
staticMode(true),
|
||||
useAirspeed(true),
|
||||
useCompass(true),
|
||||
useRangeFinder(true),
|
||||
numericalProtection(true),
|
||||
refSet(false),
|
||||
storeIndex(0),
|
||||
gpsHgt(0.0f),
|
||||
baroHgt(0.0f),
|
||||
GPSstatus(0),
|
||||
VtasMeas(0.0f),
|
||||
magDeclination(0.0f)
|
||||
{
|
||||
velNED[0] = 0.0f;
|
||||
velNED[1] = 0.0f;
|
||||
velNED[2] = 0.0f;
|
||||
AttPosEKF::AttPosEKF()
|
||||
|
||||
/* NOTE: DO NOT initialize class members here. Use ZeroVariables()
|
||||
* instead to allow clean in-air re-initialization.
|
||||
*/
|
||||
{
|
||||
|
||||
InitialiseParameters();
|
||||
ZeroVariables();
|
||||
InitialiseParameters();
|
||||
}
|
||||
|
||||
AttPosEKF::~AttPosEKF()
|
||||
|
@ -2348,7 +2328,7 @@ int AttPosEKF::CheckAndBound()
|
|||
}
|
||||
|
||||
// Reset the filter if the IMU data is too old
|
||||
if (dtIMU > 0.2f) {
|
||||
if (dtIMU > 0.3f) {
|
||||
|
||||
ResetVelocity();
|
||||
ResetPosition();
|
||||
|
@ -2424,24 +2404,10 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
|
|||
void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
|
||||
{
|
||||
|
||||
// Clear the init flag
|
||||
statesInitialised = false;
|
||||
|
||||
// Clear other flags, waiting for new data
|
||||
fusionModeGPS = 0;
|
||||
fuseVelData = false;
|
||||
fusePosData = false;
|
||||
fuseHgtData = false;
|
||||
fuseMagData = false;
|
||||
fuseVtasData = false;
|
||||
// onGround(true),
|
||||
// staticMode(true),
|
||||
useAirspeed = true;
|
||||
useCompass = true;
|
||||
useRangeFinder = true;
|
||||
|
||||
ZeroVariables();
|
||||
|
||||
// Fill variables with valid data
|
||||
velNED[0] = initvelNED[0];
|
||||
velNED[1] = initvelNED[1];
|
||||
velNED[2] = initvelNED[2];
|
||||
magDeclination = declination;
|
||||
|
||||
// Calculate initial filter quaternion states from raw measurements
|
||||
|
@ -2513,6 +2479,30 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
|
|||
|
||||
void AttPosEKF::ZeroVariables()
|
||||
{
|
||||
|
||||
// Initialize on-init initialized variables
|
||||
fusionModeGPS = 0;
|
||||
covSkipCount = 0;
|
||||
statesInitialised = false;
|
||||
fuseVelData = false;
|
||||
fusePosData = false;
|
||||
fuseHgtData = false;
|
||||
fuseMagData = false;
|
||||
fuseVtasData = false;
|
||||
onGround = true;
|
||||
staticMode = true;
|
||||
useAirspeed = true;
|
||||
useCompass = true;
|
||||
useRangeFinder = true;
|
||||
numericalProtection = true;
|
||||
refSet = false;
|
||||
storeIndex = 0;
|
||||
gpsHgt = 0.0f;
|
||||
baroHgt = 0.0f;
|
||||
GPSstatus = 0;
|
||||
VtasMeas = 0.0f;
|
||||
magDeclination = 0.0f;
|
||||
|
||||
// Do the data structure init
|
||||
for (unsigned i = 0; i < n_states; i++) {
|
||||
for (unsigned j = 0; j < n_states; j++) {
|
||||
|
@ -2529,6 +2519,9 @@ void AttPosEKF::ZeroVariables()
|
|||
summedDelAng.zero();
|
||||
summedDelVel.zero();
|
||||
|
||||
dAngIMU.zero();
|
||||
dVelIMU.zero();
|
||||
|
||||
for (unsigned i = 0; i < data_buffer_size; i++) {
|
||||
|
||||
for (unsigned j = 0; j < n_states; j++) {
|
||||
|
|
|
@ -546,24 +546,8 @@ FixedwingEstimator::task_main()
|
|||
/* sets also parameters in the EKF object */
|
||||
parameters_update();
|
||||
|
||||
/* set initial filter state */
|
||||
_ekf->fuseVelData = false;
|
||||
_ekf->fusePosData = false;
|
||||
_ekf->fuseHgtData = false;
|
||||
_ekf->fuseMagData = false;
|
||||
_ekf->fuseVtasData = false;
|
||||
_ekf->statesInitialised = false;
|
||||
|
||||
/* initialize measurement data */
|
||||
_ekf->VtasMeas = 0.0f;
|
||||
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
|
||||
Vector3f lastAccel = {0.0f, 0.0f, -9.81f};
|
||||
_ekf->dVelIMU.x = 0.0f;
|
||||
_ekf->dVelIMU.y = 0.0f;
|
||||
_ekf->dVelIMU.z = 0.0f;
|
||||
_ekf->dAngIMU.x = 0.0f;
|
||||
_ekf->dAngIMU.y = 0.0f;
|
||||
_ekf->dAngIMU.z = 0.0f;
|
||||
Vector3f lastAccel = {0.0f, 0.0f, 0.0f};
|
||||
|
||||
/* wakeup source(s) */
|
||||
struct pollfd fds[2];
|
||||
|
@ -621,12 +605,14 @@ FixedwingEstimator::task_main()
|
|||
bool accel_updated;
|
||||
bool mag_updated;
|
||||
|
||||
hrt_abstime last_sensor_timestamp;
|
||||
|
||||
perf_count(_perf_gyro);
|
||||
|
||||
/* Reset baro reference if switching to HIL, reset sensor states */
|
||||
if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
|
||||
/* system is in HIL now, wait for measurements to come in one last round */
|
||||
usleep(65000);
|
||||
usleep(60000);
|
||||
|
||||
#ifndef SENSOR_COMBINED_SUB
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
|
||||
|
@ -644,8 +630,13 @@ FixedwingEstimator::task_main()
|
|||
|
||||
_baro_init = false;
|
||||
_gps_initialized = false;
|
||||
last_sensor_timestamp = hrt_absolute_time();
|
||||
last_run = last_sensor_timestamp;
|
||||
|
||||
/* now skip this loop and get data on the next one */
|
||||
_ekf->ZeroVariables();
|
||||
_ekf->dtIMU = 0.01f;
|
||||
|
||||
/* now skip this loop and get data on the next one, which will also re-init the filter */
|
||||
continue;
|
||||
}
|
||||
|
||||
|
@ -653,8 +644,6 @@ FixedwingEstimator::task_main()
|
|||
* PART ONE: COLLECT ALL DATA
|
||||
**/
|
||||
|
||||
hrt_abstime last_sensor_timestamp;
|
||||
|
||||
/* load local copies */
|
||||
#ifndef SENSOR_COMBINED_SUB
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
|
||||
|
@ -668,7 +657,7 @@ FixedwingEstimator::task_main()
|
|||
}
|
||||
|
||||
last_sensor_timestamp = _gyro.timestamp;
|
||||
_ekf.IMUmsec = _gyro.timestamp / 1e3f;
|
||||
IMUmsec = _gyro.timestamp / 1e3f;
|
||||
|
||||
float deltaT = (_gyro.timestamp - last_run) / 1e6f;
|
||||
last_run = _gyro.timestamp;
|
||||
|
@ -689,6 +678,11 @@ FixedwingEstimator::task_main()
|
|||
_ekf->angRate.x = _gyro.x;
|
||||
_ekf->angRate.y = _gyro.y;
|
||||
_ekf->angRate.z = _gyro.z;
|
||||
|
||||
if (!_gyro_valid) {
|
||||
lastAngRate = _ekf->angRate;
|
||||
}
|
||||
|
||||
_gyro_valid = true;
|
||||
}
|
||||
|
||||
|
@ -696,6 +690,11 @@ FixedwingEstimator::task_main()
|
|||
_ekf->accel.x = _accel.x;
|
||||
_ekf->accel.y = _accel.y;
|
||||
_ekf->accel.z = _accel.z;
|
||||
|
||||
if (!_accel_valid) {
|
||||
lastAccel = _ekf->accel;
|
||||
}
|
||||
|
||||
_accel_valid = true;
|
||||
}
|
||||
|
||||
|
@ -741,6 +740,11 @@ FixedwingEstimator::task_main()
|
|||
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
|
||||
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
|
||||
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
|
||||
|
||||
if (!_gyro_valid) {
|
||||
lastAngRate = _ekf->angRate;
|
||||
}
|
||||
|
||||
_gyro_valid = true;
|
||||
}
|
||||
|
||||
|
@ -748,6 +752,11 @@ FixedwingEstimator::task_main()
|
|||
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
|
||||
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
|
||||
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
|
||||
|
||||
if (!_accel_valid) {
|
||||
lastAccel = _ekf->accel;
|
||||
}
|
||||
|
||||
_accel_valid = true;
|
||||
}
|
||||
|
||||
|
@ -986,6 +995,22 @@ FixedwingEstimator::task_main()
|
|||
_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
|
||||
}
|
||||
|
||||
/* set sensors to de-initialized state */
|
||||
_gyro_valid = false;
|
||||
_accel_valid = false;
|
||||
_mag_valid = false;
|
||||
|
||||
_baro_init = false;
|
||||
_gps_initialized = false;
|
||||
last_sensor_timestamp = hrt_absolute_time();
|
||||
last_run = last_sensor_timestamp;
|
||||
|
||||
_ekf->ZeroVariables();
|
||||
_ekf->dtIMU = 0.01f;
|
||||
|
||||
// Let the system re-initialize itself
|
||||
continue;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
@ -995,15 +1020,13 @@ FixedwingEstimator::task_main()
|
|||
|
||||
if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
|
||||
|
||||
// bool home_set;
|
||||
// orb_check(_home_sub, &home_set);
|
||||
// struct home_position_s home;
|
||||
// orb_copy(ORB_ID(home_position), _home_sub, &home);
|
||||
float initVelNED[3];
|
||||
|
||||
if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
|
||||
_ekf->velNED[0] = _gps.vel_n_m_s;
|
||||
_ekf->velNED[1] = _gps.vel_e_m_s;
|
||||
_ekf->velNED[2] = _gps.vel_d_m_s;
|
||||
|
||||
initVelNED[0] = _gps.vel_n_m_s;
|
||||
initVelNED[1] = _gps.vel_e_m_s;
|
||||
initVelNED[2] = _gps.vel_d_m_s;
|
||||
|
||||
// GPS is in scaled integers, convert
|
||||
double lat = _gps.lat / 1.0e7;
|
||||
|
@ -1018,9 +1041,6 @@ FixedwingEstimator::task_main()
|
|||
|
||||
// Set up position variables correctly
|
||||
_ekf->GPSstatus = _gps.fix_type;
|
||||
_ekf->velNED[0] = _gps.vel_n_m_s;
|
||||
_ekf->velNED[1] = _gps.vel_e_m_s;
|
||||
_ekf->velNED[2] = _gps.vel_d_m_s;
|
||||
|
||||
_ekf->gpsLat = math::radians(lat);
|
||||
_ekf->gpsLon = math::radians(lon) - M_PI;
|
||||
|
@ -1029,7 +1049,7 @@ FixedwingEstimator::task_main()
|
|||
// Look up mag declination based on current position
|
||||
float declination = math::radians(get_mag_declination(lat, lon));
|
||||
|
||||
_ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
|
||||
_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
|
||||
|
||||
// Initialize projection
|
||||
_local_pos.ref_lat = lat;
|
||||
|
@ -1041,21 +1061,23 @@ FixedwingEstimator::task_main()
|
|||
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
|
||||
warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
|
||||
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
|
||||
warnx("BARO: %8.4f m / ref: %8.4f m", _ekf->baroHgt, _ekf->hgtMea);
|
||||
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
|
||||
|
||||
_gps_initialized = true;
|
||||
|
||||
} else if (!_ekf->statesInitialised) {
|
||||
_ekf->velNED[0] = 0.0f;
|
||||
_ekf->velNED[1] = 0.0f;
|
||||
_ekf->velNED[2] = 0.0f;
|
||||
|
||||
initVelNED[0] = 0.0f;
|
||||
initVelNED[1] = 0.0f;
|
||||
initVelNED[2] = 0.0f;
|
||||
_ekf->posNED[0] = 0.0f;
|
||||
_ekf->posNED[1] = 0.0f;
|
||||
_ekf->posNED[2] = 0.0f;
|
||||
|
||||
_ekf->posNE[0] = _ekf->posNED[0];
|
||||
_ekf->posNE[1] = _ekf->posNED[1];
|
||||
_ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f);
|
||||
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue