From 90569d22bcee65b102c9f55be71806d301829cee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 18:34:33 +0200 Subject: Added support for automatic mag declination setup --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp') diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a82e8d704..23f9e80bd 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -994,11 +994,14 @@ FixedwingEstimator::task_main() _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); - _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - _ekf->gpsHgt = _gps.alt / 1e3f; + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; + + // 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); + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = _gps.lat; @@ -1024,7 +1027,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f); } } -- cgit v1.2.3 From 11a1053b73787bb2afbaff360f720de430447455 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 19:15:58 +0200 Subject: ekf_att_pos_estimator: local position reference fixed --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp') diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 23f9e80bd..fb019a354 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1004,8 +1004,8 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.alt; + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; -- cgit v1.2.3 From be1f39e3851036e43be1bea0fa09096691559a0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:52:37 +0200 Subject: ekf Print declination on init --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp') diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index fb019a354..b87338282 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1013,7 +1013,7 @@ 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("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m); + 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; -- cgit v1.2.3 From 6cb96d074d282af92540b6481d843c2295426773 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 17:01:32 +0200 Subject: EKF: Introduce proper check flags for sensor init states --- .../fw_att_pos_estimator_main.cpp | 40 ++++++++++++++++++---- 1 file changed, 34 insertions(+), 6 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp') diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index b87338282..40729e94b 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -194,6 +194,9 @@ private: bool _baro_init; bool _gps_initialized; uint64_t _gps_start_time; + bool _gyro_valid; + bool _accel_valid; + bool _mag_valid; int _mavlink_fd; @@ -341,6 +344,9 @@ FixedwingEstimator::FixedwingEstimator() : _initialized(false), _baro_init(false), _gps_initialized(false), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), _mavlink_fd(-1), _ekf(nullptr) { @@ -617,10 +623,30 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); - /* Reset baro reference if switching to HIL */ + /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + /* system is in HIL now, we got plenty of time - make sure real sensors are off */ + usleep(250000); + +#ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); +#else + /* now read all sensor publications to ensure all real sensor data is purged */ + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); +#endif + + /* set sensors to de-initialized state */ + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + _baro_init = false; _gps_initialized = false; + + /* now skip this loop and get data on the next one */ + continue; } /** @@ -663,12 +689,14 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; + _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; + _accel_valid = true; } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; @@ -713,12 +741,14 @@ 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]; + _gyro_valid = true; } if (accel_updated) { _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]; + _accel_valid = true; } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; @@ -837,6 +867,8 @@ FixedwingEstimator::task_main() if (mag_updated) { + _mag_valid = true; + perf_count(_perf_mag); #ifndef SENSOR_COMBINED_SUB @@ -961,11 +993,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - // Wait long enough to ensure all sensors updated once - // XXX we rather want to check all updated - - - if (hrt_elapsed_time(&_gps_start_time) > 50000) { + if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { // bool home_set; // orb_check(_home_sub, &home_set); -- cgit v1.2.3 From 319ce3de1005d7199a992b227c7e75acb9db376b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 08:40:23 +0200 Subject: Minor cleanups in EKF estimator --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp') diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 40729e94b..e563b4b87 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -625,8 +625,8 @@ FixedwingEstimator::task_main() /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { - /* system is in HIL now, we got plenty of time - make sure real sensors are off */ - usleep(250000); + /* system is in HIL now, wait for measurements to come in one last round */ + usleep(65000); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -1399,13 +1399,13 @@ int FixedwingEstimator::trip_nan() { _ekf->states[5] = nan_val; usleep(100000); - // warnx("tripping covariance #1 with NaN values"); - // KH[2][2] = nan_val; // intermediate result used for covariance updates - // usleep(100000); + warnx("tripping covariance #1 with NaN values"); + _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates + usleep(100000); - // warnx("tripping covariance #2 with NaN values"); - // KHP[5][5] = nan_val; // intermediate result used for covariance updates - // usleep(100000); + warnx("tripping covariance #2 with NaN values"); + _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates + usleep(100000); warnx("tripping covariance #3 with NaN values"); _ekf->P[3][3] = nan_val; // covariance matrix -- cgit v1.2.3 From 21edf72779286bbfb3c4c6bb4acad5c353300024 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 14:15:11 +0200 Subject: Do not send a critical message when switching to dynamic state --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp') diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index e563b4b87..05cc39f11 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -936,7 +936,7 @@ FixedwingEstimator::task_main() { const char* str = "switching to dynamic state"; warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); break; } -- cgit v1.2.3 From 23937150bce38463612ac170803a06a3424d480d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 18:07:57 +0200 Subject: Fixed re-initialization of estimator, re-initializes in air now reliably. Does give useful HIL results. --- src/modules/ekf_att_pos_estimator/estimator.cpp | 83 +++++++++---------- .../fw_att_pos_estimator_main.cpp | 96 +++++++++++++--------- 2 files changed, 97 insertions(+), 82 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp') diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 86b7efafb..a6d6a9db5 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -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) +AttPosEKF::AttPosEKF() + + /* NOTE: DO NOT initialize class members here. Use ZeroVariables() + * instead to allow clean in-air re-initialization. + */ { - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; - 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++) { diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 05cc39f11..07dfdbd68 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -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; + + _ekf->ZeroVariables(); + _ekf->dtIMU = 0.01f; - /* now skip this loop and get data on the next one */ + /* 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); } } -- cgit v1.2.3