diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-12 14:43:23 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-12 14:43:23 +0100 |
commit | 6af2a38f3621afb57ec33e4298e4399fb567e25a (patch) | |
tree | 287918edecf6e2239fd9bbe4c8b1704642c8858f /src | |
parent | a8c298c77260b496b2b04ba9348ce351086c68c1 (diff) | |
download | px4-firmware-6af2a38f3621afb57ec33e4298e4399fb567e25a.tar.gz px4-firmware-6af2a38f3621afb57ec33e4298e4399fb567e25a.tar.bz2 px4-firmware-6af2a38f3621afb57ec33e4298e4399fb567e25a.zip |
AttPosEKF: Moved class declaration to header file
Diffstat (limited to 'src')
3 files changed, 343 insertions, 293 deletions
diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h new file mode 100644 index 000000000..07db924b2 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -0,0 +1,338 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file AttitudePositionEstimatorEKF.h + * Implementation of the attitude and position estimator. This is a PX4 wrapper around + * the EKF IntertialNav filter of Paul Riseborough (@see estimator_22states.cpp) + * + * @author Paul Riseborough <p_riseborough@live.com.au> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#pragma once + +#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 <uORB/topics/sensor_combined.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 <geo/geo.h> +#include <systemlib/perf_counter.h> + +//Forward declaration +class AttPosEKF; + +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(); +}; 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 { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index e912e699e..f8c6510a2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -1087,8 +1087,8 @@ void AttPosEKF::FuseVelposNED() else horizRetryTime = gpsRetryTimeNoTAS; // Form the observation vector - for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; - for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; + for (uint8_t i=0; i <=2; i++) observation[i] = velNED[i]; + for (uint8_t i=3; i <=4; i++) observation[i] = posNE[i-3]; observation[5] = -(hgtMea); // Estimate the GPS Velocity, GPS horiz position and height measurement variances. |