forked from Archive/PX4-Autopilot
[DO NOT MERGE] mc_att_control debug heading reset
This commit is contained in:
parent
982c998ab9
commit
060aa496de
|
@ -117,8 +117,8 @@ void LoggedTopics::add_default_topics()
|
||||||
add_topic("vehicle_acceleration", 50);
|
add_topic("vehicle_acceleration", 50);
|
||||||
add_topic("vehicle_air_data", 200);
|
add_topic("vehicle_air_data", 200);
|
||||||
add_topic("vehicle_angular_velocity", 20);
|
add_topic("vehicle_angular_velocity", 20);
|
||||||
add_topic("vehicle_attitude", 50);
|
add_topic("vehicle_attitude", 0);
|
||||||
add_topic("vehicle_attitude_setpoint", 50);
|
add_topic("vehicle_attitude_setpoint", 0);
|
||||||
add_topic("vehicle_command");
|
add_topic("vehicle_command");
|
||||||
add_topic("vehicle_command_ack");
|
add_topic("vehicle_command_ack");
|
||||||
add_topic("vehicle_constraints", 1000);
|
add_topic("vehicle_constraints", 1000);
|
||||||
|
@ -127,9 +127,9 @@ void LoggedTopics::add_default_topics()
|
||||||
add_topic("vehicle_gps_position", 100);
|
add_topic("vehicle_gps_position", 100);
|
||||||
add_topic("vehicle_land_detected");
|
add_topic("vehicle_land_detected");
|
||||||
add_topic("vehicle_local_position", 100);
|
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_magnetometer", 200);
|
||||||
add_topic("vehicle_rates_setpoint", 20);
|
add_topic("vehicle_rates_setpoint", 0);
|
||||||
add_topic("vehicle_roi", 1000);
|
add_topic("vehicle_roi", 1000);
|
||||||
add_topic("vehicle_status");
|
add_topic("vehicle_status");
|
||||||
add_optional_topic("vtol_vehicle_status", 200);
|
add_optional_topic("vtol_vehicle_status", 200);
|
||||||
|
|
|
@ -93,6 +93,8 @@ public:
|
||||||
_attitude_setpoint_q.normalize();
|
_attitude_setpoint_q.normalize();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const auto &attitude_setpoint_q() const { return _attitude_setpoint_q; }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Run one control loop cycle calculation
|
* Run one control loop cycle calculation
|
||||||
* @param q estimation of the current vehicle attitude unit quaternion
|
* @param q estimation of the current vehicle attitude unit quaternion
|
||||||
|
|
|
@ -301,6 +301,9 @@ MulticopterAttitudeControl::Run()
|
||||||
|
|
||||||
// Check for a heading reset
|
// Check for a heading reset
|
||||||
if (_quat_reset_counter != v_att.quat_reset_counter) {
|
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);
|
const Quatf delta_q_reset(v_att.delta_q_reset);
|
||||||
|
|
||||||
// for stabilized attitude generation only extract the heading change from the delta quaternion
|
// 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) {
|
if (v_att.timestamp > _last_attitude_setpoint) {
|
||||||
// adapt existing attitude setpoint unless it was generated after the current attitude estimate
|
// adapt existing attitude setpoint unless it was generated after the current attitude estimate
|
||||||
_attitude_control.adaptAttitudeSetpoint(delta_q_reset);
|
_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;
|
_quat_reset_counter = v_att.quat_reset_counter;
|
||||||
|
|
Loading…
Reference in New Issue