From 79334958a9e01def456886d0feeb8934007c9247 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 22 Oct 2019 17:00:36 +0200 Subject: [PATCH] WeatherVane: only update with last row of rotation matrix --- src/lib/WeatherVane/WeatherVane.cpp | 11 ++++------- src/lib/WeatherVane/WeatherVane.hpp | 6 +++--- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 3 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/lib/WeatherVane/WeatherVane.cpp b/src/lib/WeatherVane/WeatherVane.cpp index cf4bad6e42..b96fbc20c3 100644 --- a/src/lib/WeatherVane/WeatherVane.cpp +++ b/src/lib/WeatherVane/WeatherVane.cpp @@ -43,20 +43,18 @@ WeatherVane::WeatherVane() : ModuleParams(nullptr) -{ - _R_sp_prev = matrix::Dcmf(); -} +{ } -void WeatherVane::update(const matrix::Quatf &q_sp_prev, float yaw) +void WeatherVane::update(const matrix::Vector3f &dcm_z_sp_prev, float yaw) { - _R_sp_prev = matrix::Dcmf(q_sp_prev); + _dcm_z_sp_prev = dcm_z_sp_prev; _yaw = yaw; } float WeatherVane::get_weathervane_yawrate() { // direction of desired body z axis represented in earth frame - matrix::Vector3f body_z_sp(_R_sp_prev(0, 2), _R_sp_prev(1, 2), _R_sp_prev(2, 2)); + matrix::Vector3f body_z_sp(_dcm_z_sp_prev); // rotate desired body z axis into new frame which is rotated in z by the current // heading of the vehicle. we refer to this as the heading frame. @@ -74,7 +72,6 @@ float WeatherVane::get_weathervane_yawrate() } else if (roll_sp < -min_roll_rad) { roll_exceeding_treshold = roll_sp + min_roll_rad; - } return math::constrain(roll_exceeding_treshold * _param_wv_gain.get(), -math::radians(_param_wv_yrate_max.get()), diff --git a/src/lib/WeatherVane/WeatherVane.hpp b/src/lib/WeatherVane/WeatherVane.hpp index 136fe15b6e..723e94b221 100644 --- a/src/lib/WeatherVane/WeatherVane.hpp +++ b/src/lib/WeatherVane/WeatherVane.hpp @@ -60,15 +60,15 @@ public: bool weathervane_enabled() { return _param_wv_en.get(); } - void update(const matrix::Quatf &q_sp_prev, float yaw); + void update(const matrix::Vector3f &dcm_z_sp_prev, float yaw); float get_weathervane_yawrate(); void update_parameters() { ModuleParams::updateParams(); } private: - matrix::Dcmf _R_sp_prev; // previous attitude setpoint rotation matrix - float _yaw = 0.0f; // current yaw angle + matrix::Vector3f _dcm_z_sp_prev; ///< previous attitude setpoint body z axis + float _yaw = 0.0f; ///< current yaw angle bool _is_active = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index b7fca6969e..9240931a7a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -555,7 +555,7 @@ MulticopterPositionControl::Run() } } - _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); + _wv_controller->update(Quatf(_att_sp.q_d).dcm_z(), _states.yaw); } // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes