diff options
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 1713 |
1 files changed, 705 insertions, 1008 deletions
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 62965976d..1c79cb61d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -37,8 +37,12 @@ * * @author Paul Riseborough <p_riseborough@live.com.au> * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Johan Jansen <jnsn.johan@gmail.com> */ +#include "AttitudePositionEstimatorEKF.h" +#include "estimator_22states.h" + #include <nuttx/config.h> #include <stdio.h> #include <stdlib.h> @@ -51,42 +55,22 @@ #include <time.h> #include <float.h> -#define SENSOR_COMBINED_SUB - -#include <drivers/drv_hrt.h> -#include <drivers/drv_gyro.h> -#include <drivers/drv_accel.h> -#include <drivers/drv_mag.h> -#include <drivers/drv_baro.h> -#include <drivers/drv_range_finder.h> -#ifdef SENSOR_COMBINED_SUB -#include <uORB/topics/sensor_combined.h> -#endif #include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/airspeed.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/parameter_update.h> -#include <uORB/topics/estimator_status.h> -#include <uORB/topics/actuator_armed.h> -#include <uORB/topics/home_position.h> -#include <uORB/topics/wind_estimate.h> #include <systemlib/param/param.h> #include <systemlib/err.h> -#include <geo/geo.h> -#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> #include <mathlib/math/filter/LowPassFilter2p.hpp> #include <mavlink/mavlink_log.h> #include <platforms/px4_defines.h> -#include "estimator_22states.h" +static uint64_t IMUmsec = 0; +static uint64_t IMUusec = 0; + +//Constants +static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds +static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds +static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure /** * estimator app start / stop handling function @@ -99,10 +83,6 @@ __EXPORT uint32_t millis(); __EXPORT uint64_t getMicros(); -static uint64_t IMUmsec = 0; -static uint64_t IMUusec = 0; -static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds - uint32_t millis() { return IMUmsec; @@ -113,248 +93,25 @@ uint64_t getMicros() return IMUusec; } -class FixedwingEstimator -{ -public: - /** - * Constructor - */ - FixedwingEstimator(); - - /* we do not want people ever copying this class */ - FixedwingEstimator(const FixedwingEstimator& that) = delete; - FixedwingEstimator operator=(const FixedwingEstimator&) = delete; - - /** - * Destructor, also kills the sensors task. - */ - ~FixedwingEstimator(); - - /** - * Start the sensors task. - * - * @return OK on success. - */ - int start(); - - /** - * Task status - * - * @return true if the mainloop is running - */ - bool task_running() { return _task_running; } - - /** - * Print the current status. - */ - void print_status(); - - /** - * Trip the filter by feeding it NaN values. - */ - int trip_nan(); - - /** - * Enable logging. - * - * @param enable Set to true to enable logging, false to disable - */ - int enable_logging(bool enable); - - /** - * Set debug level. - * - * @param debug Desired debug level - 0 to disable. - */ - int set_debuglevel(unsigned debug) { _debug = debug; return 0; } - -private: - - bool _task_should_exit; /**< if true, sensor task should exit */ - bool _task_running; /**< if true, task is running in its mainloop */ - int _estimator_task; /**< task handle for sensor task */ -#ifndef SENSOR_COMBINED_SUB - int _gyro_sub; /**< gyro sensor subscription */ - int _accel_sub; /**< accel sensor subscription */ - int _mag_sub; /**< mag sensor subscription */ -#else - int _sensor_combined_sub; -#endif - int _distance_sub; /**< distance measurement */ - int _airspeed_sub; /**< airspeed subscription */ - int _baro_sub; /**< barometer subscription */ - int _gps_sub; /**< GPS subscription */ - int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_control_sub; /**< notification of manual control updates */ - int _mission_sub; - int _home_sub; /**< home position as defined by commander / user */ - - orb_advert_t _att_pub; /**< vehicle attitude */ - orb_advert_t _global_pos_pub; /**< global position */ - orb_advert_t _local_pos_pub; /**< position in local frame */ - orb_advert_t _estimator_status_pub; /**< status of the estimator */ - orb_advert_t _wind_pub; /**< wind estimate */ - - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct gyro_report _gyro; - struct accel_report _accel; - struct mag_report _mag; - struct airspeed_s _airspeed; /**< airspeed */ - struct baro_report _baro; /**< baro readings */ - struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct vehicle_local_position_s _local_pos; /**< local vehicle position */ - struct vehicle_gps_position_s _gps; /**< GPS position */ - struct wind_estimate_s _wind; /**< wind estimate */ - struct range_finder_report _distance; /**< distance estimate */ - - struct gyro_scale _gyro_offsets[3]; - struct accel_scale _accel_offsets[3]; - struct mag_scale _mag_offsets[3]; - -#ifdef SENSOR_COMBINED_SUB - struct sensor_combined_s _sensor_combined; -#endif - - struct map_projection_reference_s _pos_ref; - - float _baro_ref; /**< barometer reference altitude */ - float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ - float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ - hrt_abstime _last_debug_print = 0; - - perf_counter_t _loop_perf; ///< loop performance counter - perf_counter_t _loop_intvl; ///< loop rate counter - perf_counter_t _perf_gyro; ///<local performance counter for gyro updates - perf_counter_t _perf_mag; ///<local performance counter for mag updates - perf_counter_t _perf_gps; ///<local performance counter for gps updates - perf_counter_t _perf_baro; ///<local performance counter for baro updates - perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates - perf_counter_t _perf_reset; ///<local performance counter for filter resets - - bool _baro_init; - bool _gps_initialized; - hrt_abstime _gps_start_time; - hrt_abstime _filter_start_time; - hrt_abstime _last_sensor_timestamp; - hrt_abstime _last_run; - hrt_abstime _distance_last_valid; - bool _gyro_valid; - bool _accel_valid; - bool _mag_valid; - int _gyro_main; ///< index of the main gyroscope - int _accel_main; ///< index of the main accelerometer - int _mag_main; ///< index of the main magnetometer - bool _ekf_logging; ///< log EKF state - unsigned _debug; ///< debug level - default 0 - - int _mavlink_fd; - - struct { - int32_t vel_delay_ms; - int32_t pos_delay_ms; - int32_t height_delay_ms; - int32_t mag_delay_ms; - int32_t tas_delay_ms; - float velne_noise; - float veld_noise; - float posne_noise; - float posd_noise; - float mag_noise; - float gyro_pnoise; - float acc_pnoise; - float gbias_pnoise; - float abias_pnoise; - float mage_pnoise; - float magb_pnoise; - float eas_noise; - float pos_stddev_threshold; - } _parameters; /**< local copies of interesting parameters */ - - struct { - param_t vel_delay_ms; - param_t pos_delay_ms; - param_t height_delay_ms; - param_t mag_delay_ms; - param_t tas_delay_ms; - param_t velne_noise; - param_t veld_noise; - param_t posne_noise; - param_t posd_noise; - param_t mag_noise; - param_t gyro_pnoise; - param_t acc_pnoise; - param_t gbias_pnoise; - param_t abias_pnoise; - 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; - - /** - * Update our local parameter cache. - */ - int parameters_update(); - - /** - * Update control outputs - * - */ - void control_update(); - - /** - * Check for changes in vehicle status. - */ - void vehicle_status_poll(); - - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main filter task. - */ - void task_main(); - - /** - * Check filter sanity state - * - * @return zero if ok, non-zero for a filter error condition. - */ - int check_filter_state(); -}; - namespace estimator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; + /* oddly, ERROR is not defined for c++ */ + #ifdef ERROR + # undef ERROR + #endif + static const int ERROR = -1; -FixedwingEstimator *g_estimator = nullptr; + AttitudePositionEstimatorEKF *g_estimator = nullptr; } -FixedwingEstimator::FixedwingEstimator() : - +AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _task_should_exit(false), _task_running(false), _estimator_task(-1), -/* subscriptions */ -#ifndef SENSOR_COMBINED_SUB - _gyro_sub(-1), - _accel_sub(-1), - _mag_sub(-1), -#else + /* subscriptions */ _sensor_combined_sub(-1), -#endif _distance_sub(-1), _airspeed_sub(-1), _baro_sub(-1), @@ -364,8 +121,9 @@ FixedwingEstimator::FixedwingEstimator() : _manual_control_sub(-1), _mission_sub(-1), _home_sub(-1), + _landDetectorSub(-1), -/* publications */ + /* publications */ _att_pub(-1), _global_pos_pub(-1), _local_pos_pub(-1), @@ -384,21 +142,20 @@ FixedwingEstimator::FixedwingEstimator() : _gps({}), _wind({}), _distance{}, + _landDetector{}, _gyro_offsets({}), _accel_offsets({}), _mag_offsets({}), - #ifdef SENSOR_COMBINED_SUB _sensor_combined{}, - #endif _pos_ref{}, _baro_ref(0.0f), _baro_ref_offset(0.0f), _baro_gps_offset(0.0f), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), @@ -408,10 +165,14 @@ FixedwingEstimator::FixedwingEstimator() : _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), -/* states */ + /* states */ + _gps_alt_filt(0.0f), + _baro_alt_filt(0.0f), + _covariancePredictionDt(0.0f), + _gpsIsGood(false), + _previousGPSTimestamp(0), _baro_init(false), _gps_initialized(false), - _gps_start_time(0), _filter_start_time(0), _last_sensor_timestamp(0), _last_run(0), @@ -424,12 +185,18 @@ FixedwingEstimator::FixedwingEstimator() : _mag_main(0), _ekf_logging(true), _debug(0), + + _newDataGps(false), + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), + _mavlink_fd(-1), _parameters{}, _parameter_handles{}, _ekf(nullptr) { - _last_run = hrt_absolute_time(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); @@ -455,7 +222,6 @@ FixedwingEstimator::FixedwingEstimator() : parameters_update(); /* get offsets */ - int fd, res; for (unsigned s = 0; s < 3; s++) { @@ -498,7 +264,7 @@ FixedwingEstimator::FixedwingEstimator() : } } -FixedwingEstimator::~FixedwingEstimator() +AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() { if (_estimator_task != -1) { @@ -525,16 +291,14 @@ FixedwingEstimator::~FixedwingEstimator() estimator::g_estimator = nullptr; } -int -FixedwingEstimator::enable_logging(bool logging) +int AttitudePositionEstimatorEKF::enable_logging(bool logging) { _ekf_logging = logging; return 0; } -int -FixedwingEstimator::parameters_update() +int AttitudePositionEstimatorEKF::parameters_update() { param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); @@ -578,8 +342,7 @@ FixedwingEstimator::parameters_update() return OK; } -void -FixedwingEstimator::vehicle_status_poll() +void AttitudePositionEstimatorEKF::vehicle_status_poll() { bool vstatus_updated; @@ -592,8 +355,7 @@ FixedwingEstimator::vehicle_status_poll() } } -int -FixedwingEstimator::check_filter_state() +int AttitudePositionEstimatorEKF::check_filter_state() { /* * CHECK IF THE INPUT DATA IS SANE @@ -687,15 +449,15 @@ FixedwingEstimator::check_filter_state() } // Copy all states or at least all that we can fit - unsigned ekf_n_states = ekf_report.n_states; - unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + size_t ekf_n_states = ekf_report.n_states; + size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; - for (unsigned i = 0; i < rep.n_states; i++) { + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; } - for (unsigned i = 0; i < rep.n_states; i++) { + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; } @@ -709,19 +471,17 @@ FixedwingEstimator::check_filter_state() return check; } -void -FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) +void AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[]) { estimator::g_estimator->task_main(); } -void -FixedwingEstimator::task_main() +void AttitudePositionEstimatorEKF::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); - float dt = 0.0f; // time lapsed since last covariance prediction + _filter_start_time = hrt_absolute_time(); if (!_ekf) { @@ -738,63 +498,32 @@ FixedwingEstimator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_sub = orb_subscribe(ORB_ID(home_position)); + _landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); -#ifndef SENSOR_COMBINED_SUB - - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); - - /* rate limit gyro updates to 50 Hz */ - /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_gyro_sub, 4); -#else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_sensor_combined_sub, 9); -#endif /* sets also parameters in the EKF object */ parameters_update(); - Vector3f lastAngRate; - Vector3f lastAccel; - /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; -#ifndef SENSOR_COMBINED_SUB - fds[1].fd = _gyro_sub; - fds[1].events = POLLIN; -#else + fds[1].fd = _sensor_combined_sub; fds[1].events = POLLIN; -#endif - - bool newDataGps = false; - bool newHgtData = false; - bool newAdsData = false; - bool newDataMag = false; - bool newRangeData = false; - - float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) - uint64_t last_gps = 0; _gps.vel_n_m_s = 0.0f; _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; - // init lowpass filters for baro and gps altitude - float _gps_alt_filt = 0, _baro_alt_filt = 0; - float rc = 10.0f; // RC time constant of 1st order LPF in seconds - hrt_abstime baro_last = 0; - _task_running = true; while (!_task_should_exit) { @@ -832,9 +561,6 @@ FixedwingEstimator::task_main() bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON); vehicle_status_poll(); - bool accel_updated; - bool mag_updated; - perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ @@ -842,14 +568,8 @@ FixedwingEstimator::task_main() /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); -#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; @@ -872,848 +592,825 @@ FixedwingEstimator::task_main() /** * PART ONE: COLLECT ALL DATA **/ + pollData(); - /* load local copies */ -#ifndef SENSOR_COMBINED_SUB - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + /* + * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY + */ + if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { + continue; + } + /** + * PART TWO: EXECUTE THE FILTER + * + * We run the filter only once all data has been fetched + **/ - orb_check(_accel_sub, &accel_updated); + if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { - if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); - } + // maintain filtered baro and gps altitudes to calculate weather offset + // baro sample rate is ~70Hz and measurement bandwidth is high + // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds + // maintain heavily filtered values for both baro and gps altitude + // Assume the filtered output should be identical for both sensors + _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; +// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { +// _last_debug_print = hrt_absolute_time(); +// perf_print_counter(_perf_baro); +// perf_reset(_perf_baro); +// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", +// (double)_baro_gps_offset, +// (double)_baro_alt_filt, +// (double)_gps_alt_filt, +// (double)_global_pos.alt, +// (double)_local_pos.z); +// } - _last_sensor_timestamp = _gyro.timestamp; - IMUmsec = _gyro.timestamp / 1e3; - IMUusec = _gyro.timestamp; + /* Initialize the filter first */ + if (!_gps_initialized && _gpsIsGood) { + initializeGPS(); + } + else if (!_ekf->statesInitialised) { + // North, East Down position (m) + float initVelNED[3] = {0.0f, 0.0f, 0.0f}; - float deltaT = (_gyro.timestamp - _last_run) / 1e6f; - _last_run = _gyro.timestamp; + _ekf->posNE[0] = 0.0f; + _ekf->posNE[1] = 0.0f; - /* guard against too large deltaT's */ - if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { - deltaT = 0.01f; - } + _local_pos.ref_alt = _baro_ref; + _baro_ref_offset = 0.0f; + _baro_gps_offset = 0.0f; + _baro_alt_filt = _baro.altitude; + _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - // Always store data, independent of init status - /* fill in last data set */ - _ekf->dtIMU = deltaT; + } + else if (_ekf->statesInitialised) { - if (isfinite(_gyro.x) && - isfinite(_gyro.y) && - isfinite(_gyro.z)) { - _ekf->angRate.x = _gyro.x; - _ekf->angRate.y = _gyro.y; - _ekf->angRate.z = _gyro.z; + // Check if on ground - status is used by covariance prediction + _ekf->setOnGround(_landDetector.landed); - if (!_gyro_valid) { - lastAngRate = _ekf->angRate; - } + // We're apparently initialized in this case now + // check (and reset the filter as needed) + int check = check_filter_state(); + if (check) { + // Let the system re-initialize itself + continue; + } - _gyro_valid = true; - } + //Run EKF data fusion steps + updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData); - if (accel_updated) { - _ekf->accel.x = _accel.x; - _ekf->accel.y = _accel.y; - _ekf->accel.z = _accel.z; + //Publish attitude estimations + publishAttitude(); - if (!_accel_valid) { - lastAccel = _ekf->accel; - } + //Publish Local Position estimations + publishLocalPosition(); + + //Publish Global Position, but only if it's any good + if(_gps_initialized && _gpsIsGood) + { + publishGlobalPosition(); + } - _accel_valid = true; + //Publish wind estimates + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + publishWindEstimate(); + } + } } - _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; - _ekf->lastAngRate = angRate; - _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - _ekf->lastAccel = accel; + } + perf_end(_loop_perf); + } -#else - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); + _task_running = false; - static hrt_abstime last_accel = 0; - static hrt_abstime last_mag = 0; + warnx("exiting.\n"); - if (last_accel != _sensor_combined.accelerometer_timestamp) { - accel_updated = true; - } else { - accel_updated = false; - } + _estimator_task = -1; + _exit(0); +} - last_accel = _sensor_combined.accelerometer_timestamp; +void AttitudePositionEstimatorEKF::initializeGPS() +{ + // GPS is in scaled integers, convert + double lat = _gps.lat / 1.0e7; + double lon = _gps.lon / 1.0e7; + float gps_alt = _gps.alt / 1e3f; + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame - // Copy gyro and accel - _last_sensor_timestamp = _sensor_combined.timestamp; - IMUmsec = _sensor_combined.timestamp / 1e3; - IMUusec = _sensor_combined.timestamp; + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _baro_alt_filt = _baro.altitude; - float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; + _ekf->baroHgt = _baro.altitude; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); - /* guard against too large deltaT's */ - if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { - deltaT = 0.01f; - } + // Set up position variables correctly + _ekf->GPSstatus = _gps.fix_type; - _last_run = _sensor_combined.timestamp; + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; - // Always store data, independent of init status - /* fill in last data set */ - _ekf->dtIMU = deltaT; + // Look up mag declination based on current position + float declination = math::radians(get_mag_declination(lat, lon)); - int last_gyro_main = _gyro_main; + float initVelNED[3]; + initVelNED[0] = _gps.vel_n_m_s; + initVelNED[1] = _gps.vel_e_m_s; + initVelNED[2] = _gps.vel_d_m_s; - if (isfinite(_sensor_combined.gyro_rad_s[0]) && - isfinite(_sensor_combined.gyro_rad_s[1]) && - isfinite(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { + _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - _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_main = 0; - _gyro_valid = true; + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = _gps.timestamp_position; - } else if (isfinite(_sensor_combined.gyro1_rad_s[0]) && - isfinite(_sensor_combined.gyro1_rad_s[1]) && - isfinite(_sensor_combined.gyro1_rad_s[2])) { + map_projection_init(&_pos_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); - _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro1_rad_s[2]; - _gyro_main = 1; - _gyro_valid = true; + #if 0 + 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 / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); + warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); + #endif - } else { - _gyro_valid = false; - } + _gps_initialized = true; +} - if (last_gyro_main != _gyro_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main); - } +void AttitudePositionEstimatorEKF::publishAttitude() +{ + // Output results + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); - if (!_gyro_valid) { - /* keep last value if he hit an out of band value */ - lastAngRate = _ekf->angRate; - } else { - perf_count(_perf_gyro); - } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + PX4_R(_att.R, i, j) = R(i, j); + } + } - if (accel_updated) { - - int last_accel_main = _accel_main; - - /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { - _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_main = 0; - } else { - _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2]; - _accel_main = 1; - } + _att.timestamp = _last_sensor_timestamp; + _att.q[0] = _ekf->states[0]; + _att.q[1] = _ekf->states[1]; + _att.q[2] = _ekf->states[2]; + _att.q[3] = _ekf->states[3]; + _att.q_valid = true; + _att.R_valid = true; + + _att.timestamp = _last_sensor_timestamp; + _att.roll = euler(0); + _att.pitch = euler(1); + _att.yaw = euler(2); + + _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; + + // gyro offsets + _att.rate_offsets[0] = _ekf->states[10]; + _att.rate_offsets[1] = _ekf->states[11]; + _att.rate_offsets[2] = _ekf->states[12]; + + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - if (!_accel_valid) { - lastAccel = _ekf->accel; - } + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } +} - if (last_accel_main != _accel_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main); - } +void AttitudePositionEstimatorEKF::publishLocalPosition() +{ + _local_pos.timestamp = _last_sensor_timestamp; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; - _accel_valid = true; - } + // XXX need to announce change of Z reference somehow elegantly + _local_pos.z = _ekf->states[9] - _baro_ref_offset; - _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; - lastAngRate = _ekf->angRate; - _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; - lastAccel = _ekf->accel; + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; - if (last_mag != _sensor_combined.magnetometer_timestamp) { - mag_updated = true; - newDataMag = true; + _local_pos.xy_valid = _gps_initialized && _gpsIsGood; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = _gps_initialized && _gpsIsGood; + _local_pos.v_z_valid = true; + _local_pos.xy_global = _gps_initialized; //TODO: Handle optical flow mode here - } else { - newDataMag = false; - } + _local_pos.z_global = false; + _local_pos.yaw = _att.yaw; - last_mag = _sensor_combined.magnetometer_timestamp; + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); -#endif + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } +} - //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); +void AttitudePositionEstimatorEKF::publishGlobalPosition() +{ + _global_pos.timestamp = _local_pos.timestamp; + + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_utc_usec = _gps.time_utc_usec; + } - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - perf_count(_perf_airspeed); + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; - _ekf->VtasMeas = _airspeed.true_airspeed_m_s; - newAdsData = true; + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } - } else { - newAdsData = false; - } + /* terrain altitude */ + _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; + _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && + (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); - bool gps_updated; - orb_check(_gps_sub, &gps_updated); + _global_pos.yaw = _local_pos.yaw; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; - if (gps_updated) { + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the global position */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); - orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); - perf_count(_perf_gps); + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } +} - if (_gps.fix_type < 3) { - newDataGps = false; +void AttitudePositionEstimatorEKF::publishWindEstimate() +{ + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; - } else { + // XXX we need to do something smart about the covariance here + // but we default to the estimate covariance for now + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; - /* store time of valid GPS measurement */ - _gps_start_time = hrt_absolute_time(); + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - /* check if we had a GPS outage for a long time */ - float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f; + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } - const float pos_reset_threshold = 5.0f; // seconds +} - /* timeout of 5 seconds */ - if (gps_elapsed > pos_reset_threshold) { - _ekf->ResetPosition(); - _ekf->ResetVelocity(); - _ekf->ResetStoredStates(); - } - _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold)); +void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, + const bool fuseBaro, const bool fuseAirSpeed) +{ + // Run the strapdown INS equations every IMU update + _ekf->UpdateStrapdownEquationsNED(); + + // store the predicted states for subsequent use by measurement fusion + _ekf->StoreStates(IMUmsec); + + // sum delta angles and time used by covariance prediction + _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; + _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; + _covariancePredictionDt += _ekf->dtIMU; + + // perform a covariance prediction if the total delta angle has exceeded the limit + // or the time limit will be exceeded at the next IMU update + if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) + || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { + _ekf->CovariancePrediction(_covariancePredictionDt); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); + _covariancePredictionDt = 0.0f; + } - /* fuse GPS updates */ + // Fuse GPS Measurements + if (fuseGPS && _gps_initialized) { + // Convert GPS measurements to Pos NE, hgt and Vel NED - //_gps.timestamp / 1e3; - _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; + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; - // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); - _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - _ekf->gpsHgt = _gps.alt / 1e3f; + // run the fusion step + _ekf->FuseVelposNED(); - // update LPF - _gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt); + } + else if (!_gps_initialized) { - //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); + // force static mode + _ekf->staticMode = true; - // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { - // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); - // } else { - // _ekf->vneSigma = _parameters.velne_noise; - // } + // Convert GPS measurements to Pos NE, hgt and Vel NED + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; - // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { - // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); - // } else { - // _ekf->posNeSigma = _parameters.posne_noise; - // } + _ekf->posNE[0] = 0.0f; + _ekf->posNE[1] = 0.0f; - // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; - newDataGps = true; - last_gps = _gps.timestamp_position; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - } + // run the fusion step + _ekf->FuseVelposNED(); - } + } + else { + _ekf->fuseVelData = false; + _ekf->fusePosData = false; + } - bool baro_updated; - orb_check(_baro_sub, &baro_updated); + if (fuseBaro) { + // Could use a blend of GPS and baro alt data if desired + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->fuseHgtData = true; - if (baro_updated) { + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + // run the fusion step + _ekf->FuseVelposNED(); - float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; - baro_last = _baro.timestamp; + } + else { + _ekf->fuseHgtData = false; + } - _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); + // Fuse Magnetometer Measurements + if (fuseMag) { + _ekf->fuseMagData = true; + _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data - _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + _ekf->magstate.obsIndex = 0; + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); - if (!_baro_init) { - _baro_ref = _baro.altitude; - _baro_init = true; - warnx("ALT REF INIT"); - } + } + else { + _ekf->fuseMagData = false; + } - perf_count(_perf_baro); + // Fuse Airspeed Measurements + if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) { + _ekf->fuseVtasData = true; + _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + _ekf->FuseAirspeed(); - newHgtData = true; - } else { - newHgtData = false; - } + } + else { + _ekf->fuseVtasData = false; + } -#ifndef SENSOR_COMBINED_SUB - orb_check(_mag_sub, &mag_updated); -#endif + // Fuse Rangefinder Measurements + if (fuseRangeSensor) { + if (_ekf->Tnb.z.z > 0.9f) { + // _ekf->rngMea is set in sensor readout already + _ekf->fuseRngData = true; + _ekf->fuseOptFlowData = false; + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->OpticalFlowEKF(); + _ekf->fuseRngData = false; + } + } +} - if (mag_updated) { +int AttitudePositionEstimatorEKF::start() +{ + ASSERT(_estimator_task == -1); - _mag_valid = true; + /* start the task */ + _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 7500, + (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, + nullptr); - perf_count(_perf_mag); + if (_estimator_task < 0) { + warn("task start failed"); + return -errno; + } -#ifndef SENSOR_COMBINED_SUB - orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + return OK; +} - // XXX we compensate the offsets upfront - should be close to zero. - // 0.001f - _ekf->magData.x = _mag.x; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset +void AttitudePositionEstimatorEKF::print_status() +{ + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); - _ekf->magData.y = _mag.y; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); - _ekf->magData.z = _mag.z; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13: Accelerometer offset + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) -#else - int last_mag_main = _mag_main; + printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); + printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - // XXX we compensate the offsets upfront - should be close to zero. + if (EKF_STATE_ESTIMATES == 23) { + printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); - /* fail over to the 2nd mag if we know the first is down */ - if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) { - _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + } else { + printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); + printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); + printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + } + printf("states: %s %s %s %s %s %s %s %s %s %s\n", + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); +} - _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset +void AttitudePositionEstimatorEKF::pollData() +{ + static Vector3f lastAngRate; + static Vector3f lastAccel; + bool accel_updated = false; - _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 0; - } else { - _ekf->magData.x = _sensor_combined.magnetometer1_ga[0]; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + //Update Gyro and Accelerometer + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - _ekf->magData.y = _sensor_combined.magnetometer1_ga[1]; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + static hrt_abstime last_accel = 0; + static hrt_abstime last_mag = 0; - _ekf->magData.z = _sensor_combined.magnetometer1_ga[2]; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 1; - } + if (last_accel != _sensor_combined.accelerometer_timestamp) { + accel_updated = true; + } else { + accel_updated = false; + } - if (last_mag_main != _mag_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main); - } -#endif + last_accel = _sensor_combined.accelerometer_timestamp; - newDataMag = true; - } else { - newDataMag = false; - } + // Copy gyro and accel + _last_sensor_timestamp = _sensor_combined.timestamp; + IMUmsec = _sensor_combined.timestamp / 1e3; + IMUusec = _sensor_combined.timestamp; - orb_check(_distance_sub, &newRangeData); + float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; - if (newRangeData) { - orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); + /* guard against too large deltaT's */ + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { + deltaT = 0.01f; + } - if (_distance.valid) { - _ekf->rngMea = _distance.distance; - _distance_last_valid = _distance.timestamp; - } else { - newRangeData = false; - } - } + _last_run = _sensor_combined.timestamp; - /* - * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY - */ - if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { - continue; - } + // Always store data, independent of init status + /* fill in last data set */ + _ekf->dtIMU = deltaT; - /** - * PART TWO: EXECUTE THE FILTER - * - * We run the filter only once all data has been fetched - **/ + int last_gyro_main = _gyro_main; - if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (isfinite(_sensor_combined.gyro_rad_s[0]) && + isfinite(_sensor_combined.gyro_rad_s[1]) && + isfinite(_sensor_combined.gyro_rad_s[2]) && + (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { - float initVelNED[3]; + _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_main = 0; + _gyro_valid = true; - // maintain filtered baro and gps altitudes to calculate weather offset - // baro sample rate is ~70Hz and measurement bandwidth is high - // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds - // maintain heavily filtered values for both baro and gps altitude - // Assume the filtered output should be identical for both sensors - _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; -// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { -// _last_debug_print = hrt_absolute_time(); -// perf_print_counter(_perf_baro); -// perf_reset(_perf_baro); -// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", -// (double)_baro_gps_offset, -// (double)_baro_alt_filt, -// (double)_gps_alt_filt, -// (double)_global_pos.alt, -// (double)_local_pos.z); -// } + } else if (isfinite(_sensor_combined.gyro1_rad_s[0]) && + isfinite(_sensor_combined.gyro1_rad_s[1]) && + isfinite(_sensor_combined.gyro1_rad_s[2])) { - /* Initialize the filter first */ - if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { + _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro1_rad_s[2]; + _gyro_main = 1; + _gyro_valid = true; - // GPS is in scaled integers, convert - double lat = _gps.lat / 1.0e7; - double lon = _gps.lon / 1.0e7; - float gps_alt = _gps.alt / 1e3f; + } else { + _gyro_valid = false; + } - initVelNED[0] = _gps.vel_n_m_s; - initVelNED[1] = _gps.vel_e_m_s; - initVelNED[2] = _gps.vel_d_m_s; + if (last_gyro_main != _gyro_main) { + mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main); + } - // Set up height correctly - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame + if (!_gyro_valid) { + /* keep last value if he hit an out of band value */ + lastAngRate = _ekf->angRate; + } else { + perf_count(_perf_gyro); + } - // init filtered gps and baro altitudes - _gps_alt_filt = gps_alt; - _baro_alt_filt = _baro.altitude; + if (accel_updated) { - _ekf->baroHgt = _baro.altitude; - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); + int last_accel_main = _accel_main; - // Set up position variables correctly - _ekf->GPSstatus = _gps.fix_type; + /* fail over to the 2nd accel if we know the first is down */ + if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { + _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_main = 0; + } else { + _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2]; + _accel_main = 1; + } - _ekf->gpsLat = math::radians(lat); - _ekf->gpsLon = math::radians(lon) - M_PI; - _ekf->gpsHgt = gps_alt; + if (!_accel_valid) { + lastAccel = _ekf->accel; + } - // Look up mag declination based on current position - float declination = math::radians(get_mag_declination(lat, lon)); + if (last_accel_main != _accel_main) { + mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main); + } - _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); + _accel_valid = true; + } - // Initialize projection - _local_pos.ref_lat = lat; - _local_pos.ref_lon = lon; - _local_pos.ref_alt = gps_alt; - _local_pos.ref_timestamp = _gps.timestamp_position; + _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; + lastAngRate = _ekf->angRate; + _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; + lastAccel = _ekf->accel; - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + if (last_mag != _sensor_combined.magnetometer_timestamp) { + _newDataMag = true; - #if 0 - 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 / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); - warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); - #endif + } else { + _newDataMag = false; + } - _gps_initialized = true; + last_mag = _sensor_combined.magnetometer_timestamp; - } else if (!_ekf->statesInitialised) { + //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); - initVelNED[0] = 0.0f; - initVelNED[1] = 0.0f; - initVelNED[2] = 0.0f; - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; + //Update Land Detector + bool newLandData; + orb_check(_landDetectorSub, &newLandData); + if(newLandData) { + orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector); + } - _local_pos.ref_alt = _baro_ref; - _baro_ref_offset = 0.0f; - _baro_gps_offset = 0.0f; + //Update AirSpeed + orb_check(_airspeed_sub, &_newAdsData); + if (_newAdsData) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + perf_count(_perf_airspeed); - _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + _ekf->VtasMeas = _airspeed.true_airspeed_m_s; + } - } else if (_ekf->statesInitialised) { - // We're apparently initialized in this case now - // check (and reset the filter as needed) - int check = check_filter_state(); + orb_check(_gps_sub, &_newDataGps); + if (_newDataGps) { - if (check) { - // Let the system re-initialize itself - continue; - } + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); + perf_count(_perf_gps); - // Run the strapdown INS equations every IMU update - _ekf->UpdateStrapdownEquationsNED(); + //We are more strict for our first fix + float requiredAccuracy = _parameters.pos_stddev_threshold; + if(_gpsIsGood) { + requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f; + } - // store the predicted states for subsequent use by measurement fusion - _ekf->StoreStates(IMUmsec); - // Check if on ground - status is used by covariance prediction - _ekf->OnGroundCheck(); - // sum delta angles and time used by covariance prediction - _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; - _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; - dt += _ekf->dtIMU; - - // perform a covariance prediction if the total delta angle has exceeded the limit - // or the time limit will be exceeded at the next IMU update - if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { - _ekf->CovariancePrediction(dt); - _ekf->summedDelAng.zero(); - _ekf->summedDelVel.zero(); - dt = 0.0f; - } + //Check if the GPS fix is good enough for us to use + if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { + _gpsIsGood = true; + } + else { + _gpsIsGood = false; + } - // Fuse GPS Measurements - if (newDataGps && _gps_initialized) { - // Convert GPS measurements to Pos NE, hgt and Vel NED - - float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; - - // Calculate acceleration predicted by GPS velocity change - if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { - - _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; - _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; - _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; - } - - _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->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else if (!_gps_initialized) { - - // force static mode - _ekf->staticMode = true; - - // Convert GPS measurements to Pos NE, hgt and Vel NED - _ekf->velNED[0] = 0.0f; - _ekf->velNED[1] = 0.0f; - _ekf->velNED[2] = 0.0f; - - _ekf->posNE[0] = 0.0f; - _ekf->posNE[1] = 0.0f; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else { - _ekf->fuseVelData = false; - _ekf->fusePosData = false; - } + if (_gpsIsGood) { - if (newHgtData) { - // Could use a blend of GPS and baro alt data if desired - _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)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else { - _ekf->fuseHgtData = false; - } + //Calculate time since last good GPS fix + const float dtGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; - // Fuse Magnetometer Measurements - if (newDataMag) { - _ekf->fuseMagData = true; - _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + _ekf->updateDtGpsFilt(math::constrain(dtGoodGPS, 0.01f, POS_RESET_THRESHOLD)); - _ekf->magstate.obsIndex = 0; - _ekf->FuseMagnetometer(); - _ekf->FuseMagnetometer(); - _ekf->FuseMagnetometer(); + /* fuse GPS updates */ - } else { - _ekf->fuseMagData = false; - } + //_gps.timestamp / 1e3; + _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; - // Fuse Airspeed Measurements - if (newAdsData && _ekf->VtasMeas > 7.0f) { - _ekf->fuseVtasData = true; - _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - _ekf->FuseAirspeed(); + // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); - } else { - _ekf->fuseVtasData = false; - } + _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; - if (newRangeData) { + //check if we had a GPS outage for a long time + if(_gps_initialized) { - if (_ekf->Tnb.z.z > 0.9f) { - // _ekf->rngMea is set in sensor readout already - _ekf->fuseRngData = true; - _ekf->fuseOptFlowData = false; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); - _ekf->OpticalFlowEKF(); - _ekf->fuseRngData = false; - } - } + //Convert from global frame to local frame + float posNED[3] = {0.0f, 0.0f, 0.0f}; + _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; + if (dtGoodGPS > POS_RESET_THRESHOLD) { + _ekf->ResetPosition(); + _ekf->ResetVelocity(); + } + } - // Output results - math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); - math::Matrix<3, 3> R = q.to_dcm(); - math::Vector<3> euler = R.to_euler(); - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - PX4_R(_att.R, i, j) = R(i, j); - - _att.timestamp = _last_sensor_timestamp; - _att.q[0] = _ekf->states[0]; - _att.q[1] = _ekf->states[1]; - _att.q[2] = _ekf->states[2]; - _att.q[3] = _ekf->states[3]; - _att.q_valid = true; - _att.R_valid = true; - - _att.timestamp = _last_sensor_timestamp; - _att.roll = euler(0); - _att.pitch = euler(1); - _att.yaw = euler(2); - - _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; - // gyro offsets - _att.rate_offsets[0] = _ekf->states[10]; - _att.rate_offsets[1] = _ekf->states[11]; - _att.rate_offsets[2] = _ekf->states[12]; - - /* lazily publish the attitude only once available */ - if (_att_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - - } else { - /* advertise and publish */ - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - } + // update LPF + _gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); - if (_gps_initialized) { - _local_pos.timestamp = _last_sensor_timestamp; - _local_pos.x = _ekf->states[7]; - _local_pos.y = _ekf->states[8]; - // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset; - - _local_pos.vx = _ekf->states[4]; - _local_pos.vy = _ekf->states[5]; - _local_pos.vz = _ekf->states[6]; - - _local_pos.xy_valid = _gps_initialized; - _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized; - _local_pos.v_z_valid = true; - _local_pos.xy_global = true; - - _local_pos.z_global = false; - _local_pos.yaw = _att.yaw; - - /* lazily publish the local position only once available */ - if (_local_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); - - } else { - /* advertise and publish */ - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); - } - - _global_pos.timestamp = _local_pos.timestamp; - - if (_local_pos.xy_global) { - double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); - _global_pos.lat = est_lat; - _global_pos.lon = est_lon; - _global_pos.time_utc_usec = _gps.time_utc_usec; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; - } - - if (_local_pos.v_xy_valid) { - _global_pos.vel_n = _local_pos.vx; - _global_pos.vel_e = _local_pos.vy; - } else { - _global_pos.vel_n = 0.0f; - _global_pos.vel_e = 0.0f; - } - - /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; - - if (_local_pos.v_z_valid) { - _global_pos.vel_d = _local_pos.vz; - } - - /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); - - _global_pos.yaw = _local_pos.yaw; - - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; - - _global_pos.timestamp = _local_pos.timestamp; - - /* lazily publish the global position only once available */ - if (_global_pos_pub > 0) { - /* publish the global position */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); - - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); - } - - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - // XXX we need to do something smart about the covariance here - // but we default to the estimate covariance for now - _wind.covariance_north = _ekf->P[14][14]; - _wind.covariance_east = _ekf->P[15][15]; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - } + //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); - } + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { + // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); + // } else { + // _ekf->vneSigma = _parameters.velne_noise; + // } - } + // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // } else { + // _ekf->posNeSigma = _parameters.posne_noise; + // } - } + // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + _previousGPSTimestamp = _gps.timestamp_position; } + else { + //Too poor GPS fix to use + _newDataGps = false; + } + } - perf_end(_loop_perf); + // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, + // then something is very wrong with the GPS (possibly a hardware failure or comlink error) + else if( (static_cast<float>(hrt_elapsed_time(&_gps.timestamp_position)) / 1e6f) > POS_RESET_THRESHOLD) { + _gpsIsGood = false; } - _task_running = false; + //Update barometer + orb_check(_baro_sub, &_newHgtData); - warnx("exiting.\n"); + if (_newHgtData) { + static hrt_abstime baro_last = 0; - _estimator_task = -1; - _exit(0); -} + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); -int -FixedwingEstimator::start() -{ - ASSERT(_estimator_task == -1); + // init lowpass filters for baro and gps altitude + float baro_elapsed; + if(baro_last == 0) { + baro_elapsed = 0.0f; + } + else { + baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; + } + baro_last = _baro.timestamp; - /* start the task */ - _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 7500, - (main_t)&FixedwingEstimator::task_main_trampoline, - nullptr); + _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); - if (_estimator_task < 0) { - warn("task start failed"); - return -errno; + _ekf->baroHgt = _baro.altitude; + _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + + if (!_baro_init) { + _baro_ref = _baro.altitude; + _baro_init = true; + warnx("ALT REF INIT"); + } + + perf_count(_perf_baro); + + _newHgtData = true; } - return OK; -} + //Update Magnetometer + if (_newDataMag) { -void -FixedwingEstimator::print_status() -{ - math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); - math::Matrix<3, 3> R = q.to_dcm(); - math::Vector<3> euler = R.to_euler(); + _mag_valid = true; - printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); + perf_count(_perf_mag); - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13: Accelerometer offset - // 14-15: Wind Vector - m/sec (North,East) - // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) - // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + int last_mag_main = _mag_main; - printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); - printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + // XXX we compensate the offsets upfront - should be close to zero. - if (n_states == 23) { - printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); - printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); + /* fail over to the 2nd mag if we know the first is down */ + if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) { + _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - } else { - printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); - printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + + _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + _mag_main = 0; + } else { + _ekf->magData.x = _sensor_combined.magnetometer1_ga[0]; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + + _ekf->magData.y = _sensor_combined.magnetometer1_ga[1]; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + + _ekf->magData.z = _sensor_combined.magnetometer1_ga[2]; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + _mag_main = 1; + } + + if (last_mag_main != _mag_main) { + mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main); + } + } + + //Update range data + orb_check(_distance_sub, &_newRangeData); + if (_newRangeData) { + orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); + + if (_distance.valid) { + _ekf->rngMea = _distance.distance; + _distance_last_valid = _distance.timestamp; + } else { + _newRangeData = false; + } } - printf("states: %s %s %s %s %s %s %s %s %s %s\n", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } -int FixedwingEstimator::trip_nan() { +int AttitudePositionEstimatorEKF::trip_nan() { int ret = 0; @@ -1772,7 +1469,7 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) if (estimator::g_estimator != nullptr) errx(1, "already running"); - estimator::g_estimator = new FixedwingEstimator; + estimator::g_estimator = new AttitudePositionEstimatorEKF(); if (estimator::g_estimator == nullptr) errx(1, "alloc failed"); |