From ce7272a39c7c79a7c5ed76751083a1be231d7e00 Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Mon, 17 Sep 2018 10:09:42 +0200
Subject: [PATCH] WeatherVane: calculate rotation matrix directly when updating
Signed-off-by: Roman <bapstroman@gmail.com>
---
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) {
--
GitLab