From ab92b46e6964ef36fe571e304179a3e151d3261f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 16 Mar 2020 03:57:25 +1100 Subject: [PATCH] Update ecl to add ability to recover from bad magnetic yaw * msg: Add EKF-GSF yaw estimator logging data * ecl: update to version with EKF-GSF yaw estimator * ekf2: Add param control and logging for EKF-GSF yaw estimator * logger: Add logging for EKF-GSF yaw esimtator --- boards/px4/fmu-v2/multicopter.cmake | 2 +- msg/CMakeLists.txt | 1 + msg/tools/uorb_rtps_message_ids.yaml | 2 ++ msg/yaw_estimator_status.msg | 7 ++++ src/lib/ecl | 2 +- src/modules/ekf2/ekf2_main.cpp | 50 +++++++++++++++++++--------- src/modules/ekf2/ekf2_params.c | 12 +++++++ src/modules/logger/logged_topics.cpp | 1 + 8 files changed, 60 insertions(+), 17 deletions(-) create mode 100644 msg/yaw_estimator_status.msg diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index 002f086ad3..af1fcc85c8 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -55,7 +55,7 @@ px4_add_board( navigator rc_update sensors - sih + #sih #temperature_compensation vmount SYSTEMCMDS diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 92d3c1aac8..201a6326fd 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -168,6 +168,7 @@ set(msg_files vtol_vehicle_status.msg wheel_encoders.msg wind_estimate.msg + yaw_estimator_status.msg ) set(deprecated_msgs diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 8d1056f2ee..dcae9393f4 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -299,6 +299,8 @@ rtps: id: 133 - msg: orb_test_large id: 134 + - msg: yaw_estimator_status + id: 135 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/msg/yaw_estimator_status.msg b/msg/yaw_estimator_status.msg new file mode 100644 index 0000000000..39e8b041d5 --- /dev/null +++ b/msg/yaw_estimator_status.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) +float32 yaw_composite # composite yaw from GSF (rad) +float32 yaw_variance # composite yaw variance from GSF (rad^2) +float32[5] yaw # yaw estimate for each model in the filter bank (rad) +float32[5] innov_vn # North velocity innovation for each model in the filter bank (m/s) +float32[5] innov_ve # East velocity innovation for each model in the filter bank (m/s) +float32[5] weight # weighting for each model in the filter bank diff --git a/src/lib/ecl b/src/lib/ecl index 230c865fa9..975060d108 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 230c865fa9c02b07b0371df050b339bc37ce0c29 +Subproject commit 975060d108e901f3ea70a9b88d1e5fa2112e49ff diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 1f6cfe0491..bae1fcb6bb 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -78,6 +78,7 @@ #include #include #include +#include #include "Utility/PreFlightChecker.hpp" @@ -130,8 +131,9 @@ private: template bool update_mag_decl(Param &mag_decl_param); - bool publish_attitude(const hrt_abstime &now); - bool publish_wind_estimate(const hrt_abstime ×tamp); + void publish_attitude(const hrt_abstime ×tamp); + void publish_wind_estimate(const hrt_abstime ×tamp); + void publish_yaw_estimator_status(const hrt_abstime ×tamp); /* * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical @@ -274,6 +276,7 @@ private: uORB::Publication _estimator_status_pub{ORB_ID(estimator_status)}; uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; + uORB::Publication _yaw_est_pub{ORB_ID(yaw_estimator_status)}; uORB::PublicationData _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; uORB::PublicationData _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; uORB::PublicationData _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)}; @@ -525,7 +528,11 @@ private: _param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving. (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time - (ParamExtInt) _param_ekf2_mag_check ///< Mag field strength check + (ParamExtInt) _param_ekf2_mag_check, ///< Mag field strength check + + // Used by EKF-GSF experimental yaw estimator + (ParamExtFloat) + _param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation ) @@ -637,7 +644,8 @@ Ekf2::Ekf2(bool replay_mode): _param_ekf2_pcoef_yn(_params->static_pressure_coef_yn), _param_ekf2_pcoef_z(_params->static_pressure_coef_z), _param_ekf2_move_test(_params->is_moving_scaler), - _param_ekf2_mag_check(_params->check_mag_strength) + _param_ekf2_mag_check(_params->check_mag_strength), + _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default) { // initialise parameter cache updateParams(); @@ -1598,6 +1606,8 @@ void Ekf2::Run() publish_wind_estimate(now); + publish_yaw_estimator_status(now); + if (!_mag_decl_saved && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) { _mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl); } @@ -1749,12 +1759,12 @@ int Ekf2::getRangeSubIndex() return -1; } -bool Ekf2::publish_attitude(const hrt_abstime &now) +void Ekf2::publish_attitude(const hrt_abstime ×tamp) { if (_ekf.attitude_valid()) { // generate vehicle attitude quaternion data vehicle_attitude_s att; - att.timestamp = now; + att.timestamp = timestamp; const Quatf q{_ekf.calculate_quaternion()}; q.copyTo(att.q); @@ -1763,19 +1773,33 @@ bool Ekf2::publish_attitude(const hrt_abstime &now) _att_pub.publish(att); - return true; - } else if (_replay_mode) { // in replay mode we have to tell the replay module not to wait for an update // we do this by publishing an attitude with zero timestamp vehicle_attitude_s att{}; _att_pub.publish(att); } - - return false; } -bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) +void Ekf2::publish_yaw_estimator_status(const hrt_abstime ×tamp) +{ + yaw_estimator_status_s yaw_est_test_data{}; + + static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF, + "yaw_estimator_status_s::yaw wrong size"); + + if (_ekf.getDataEKFGSF(&yaw_est_test_data.yaw_composite, &yaw_est_test_data.yaw_variance, + &yaw_est_test_data.yaw[0], + &yaw_est_test_data.innov_vn[0], &yaw_est_test_data.innov_ve[0], + &yaw_est_test_data.weight[0])) { + + yaw_est_test_data.timestamp = timestamp; + + _yaw_est_pub.publish(yaw_est_test_data); + } +} + +void Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) { if (_ekf.get_wind_status()) { // Publish wind estimate only if ekf declares them valid @@ -1796,11 +1820,7 @@ bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf _wind_pub.publish(wind_estimate); - - return true; } - - return false; } bool Ekf2::blend_gps_data() diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 7d938b895a..c4f256b826 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1430,3 +1430,15 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f); * @boolean */ PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 0); + +/** + * Default value of true airspeed used in EKF-GSF AHRS calculation. + * If no airspeed measurements are avalable, the EKF-GSF AHRS calculation will assume this value of true airspeed when compensating for centripetal acceleration during turns. Set to zero to disable centripetal acceleration compensation during fixed wing flight modes. + * + * @group EKF2 + * @min 0.0 + * @unit m/s + * @max 100.0 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 91418e5d77..56626b890e 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -95,6 +95,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_status", 200); add_topic("vehicle_status_flags"); add_topic("vtol_vehicle_status", 200); + add_topic("yaw_estimator_status", 200); // multi topics add_topic_multi("actuator_outputs", 100);