From 060aa496de74da2366caff9c2c05b96793130f14 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Feb 2024 18:06:49 -0500 Subject: [PATCH] [DO NOT MERGE] mc_att_control debug heading reset --- src/modules/logger/logged_topics.cpp | 8 ++++---- .../AttitudeControl/AttitudeControl.hpp | 2 ++ src/modules/mc_att_control/mc_att_control_main.cpp | 13 +++++++++++++ 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index c138da8069..b39c079af8 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -117,8 +117,8 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_acceleration", 50); add_topic("vehicle_air_data", 200); add_topic("vehicle_angular_velocity", 20); - add_topic("vehicle_attitude", 50); - add_topic("vehicle_attitude_setpoint", 50); + add_topic("vehicle_attitude", 0); + add_topic("vehicle_attitude_setpoint", 0); add_topic("vehicle_command"); add_topic("vehicle_command_ack"); add_topic("vehicle_constraints", 1000); @@ -127,9 +127,9 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_gps_position", 100); add_topic("vehicle_land_detected"); add_topic("vehicle_local_position", 100); - add_topic("vehicle_local_position_setpoint", 100); + add_topic("vehicle_local_position_setpoint", 0); add_topic("vehicle_magnetometer", 200); - add_topic("vehicle_rates_setpoint", 20); + add_topic("vehicle_rates_setpoint", 0); add_topic("vehicle_roi", 1000); add_topic("vehicle_status"); add_optional_topic("vtol_vehicle_status", 200); diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index e435e91262..7ac3f205c1 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -93,6 +93,8 @@ public: _attitude_setpoint_q.normalize(); } + const auto &attitude_setpoint_q() const { return _attitude_setpoint_q; } + /** * Run one control loop cycle calculation * @param q estimation of the current vehicle attitude unit quaternion diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 39cb033e74..3b7107f00e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -301,6 +301,9 @@ MulticopterAttitudeControl::Run() // Check for a heading reset if (_quat_reset_counter != v_att.quat_reset_counter) { + + const Quatf q_before = _attitude_control.attitude_setpoint_q(); + const Quatf delta_q_reset(v_att.delta_q_reset); // for stabilized attitude generation only extract the heading change from the delta quaternion @@ -309,6 +312,16 @@ MulticopterAttitudeControl::Run() if (v_att.timestamp > _last_attitude_setpoint) { // adapt existing attitude setpoint unless it was generated after the current attitude estimate _attitude_control.adaptAttitudeSetpoint(delta_q_reset); + + const Eulerf euler_before(q_before); + const Eulerf euler_delta(delta_q_reset); + const Eulerf euler_after(_attitude_control.attitude_setpoint_q()); + + PX4_INFO("q reset: [%.4f, %.4f, %.4f] -> [%.4f, %.4f, %.4f] (delta [%.4f, %.4f, %.4f])", + (double)euler_before(0), (double)euler_before(1), (double)euler_before(2), + (double)euler_after(0), (double)euler_after(1), (double)euler_after(2), + (double)euler_delta(0), (double)euler_delta(1), (double)euler_delta(2) + ); } _quat_reset_counter = v_att.quat_reset_counter;