fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 12:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

control_allocator: remove direct mixer, add actuator_{motors,servos} instead

Browse Source
This commit is contained in:
Beat Küng
2021-10-18 18:45:19 -04:00
committed by Daniel Agar
parent a51c465b54
commit 38fa65a47e
21 changed files with 49 additions and 371 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp
+2 -2
View File
@@ -97,10 +97,10 @@ const
for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) {
if (_actuator_min(i) < _actuator_max(i)) {
actuator_normalized(i) = -1.0f + 2.0f * (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
actuator_normalized(i) = (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
} else {
actuator_normalized(i) = -1.0f + 2.0f * (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
actuator_normalized(i) = (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
}
}
src/modules/control_allocator/ControlAllocator.cpp
+8 -13
View File
@@ -436,26 +436,21 @@ ControlAllocator::publish_control_allocator_status()
void
ControlAllocator::publish_legacy_actuator_controls()
{
// For compatibility with the current mixer system,
// publish normalized version on actuator_controls_4/5
actuator_controls_s actuator_controls_4{};
actuator_controls_s actuator_controls_5{};
actuator_controls_4.timestamp = hrt_absolute_time();
actuator_controls_5.timestamp = hrt_absolute_time();
actuator_controls_4.timestamp_sample = _timestamp_sample;
actuator_controls_5.timestamp_sample = _timestamp_sample;
actuator_motors_s actuator_motors;
actuator_motors.timestamp = hrt_absolute_time();
actuator_motors.timestamp_sample = _timestamp_sample;
matrix::Vector<float, NUM_ACTUATORS> actuator_sp = _control_allocation->getActuatorSetpoint();
matrix::Vector<float, NUM_ACTUATORS> actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint(
actuator_sp);
for (size_t i = 0; i < 8; i++) {
actuator_controls_4.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i))) ? actuator_sp_normalized(i) : 0.0f;
actuator_controls_5.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i + 8))) ? actuator_sp_normalized(i + 8) : 0.0f;
for (size_t i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
actuator_motors.control[i] = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN;
}
_actuator_controls_4_pub.publish(actuator_controls_4);
_actuator_controls_5_pub.publish(actuator_controls_5);
_actuator_motors_pub.publish(actuator_motors);
// TODO: servos
}
int ControlAllocator::task_spawn(int argc, char *argv[])
src/modules/control_allocator/ControlAllocator.hpp
+4 -4
View File
@@ -59,7 +59,8 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_allocator_status.h>
@@ -140,9 +141,8 @@ private:
uORB::Publication<vehicle_actuator_setpoint_s> _vehicle_actuator_setpoint_pub{ORB_ID(vehicle_actuator_setpoint)}; /**< actuator setpoint publication */
uORB::Publication<control_allocator_status_s> _control_allocator_status_pub{ORB_ID(control_allocator_status)}; /**< actuator setpoint publication */
// actuator_controls publication handles (temporary hack to plug actuator_setpoint into the mixer system)
uORB::Publication<actuator_controls_s> _actuator_controls_4_pub{ORB_ID(actuator_controls_4)}; /**< actuator controls 4 publication */
uORB::Publication<actuator_controls_s> _actuator_controls_5_pub{ORB_ID(actuator_controls_5)}; /**< actuator controls 5 publication */
uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};