aboutsummaryrefslogtreecommitdiff
path: root/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp')
-rw-r--r--src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp85
1 files changed, 54 insertions, 31 deletions
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 fcbd90405..a8b6eae1c 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
@@ -191,6 +191,7 @@ private:
perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates
bool _initialized;
+ bool _baro_init;
bool _gps_initialized;
uint64_t _gps_start_time;
@@ -214,6 +215,7 @@ private:
float mage_pnoise;
float magb_pnoise;
float eas_noise;
+ float pos_stddev_threshold;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -234,6 +236,7 @@ private:
param_t mage_pnoise;
param_t magb_pnoise;
param_t eas_noise;
+ param_t pos_stddev_threshold;
} _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf;
@@ -340,6 +343,7 @@ FixedwingEstimator::FixedwingEstimator() :
/* states */
_initialized(false),
+ _baro_init(false),
_gps_initialized(false),
_mavlink_fd(-1),
_ekf(nullptr),
@@ -367,6 +371,7 @@ FixedwingEstimator::FixedwingEstimator() :
_parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE");
_parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
+ _parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
/* fetch initial parameter values */
parameters_update();
@@ -455,6 +460,7 @@ FixedwingEstimator::parameters_update()
param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise));
param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
+ param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold));
if (_ekf) {
// _ekf->yawVarScale = 1.0f;
@@ -575,18 +581,10 @@ FixedwingEstimator::task_main()
#endif
bool newDataGps = false;
+ bool newHgtData = false;
bool newAdsData = false;
bool newDataMag = false;
- // Reset relevant structs
- _gps = {};
-
- #ifndef SENSOR_COMBINED_SUB
- _gyro = {};
- #else
- _sensor_combined = {};
- #endif
-
while (!_task_should_exit) {
/* wait for up to 500ms for data */
@@ -618,6 +616,7 @@ FixedwingEstimator::task_main()
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
+ bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
vehicle_status_poll();
bool accel_updated;
@@ -625,6 +624,12 @@ FixedwingEstimator::task_main()
perf_count(_perf_gyro);
+ /* Reset baro reference if switching to HIL */
+ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
+ _baro_init = false;
+ _gps_initialized = false;
+ }
+
/**
* PART ONE: COLLECT ALL DATA
**/
@@ -820,10 +825,17 @@ FixedwingEstimator::task_main()
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
- _ekf->baroHgt = _baro.altitude - _baro_ref;
+ _ekf->baroHgt = _baro.altitude;
- // Could use a blend of GPS and baro alt data if desired
- _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
+ if (!_baro_init) {
+ _baro_ref = _baro.altitude;
+ _baro_init = true;
+ warnx("ALT REF INIT");
+ }
+
+ newHgtData = true;
+ } else {
+ newHgtData = false;
}
#ifndef SENSOR_COMBINED_SUB
@@ -911,9 +923,9 @@ FixedwingEstimator::task_main()
}
}
- // XXX trap for testing
+ // warn on fatal resets
if (check == 1) {
- errx(1, "NUMERIC ERROR IN FILTER");
+ warnx("NUMERIC ERROR IN FILTER");
}
// If non-zero, we got a filter reset
@@ -967,7 +979,7 @@ FixedwingEstimator::task_main()
// struct home_position_s home;
// orb_copy(ORB_ID(home_position), _home_sub, &home);
- if (!_gps_initialized && _gps.fix_type > 2) {
+ 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;
@@ -975,26 +987,37 @@ FixedwingEstimator::task_main()
// GPS is in scaled integers, convert
double lat = _gps.lat / 1.0e7;
double lon = _gps.lon / 1.0e7;
- float alt = _gps.alt / 1e3f;
+ float gps_alt = _gps.alt / 1e3f;
+
+ // Set up height correctly
+ orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+ _baro_gps_offset = gps_alt - _baro.altitude;
+ _ekf->baroHgt = _baro.altitude;
+ _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
- _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt);
+ // 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(_gps.lat / (double)1e7);
+ _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
+ _ekf->gpsHgt = _gps.alt / 1e3f;
+
+ _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt);
// Initialize projection
_local_pos.ref_lat = _gps.lat;
_local_pos.ref_lon = _gps.alt;
- _local_pos.ref_alt = alt;
+ _local_pos.ref_alt = _baro_ref + _baro_gps_offset;
_local_pos.ref_timestamp = _gps.timestamp_position;
- // Store
- orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
- _baro_ref = _baro.altitude;
- _ekf->baroHgt = _baro.altitude - _baro_ref;
- _baro_gps_offset = _baro_ref - _local_pos.ref_alt;
-
map_projection_init(&_pos_ref, lat, lon);
- mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt);
- warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt,
+ 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);
_gps_initialized = true;
@@ -1096,9 +1119,9 @@ FixedwingEstimator::task_main()
_ekf->fusePosData = false;
}
- if (newAdsData && _ekf->statesInitialised) {
+ if (newHgtData && _ekf->statesInitialised) {
// Could use a blend of GPS and baro alt data if desired
- _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
+ _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
_ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
@@ -1187,7 +1210,7 @@ FixedwingEstimator::task_main()
_local_pos.timestamp = last_sensor_timestamp;
_local_pos.x = _ekf->states[7];
_local_pos.y = _ekf->states[8];
- _local_pos.z = _ekf->states[9];
+ _local_pos.z = _ekf->states[9] + _baro_gps_offset;
_local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5];
@@ -1236,8 +1259,8 @@ FixedwingEstimator::task_main()
_global_pos.lat = est_lat;
_global_pos.lon = est_lon;
_global_pos.time_gps_usec = _gps.time_gps_usec;
- _global_pos.eph = _gps.eph;
- _global_pos.epv = _gps.epv;
+ _global_pos.eph = _gps.eph_m;
+ _global_pos.epv = _gps.epv_m;
}
if (_local_pos.v_xy_valid) {
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-03 14:33:18 +0000