[DO NOT MERGE] mc_att_control debug heading reset

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);

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

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;