dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

[DO NOT MERGE] mc_att_control debug heading reset

This commit is contained in:
Daniel Agar 2024-02-06 18:06:49 -05:00
parent 982c998ab9
commit 060aa496de
3 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/logger/logged_topics.cpp
View File

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

2
src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp
View File

@ -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

13
src/modules/mc_att_control/mc_att_control_main.cpp
View File

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