fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 19:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC mixer: replace multirotor_motor_limits by control_allocator_status

Browse Source 
CA: fix saturation computation
Since the CA matrix is normalized, the same scale applied to be used when using the effectiveness matrix

MCRateControl: use control_allocator_status to get saturation info
This commit is contained in:
bresch
2021-11-09 10:35:10 -05:00
committed by Daniel Agar
parent dd83ef1813
commit d47f9f155a
17 changed files with 115 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mixer_module/mixer_module.cpp
+44 -4
View File
@@ -861,11 +861,51 @@ MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
saturation_status.value = _mixers->get_saturation_status();
if (saturation_status.flags.valid) {
multirotor_motor_limits_s motor_limits;
motor_limits.timestamp = actuator_outputs.timestamp;
motor_limits.saturation_status = saturation_status.value;
control_allocator_status_s sat{};
sat.timestamp = hrt_absolute_time();
sat.torque_setpoint_achieved = true;
sat.thrust_setpoint_achieved = true;
_to_mixer_status.publish(motor_limits);
// Note: the values '-1', '1' and '0' are just to indicate a negative,
// positive or no saturation to the rate controller. The actual magnitude
// is not used.
if (saturation_status.flags.roll_pos) {
sat.unallocated_torque[0] = 1.f;
sat.torque_setpoint_achieved = false;
} else if (saturation_status.flags.roll_neg) {
sat.unallocated_torque[0] = -1.f;
sat.torque_setpoint_achieved = false;
}
if (saturation_status.flags.pitch_pos) {
sat.unallocated_torque[1] = 1.f;
sat.torque_setpoint_achieved = false;
} else if (saturation_status.flags.pitch_neg) {
sat.unallocated_torque[1] = -1.f;
sat.torque_setpoint_achieved = false;
}
if (saturation_status.flags.yaw_pos) {
sat.unallocated_torque[2] = 1.f;
sat.torque_setpoint_achieved = false;
} else if (saturation_status.flags.yaw_neg) {
sat.unallocated_torque[2] = -1.f;
sat.torque_setpoint_achieved = false;
}
if (saturation_status.flags.thrust_pos) {
sat.unallocated_thrust[2] = 1.f;
sat.thrust_setpoint_achieved = false;
} else if (saturation_status.flags.thrust_neg) {
sat.unallocated_thrust[2] = -1.f;
sat.thrust_setpoint_achieved = false;
}
_control_allocator_status_pub.publish(sat);
}
}
src/lib/mixer_module/mixer_module.hpp
+2 -2
View File
@@ -50,7 +50,7 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/test_motor.h>
@@ -294,7 +294,7 @@ private:
uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
uORB::PublicationMulti<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs)};
uORB::PublicationMulti<multirotor_motor_limits_s> _to_mixer_status{ORB_ID(multirotor_motor_limits)}; ///< mixer status flags
uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub{ORB_ID(control_allocator_status)};
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {};
actuator_armed_s _armed{};