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 | 294 |
1 files changed, 3 insertions, 291 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 8ec7c43a1..511492b5a 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 @@ -40,6 +40,9 @@ * @author Johan Jansen <jnsn.johan@gmail.com> */ +#include "AttitudePositionEstimatorEKF.h" +#include "estimator_22states.h" + #include <nuttx/config.h> #include <stdio.h> #include <stdlib.h> @@ -52,39 +55,15 @@ #include <time.h> #include <float.h> -#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> -#include <uORB/topics/sensor_combined.h> #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; @@ -114,273 +93,6 @@ uint64_t getMicros() return IMUusec; } -class AttitudePositionEstimatorEKF -{ -public: - /** - * Constructor - */ - AttitudePositionEstimatorEKF(); - - /* we do not want people ever copying this class */ - AttitudePositionEstimatorEKF(const AttitudePositionEstimatorEKF& that) = delete; - AttitudePositionEstimatorEKF operator=(const AttitudePositionEstimatorEKF&) = delete; - - /** - * Destructor, also kills the sensors task. - */ - ~AttitudePositionEstimatorEKF(); - - /** - * 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 */ - - int _sensor_combined_sub; - 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]; - - struct sensor_combined_s _sensor_combined; - - 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 - - float _gps_alt_filt; - float _baro_alt_filt; - float _covariancePredictionDt; ///< time lapsed since last covariance prediction - bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use - uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received - bool _baro_init; - bool _gps_initialized; - 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 - - bool _newDataGps; - bool _newHgtData; - bool _newAdsData; - bool _newDataMag; - bool _newRangeData; - - 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; - -private: - /** - * 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(); - - /** - * @brief - * Publish the euler and quaternions for attitude estimation - **/ - void publishAttitude(); - - /** - * @brief - * Publish local position relative to reference point where filter was initialized - **/ - void publishLocalPosition(); - - /** - * @brief - * Publish global position estimation (WSG84 and AMSL). - * A global position estimate is only valid if we have a good GPS fix - **/ - void publishGlobalPosition(); - - /** - * @brief - * Publish wind estimates for north and east in m/s - **/ - void publishWindEstimate(); - - /** - * @brief - * Runs the sensor fusion step of the filter. The parameters determine which of the sensors - * are fused with each other - **/ - void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, - const bool fuseBaro, const bool fuseAirSpeed); - - /** - * @brief - * Initialize first time good GPS fix so we can get a reference point to calculate local position from - * Should only be required to call once - **/ - void initializeGPS(); - - /** - * @brief - * Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate - * flags to true (e.g newDataGps) - **/ - void pollData(); - -}; - namespace estimator { |