From ce7272a39c7c79a7c5ed76751083a1be231d7e00 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 17 Sep 2018 10:09:42 +0200 Subject: [PATCH] WeatherVane: calculate rotation matrix directly when updating Signed-off-by: Roman --- src/lib/WeatherVane/WeatherVane.cpp | 8 +++----- src/lib/WeatherVane/WeatherVane.hpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/lib/WeatherVane/WeatherVane.cpp b/src/lib/WeatherVane/WeatherVane.cpp index e6bdc66774..f5b2343d68 100644 --- a/src/lib/WeatherVane/WeatherVane.cpp +++ b/src/lib/WeatherVane/WeatherVane.cpp @@ -44,21 +44,19 @@ WeatherVane::WeatherVane() : ModuleParams(nullptr) { - _q_sp_prev = matrix::Quatf(); + _R_sp_prev = matrix::Dcmf(); } void WeatherVane::update(matrix::Quatf q_sp_prev, float yaw) { - _q_sp_prev = q_sp_prev; + _R_sp_prev = matrix::Dcmf(q_sp_prev); _yaw = yaw; } float WeatherVane::get_weathervane_yawrate() { - matrix::Dcmf R_sp(_q_sp_prev); - // direction of desired body z axis represented in earth frame - matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + matrix::Vector3f body_z_sp(_R_sp_prev(0, 2), _R_sp_prev(1, 2), _R_sp_prev(2, 2)); // 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. diff --git a/src/lib/WeatherVane/WeatherVane.hpp b/src/lib/WeatherVane/WeatherVane.hpp index d732f64749..7a43557f7a 100644 --- a/src/lib/WeatherVane/WeatherVane.hpp +++ b/src/lib/WeatherVane/WeatherVane.hpp @@ -69,7 +69,7 @@ public: void update_parameters() { ModuleParams::updateParams(); } private: - matrix::Quatf _q_sp_prev; // previous attitude setpoint quaternion + matrix::Dcmf _R_sp_prev; // previous attitude setpoint rotation matrix 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 17a33d66ea..9255993923 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -741,7 +741,7 @@ MulticopterPositionControl::task_main() _att_sp.disable_mc_yaw_control = false; _att_sp.apply_flaps = false; - _wv_controller.update(matrix::Quatf(&_att_sp.q_d[0]), _local_pos.yaw); + _wv_controller.update(matrix::Quatf(_att_sp.q_d), _local_pos.yaw); if (!constraints.landing_gear) { if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) {