mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 16:13:33 +08:00
delete lib/mixer and mixer_module static mixing
This commit is contained in:
Daniel Agar
@@ -63,6 +63,5 @@ px4_add_library(mixer_module
|
||||
add_dependencies(mixer_module output_functions_header)
|
||||
target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(mixer_module PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
|
||||
target_link_libraries(mixer_module PRIVATE mixer)
|
||||
|
||||
px4_add_functional_gtest(SRC mixer_module_tests.cpp LINKLIBS mixer_module mixer)
|
||||
px4_add_functional_gtest(SRC mixer_module_tests.cpp LINKLIBS mixer_module)
|
||||
|
||||
@@ -33,8 +33,6 @@
|
||||
|
||||
#include "mixer_module.hpp"
|
||||
|
||||
#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
@@ -68,21 +66,15 @@ static const FunctionProvider all_function_providers[] = {
|
||||
};
|
||||
|
||||
MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface,
|
||||
SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up)
|
||||
: ModuleParams(&interface),
|
||||
_output_ramp_up(ramp_up),
|
||||
_control_subs{
|
||||
{&interface, ORB_ID(actuator_controls_0)},
|
||||
{&interface, ORB_ID(actuator_controls_1)},
|
||||
{&interface, ORB_ID(actuator_controls_2)},
|
||||
{&interface, ORB_ID(actuator_controls_3)},
|
||||
},
|
||||
_scheduling_policy(scheduling_policy),
|
||||
_support_esc_calibration(support_esc_calibration),
|
||||
_max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS),
|
||||
_interface(interface),
|
||||
_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")),
|
||||
_param_prefix(param_prefix)
|
||||
SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up) :
|
||||
ModuleParams(&interface),
|
||||
_output_ramp_up(ramp_up),
|
||||
_scheduling_policy(scheduling_policy),
|
||||
_support_esc_calibration(support_esc_calibration),
|
||||
_max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS),
|
||||
_interface(interface),
|
||||
_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")),
|
||||
_param_prefix(param_prefix)
|
||||
{
|
||||
/* Safely initialize armed flags */
|
||||
_armed.armed = false;
|
||||
@@ -94,28 +86,20 @@ _param_prefix(param_prefix)
|
||||
|
||||
px4_sem_init(&_lock, 0, 1);
|
||||
|
||||
_use_dynamic_mixing = _param_sys_ctrl_alloc.get();
|
||||
initParamHandles();
|
||||
|
||||
if (_use_dynamic_mixing) {
|
||||
initParamHandles();
|
||||
|
||||
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
|
||||
_failsafe_value[i] = UINT16_MAX;
|
||||
}
|
||||
|
||||
updateParams();
|
||||
|
||||
} else {
|
||||
_control_allocator_status_pub.advertise();
|
||||
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
|
||||
_failsafe_value[i] = UINT16_MAX;
|
||||
}
|
||||
|
||||
updateParams();
|
||||
|
||||
_outputs_pub.advertise();
|
||||
}
|
||||
|
||||
MixingOutput::~MixingOutput()
|
||||
{
|
||||
perf_free(_control_latency_perf);
|
||||
delete _mixers;
|
||||
px4_sem_destroy(&_lock);
|
||||
|
||||
cleanupFunctions();
|
||||
@@ -153,28 +137,12 @@ void MixingOutput::printStatus() const
|
||||
PX4_INFO("Switched to rate_ctrl work queue");
|
||||
}
|
||||
|
||||
|
||||
if (!_use_dynamic_mixing) {
|
||||
PX4_INFO("Mixer loaded: %s", _mixers ? "yes" : "no");
|
||||
PX4_INFO("Driver instance: %i", _driver_instance);
|
||||
}
|
||||
|
||||
PX4_INFO_RAW("Channel Configuration:\n");
|
||||
|
||||
if (_use_dynamic_mixing) {
|
||||
for (unsigned i = 0; i < _max_num_outputs; i++) {
|
||||
PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i,
|
||||
(int)_function_assignment[i], _current_output_value[i],
|
||||
actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i]);
|
||||
}
|
||||
|
||||
} else {
|
||||
for (unsigned i = 0; i < _max_num_outputs; i++) {
|
||||
int reordered_i = reorderedMotorIndex(i);
|
||||
PX4_INFO_RAW("Channel %i: value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", reordered_i,
|
||||
_current_output_value[i],
|
||||
_failsafe_value[reordered_i], _disarmed_value[reordered_i], _min_value[reordered_i], _max_value[reordered_i]);
|
||||
}
|
||||
for (unsigned i = 0; i < _max_num_outputs; i++) {
|
||||
PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i,
|
||||
(int)_function_assignment[i], _current_output_value[i],
|
||||
actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i]);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -182,154 +150,54 @@ void MixingOutput::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
|
||||
// update mixer if we have one
|
||||
if (_mixers) {
|
||||
if (_param_mot_slew_max.get() <= FLT_EPSILON) {
|
||||
_mixers->set_max_delta_out_once(0.f);
|
||||
_param_mot_ordering.set(0); // not used with dynamic mixing
|
||||
|
||||
bool function_changed = false;
|
||||
|
||||
for (unsigned i = 0; i < _max_num_outputs; i++) {
|
||||
int32_t val;
|
||||
|
||||
if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &val) == 0) {
|
||||
if (val != (int32_t)_function_assignment[i]) {
|
||||
function_changed = true;
|
||||
}
|
||||
|
||||
// we set _function_assignment[i] later to ensure _functions[i] is updated at the same time
|
||||
}
|
||||
|
||||
_mixers->set_thrust_factor(_param_thr_mdl_fac.get());
|
||||
_mixers->set_airmode((Mixer::Airmode)_param_mc_airmode.get());
|
||||
}
|
||||
|
||||
if (_use_dynamic_mixing) {
|
||||
_param_mot_ordering.set(0); // not used with dynamic mixing
|
||||
|
||||
bool function_changed = false;
|
||||
|
||||
for (unsigned i = 0; i < _max_num_outputs; i++) {
|
||||
int32_t val;
|
||||
|
||||
if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &val) == 0) {
|
||||
if (val != (int32_t)_function_assignment[i]) {
|
||||
function_changed = true;
|
||||
}
|
||||
|
||||
// we set _function_assignment[i] later to ensure _functions[i] is updated at the same time
|
||||
}
|
||||
|
||||
if (_param_handles[i].disarmed != PARAM_INVALID && param_get(_param_handles[i].disarmed, &val) == 0) {
|
||||
_disarmed_value[i] = val;
|
||||
}
|
||||
|
||||
if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &val) == 0) {
|
||||
_min_value[i] = val;
|
||||
}
|
||||
|
||||
if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) {
|
||||
_max_value[i] = val;
|
||||
}
|
||||
|
||||
if (_min_value[i] > _max_value[i]) {
|
||||
uint16_t tmp = _min_value[i];
|
||||
_min_value[i] = _max_value[i];
|
||||
_max_value[i] = tmp;
|
||||
}
|
||||
|
||||
if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) {
|
||||
_failsafe_value[i] = val;
|
||||
}
|
||||
if (_param_handles[i].disarmed != PARAM_INVALID && param_get(_param_handles[i].disarmed, &val) == 0) {
|
||||
_disarmed_value[i] = val;
|
||||
}
|
||||
|
||||
_reverse_output_mask = 0;
|
||||
int32_t rev_range_param;
|
||||
|
||||
if (_param_handle_rev_range != PARAM_INVALID && param_get(_param_handle_rev_range, &rev_range_param) == 0) {
|
||||
_reverse_output_mask = rev_range_param;
|
||||
if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &val) == 0) {
|
||||
_min_value[i] = val;
|
||||
}
|
||||
|
||||
if (function_changed) {
|
||||
_need_function_update = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool MixingOutput::updateSubscriptions(bool allow_wq_switch, bool limit_callbacks_to_primary)
|
||||
{
|
||||
if (_use_dynamic_mixing) {
|
||||
return updateSubscriptionsDynamicMixer(allow_wq_switch, limit_callbacks_to_primary);
|
||||
|
||||
} else {
|
||||
return updateSubscriptionsStaticMixer(allow_wq_switch, limit_callbacks_to_primary);
|
||||
}
|
||||
}
|
||||
|
||||
bool MixingOutput::updateSubscriptionsStaticMixer(bool allow_wq_switch, bool limit_callbacks_to_primary)
|
||||
{
|
||||
if (_groups_subscribed == _groups_required) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// must be locked to potentially change WorkQueue
|
||||
lock();
|
||||
|
||||
if (_scheduling_policy == SchedulingPolicy::Auto) {
|
||||
// first clear everything
|
||||
unregister();
|
||||
_interface.ScheduleClear();
|
||||
|
||||
// if subscribed to control group 0 or 1 then move to the rate_ctrl WQ
|
||||
const bool sub_group_0 = (_groups_required & (1 << 0));
|
||||
const bool sub_group_1 = (_groups_required & (1 << 1));
|
||||
|
||||
if (allow_wq_switch && !_wq_switched && (sub_group_0 || sub_group_1)) {
|
||||
if (_interface.ChangeWorkQueue(px4::wq_configurations::rate_ctrl)) {
|
||||
// let the new WQ handle the subscribe update
|
||||
_wq_switched = true;
|
||||
_interface.ScheduleNow();
|
||||
unlock();
|
||||
return false;
|
||||
}
|
||||
if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) {
|
||||
_max_value[i] = val;
|
||||
}
|
||||
|
||||
bool sub_group_0_callback_registered = false;
|
||||
bool sub_group_1_callback_registered = false;
|
||||
|
||||
// register callback to all required actuator control groups
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
|
||||
if (limit_callbacks_to_primary) {
|
||||
// don't register additional callbacks if actuator_controls_0 or actuator_controls_1 are already registered
|
||||
if ((i > 1) && (sub_group_0_callback_registered || sub_group_1_callback_registered)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (_groups_required & (1 << i)) {
|
||||
if (_control_subs[i].registerCallback()) {
|
||||
PX4_DEBUG("subscribed to actuator_controls_%d", i);
|
||||
|
||||
if (limit_callbacks_to_primary) {
|
||||
if (i == 0) {
|
||||
sub_group_0_callback_registered = true;
|
||||
|
||||
} else if (i == 1) {
|
||||
sub_group_1_callback_registered = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("actuator_controls_%d register callback failed!", i);
|
||||
}
|
||||
}
|
||||
if (_min_value[i] > _max_value[i]) {
|
||||
uint16_t tmp = _min_value[i];
|
||||
_min_value[i] = _max_value[i];
|
||||
_max_value[i] = tmp;
|
||||
}
|
||||
|
||||
// if nothing required keep periodic schedule (so the module can update other things)
|
||||
if (_groups_required == 0) {
|
||||
// TODO: this might need to be configurable depending on the module
|
||||
_interface.ScheduleOnInterval(100_ms);
|
||||
if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) {
|
||||
_failsafe_value[i] = val;
|
||||
}
|
||||
}
|
||||
|
||||
_groups_subscribed = _groups_required;
|
||||
setMaxTopicUpdateRate(_max_topic_update_interval_us);
|
||||
_reverse_output_mask = 0;
|
||||
int32_t rev_range_param;
|
||||
|
||||
PX4_DEBUG("_groups_required 0x%08" PRIx32, _groups_required);
|
||||
PX4_DEBUG("_groups_subscribed 0x%08" PRIx32, _groups_subscribed);
|
||||
if (_param_handle_rev_range != PARAM_INVALID && param_get(_param_handle_rev_range, &rev_range_param) == 0) {
|
||||
_reverse_output_mask = rev_range_param;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
return true;
|
||||
if (function_changed) {
|
||||
_need_function_update = true;
|
||||
}
|
||||
}
|
||||
|
||||
void MixingOutput::cleanupFunctions()
|
||||
@@ -346,7 +214,7 @@ void MixingOutput::cleanupFunctions()
|
||||
}
|
||||
}
|
||||
|
||||
bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool limit_callbacks_to_primary)
|
||||
bool MixingOutput::updateSubscriptions(bool allow_wq_switch, bool limit_callbacks_to_primary)
|
||||
{
|
||||
if (!_need_function_update || _armed.armed) {
|
||||
return false;
|
||||
@@ -491,17 +359,8 @@ void MixingOutput::setMaxTopicUpdateRate(unsigned max_topic_update_interval_us)
|
||||
{
|
||||
_max_topic_update_interval_us = max_topic_update_interval_us;
|
||||
|
||||
if (_use_dynamic_mixing) {
|
||||
if (_subscription_callback) {
|
||||
_subscription_callback->set_interval_us(_max_topic_update_interval_us);
|
||||
}
|
||||
|
||||
} else {
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (_groups_subscribed & (1 << i)) {
|
||||
_control_subs[i].set_interval_us(_max_topic_update_interval_us);
|
||||
}
|
||||
}
|
||||
if (_subscription_callback) {
|
||||
_subscription_callback->set_interval_us(_max_topic_update_interval_us);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -539,45 +398,18 @@ void MixingOutput::setAllDisarmedValues(uint16_t value)
|
||||
|
||||
void MixingOutput::unregister()
|
||||
{
|
||||
for (auto &control_sub : _control_subs) {
|
||||
control_sub.unregisterCallback();
|
||||
}
|
||||
|
||||
if (_subscription_callback) {
|
||||
_subscription_callback->unregisterCallback();
|
||||
}
|
||||
}
|
||||
|
||||
void MixingOutput::updateOutputSlewrateMultirotorMixer()
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
const float dt = math::constrain((now - _time_last_dt_update_multicopter) / 1e6f, 0.0001f, 0.02f);
|
||||
_time_last_dt_update_multicopter = now;
|
||||
|
||||
// maximum value the outputs of the multirotor mixer are allowed to change in this cycle
|
||||
// factor 2 is needed because actuator outputs are in the range [-1,1]
|
||||
const float delta_out_max = 2.0f * 1000.0f * dt / (_max_value[0] - _min_value[0]) / _param_mot_slew_max.get();
|
||||
_mixers->set_max_delta_out_once(delta_out_max);
|
||||
}
|
||||
|
||||
void MixingOutput::updateOutputSlewrateSimplemixer()
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
const float dt = math::constrain((now - _time_last_dt_update_simple_mixer) / 1e6f, 0.0001f, 0.02f);
|
||||
_time_last_dt_update_simple_mixer = now;
|
||||
|
||||
// set dt for slew rate limiter in SimpleMixer (is reset internally after usig it, so needs to be set on every update)
|
||||
_mixers->set_dt_once(dt);
|
||||
}
|
||||
|
||||
|
||||
unsigned MixingOutput::motorTest()
|
||||
{
|
||||
test_motor_s test_motor;
|
||||
bool had_update = false;
|
||||
|
||||
while (_motor_test.test_motor_sub.update(&test_motor)) {
|
||||
if (test_motor.driver_instance != _driver_instance ||
|
||||
if (test_motor.driver_instance != 0 ||
|
||||
test_motor.timestamp == 0 ||
|
||||
hrt_elapsed_time(&test_motor.timestamp) > 100_ms) {
|
||||
continue;
|
||||
@@ -634,123 +466,6 @@ unsigned MixingOutput::motorTest()
|
||||
}
|
||||
|
||||
bool MixingOutput::update()
|
||||
{
|
||||
if (_use_dynamic_mixing) {
|
||||
return updateDynamicMixer();
|
||||
|
||||
} else {
|
||||
return updateStaticMixer();
|
||||
}
|
||||
}
|
||||
bool MixingOutput::updateStaticMixer()
|
||||
{
|
||||
if (!_mixers) {
|
||||
// do nothing until we have a valid mixer
|
||||
return false;
|
||||
}
|
||||
|
||||
// check arming state
|
||||
if (_armed_sub.update(&_armed)) {
|
||||
_armed.in_esc_calibration_mode &= _support_esc_calibration;
|
||||
|
||||
if (_ignore_lockdown) {
|
||||
_armed.lockdown = false;
|
||||
}
|
||||
|
||||
/* Update the armed status and check that we're not locked down.
|
||||
* We also need to arm throttle for the ESC calibration. */
|
||||
_throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode;
|
||||
|
||||
if (_armed.armed) {
|
||||
_motor_test.in_test_mode = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (_param_mot_slew_max.get() > FLT_EPSILON) {
|
||||
updateOutputSlewrateMultirotorMixer();
|
||||
}
|
||||
|
||||
updateOutputSlewrateSimplemixer(); // update dt for output slew rate in simple mixer
|
||||
|
||||
unsigned n_updates = 0;
|
||||
|
||||
/* get controls for required topics */
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (_groups_subscribed & (1 << i)) {
|
||||
if (_control_subs[i].copy(&_controls[i])) {
|
||||
n_updates++;
|
||||
}
|
||||
|
||||
/* During ESC calibration, we overwrite the throttle value. */
|
||||
if (i == 0 && _armed.in_esc_calibration_mode) {
|
||||
|
||||
/* Set all controls to 0 */
|
||||
memset(&_controls[i], 0, sizeof(_controls[i]));
|
||||
|
||||
/* except thrust to maximum. */
|
||||
_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
|
||||
|
||||
/* Switch off the output limit ramp for the calibration. */
|
||||
_output_state = OutputLimitState::ON;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// check for motor test (after topic updates)
|
||||
if (!_armed.armed && !_armed.manual_lockdown) {
|
||||
unsigned num_motor_test = motorTest();
|
||||
|
||||
if (num_motor_test > 0) {
|
||||
if (_interface.updateOutputs(false, _current_output_value, num_motor_test, 1)) {
|
||||
actuator_outputs_s actuator_outputs{};
|
||||
setAndPublishActuatorOutputs(num_motor_test, actuator_outputs);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
/* do mixing */
|
||||
float outputs[MAX_ACTUATORS] {};
|
||||
const unsigned mixed_num_outputs = _mixers->mix(outputs, _max_num_outputs);
|
||||
|
||||
/* the output limit call takes care of out of band errors, NaN and constrains */
|
||||
output_limit_calc(_throttle_armed, mixed_num_outputs, outputs);
|
||||
|
||||
/* overwrite outputs in case of force_failsafe with _failsafe_value values */
|
||||
if (_armed.force_failsafe) {
|
||||
for (size_t i = 0; i < mixed_num_outputs; i++) {
|
||||
_current_output_value[i] = _failsafe_value[i];
|
||||
}
|
||||
}
|
||||
|
||||
bool stop_motors = mixed_num_outputs == 0 || !_throttle_armed;
|
||||
|
||||
/* overwrite outputs in case of lockdown with disarmed values */
|
||||
if (_armed.lockdown || _armed.manual_lockdown) {
|
||||
for (size_t i = 0; i < mixed_num_outputs; i++) {
|
||||
_current_output_value[i] = _disarmed_value[i];
|
||||
}
|
||||
|
||||
stop_motors = true;
|
||||
}
|
||||
|
||||
/* apply _param_mot_ordering */
|
||||
reorderOutputs(_current_output_value);
|
||||
|
||||
/* now return the outputs to the driver */
|
||||
if (_interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates)) {
|
||||
actuator_outputs_s actuator_outputs{};
|
||||
setAndPublishActuatorOutputs(mixed_num_outputs, actuator_outputs);
|
||||
|
||||
publishMixerStatus(actuator_outputs);
|
||||
updateLatencyPerfCounter(actuator_outputs);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MixingOutput::updateDynamicMixer()
|
||||
{
|
||||
// check arming state
|
||||
if (_armed_sub.update(&_armed)) {
|
||||
@@ -1019,84 +734,15 @@ MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_output
|
||||
_outputs_pub.publish(actuator_outputs);
|
||||
}
|
||||
|
||||
void
|
||||
MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
|
||||
{
|
||||
MultirotorMixer::saturation_status saturation_status;
|
||||
saturation_status.value = _mixers->get_saturation_status();
|
||||
|
||||
if (saturation_status.flags.valid) {
|
||||
control_allocator_status_s sat{};
|
||||
sat.timestamp = hrt_absolute_time();
|
||||
sat.torque_setpoint_achieved = true;
|
||||
sat.thrust_setpoint_achieved = true;
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs)
|
||||
{
|
||||
if (_use_dynamic_mixing) {
|
||||
// Just check the first function. It means we only get the latency if motors are assigned first, which is the default
|
||||
if (_function_allocated[0]) {
|
||||
hrt_abstime timestamp_sample;
|
||||
// Just check the first function. It means we only get the latency if motors are assigned first, which is the default
|
||||
if (_function_allocated[0]) {
|
||||
hrt_abstime timestamp_sample;
|
||||
|
||||
if (_function_allocated[0]->getLatestSampleTimestamp(timestamp_sample)) {
|
||||
perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
// use first valid timestamp_sample for latency tracking
|
||||
for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
const bool required = _groups_required & (1 << i);
|
||||
const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample;
|
||||
|
||||
if (required && (timestamp_sample > 0)) {
|
||||
perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample);
|
||||
break;
|
||||
}
|
||||
if (_function_allocated[0]->getLatestSampleTimestamp(timestamp_sample)) {
|
||||
perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1104,10 +750,6 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output
|
||||
uint16_t
|
||||
MixingOutput::actualFailsafeValue(int index) const
|
||||
{
|
||||
if (!_use_dynamic_mixing) {
|
||||
return _failsafe_value[index];
|
||||
}
|
||||
|
||||
uint16_t value = 0;
|
||||
|
||||
if (_failsafe_value[index] == UINT16_MAX) { // if set to default, use the one provided by the function
|
||||
@@ -1126,34 +768,6 @@ MixingOutput::actualFailsafeValue(int index) const
|
||||
return value;
|
||||
}
|
||||
|
||||
void
|
||||
MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
|
||||
{
|
||||
if (MAX_ACTUATORS < 4) {
|
||||
return;
|
||||
}
|
||||
|
||||
if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
|
||||
/*
|
||||
* Betaflight default motor ordering:
|
||||
* 4 2
|
||||
* ^
|
||||
* 3 1
|
||||
*/
|
||||
const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3] };
|
||||
values[0] = value_tmp[3];
|
||||
values[1] = value_tmp[0];
|
||||
values[2] = value_tmp[1];
|
||||
values[3] = value_tmp[2];
|
||||
}
|
||||
|
||||
/* else: PX4, no need to reorder
|
||||
* 3 1
|
||||
* ^
|
||||
* 2 4
|
||||
*/
|
||||
}
|
||||
|
||||
int MixingOutput::reorderedMotorIndex(int index) const
|
||||
{
|
||||
if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
|
||||
@@ -1170,37 +784,3 @@ int MixingOutput::reorderedMotorIndex(int index) const
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
|
||||
{
|
||||
const MixingOutput *output = (const MixingOutput *)handle;
|
||||
|
||||
input = output->_controls[control_group].control[control_index];
|
||||
|
||||
/* limit control input */
|
||||
input = math::constrain(input, -1.f, 1.f);
|
||||
|
||||
/* motor spinup phase - lock throttle to zero */
|
||||
if (output->_output_state == OutputLimitState::RAMP) {
|
||||
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
|
||||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
|
||||
control_index == actuator_controls_s::INDEX_THROTTLE) {
|
||||
/* limit the throttle output to zero during motor spinup,
|
||||
* as the motors cannot follow any demand yet
|
||||
*/
|
||||
input = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* throttle not arming - mark throttle input as invalid */
|
||||
if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) {
|
||||
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
|
||||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
|
||||
control_index == actuator_controls_s::INDEX_THROTTLE) {
|
||||
/* set the throttle to an invalid value */
|
||||
input = NAN;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -48,7 +48,6 @@
|
||||
|
||||
#include <board_config.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <lib/mixer/MixerGroup.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
@@ -57,9 +56,7 @@
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/control_allocator_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/test_motor.h>
|
||||
|
||||
@@ -74,8 +71,10 @@ class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams
|
||||
public:
|
||||
static constexpr int MAX_ACTUATORS = PWM_OUTPUT_MAX_CHANNELS;
|
||||
|
||||
OutputModuleInterface(const char *name, const px4::wq_config_t &config)
|
||||
: px4::ScheduledWorkItem(name, config), ModuleParams(nullptr) {}
|
||||
OutputModuleInterface(const char *name, const px4::wq_config_t &config) :
|
||||
px4::ScheduledWorkItem(name, config),
|
||||
ModuleParams(nullptr)
|
||||
{}
|
||||
|
||||
/**
|
||||
* Callback to update the (physical) actuator outputs in the driver
|
||||
@@ -125,21 +124,21 @@ public:
|
||||
|
||||
~MixingOutput();
|
||||
|
||||
void setDriverInstance(uint8_t instance) { _driver_instance = instance; }
|
||||
|
||||
void printStatus() const;
|
||||
|
||||
bool useDynamicMixing() const { return _use_dynamic_mixing; }
|
||||
|
||||
/**
|
||||
* Permanently disable an output function
|
||||
*/
|
||||
void disableFunction(int index) { _param_handles[index].function = PARAM_INVALID; _need_function_update = true; }
|
||||
void disableFunction(int index)
|
||||
{
|
||||
_param_handles[index].function = PARAM_INVALID;
|
||||
_need_function_update = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if a function is configured, i.e. not set to Disabled and initialized
|
||||
*/
|
||||
bool isFunctionSet(int index) const { return !_use_dynamic_mixing || _functions[index] != nullptr; }
|
||||
bool isFunctionSet(int index) const { return _functions[index] != nullptr; }
|
||||
|
||||
OutputFunction outputFunction(int index) const { return _function_assignment[index]; }
|
||||
|
||||
@@ -150,7 +149,7 @@ public:
|
||||
bool update();
|
||||
|
||||
/**
|
||||
* Check for subscription updates (e.g. after a mixer is loaded).
|
||||
* Check for subscription updates.
|
||||
* Call this at the very end of Run() if allow_wq_switch
|
||||
* @param allow_wq_switch if true
|
||||
* @param limit_callbacks_to_primary set to only register callbacks for primary actuator controls (if used)
|
||||
@@ -167,10 +166,6 @@ public:
|
||||
|
||||
const actuator_armed_s &armed() const { return _armed; }
|
||||
|
||||
bool initialized() const { return _use_dynamic_mixing || _mixers != nullptr; }
|
||||
|
||||
MixerGroup *mixers() const { return _mixers; }
|
||||
|
||||
void setAllFailsafeValues(uint16_t value);
|
||||
void setAllDisarmedValues(uint16_t value);
|
||||
void setAllMinValues(uint16_t value);
|
||||
@@ -219,11 +214,6 @@ protected:
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
bool updateSubscriptionsStaticMixer(bool allow_wq_switch, bool limit_callbacks_to_primary);
|
||||
bool updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool limit_callbacks_to_primary);
|
||||
|
||||
bool updateStaticMixer();
|
||||
bool updateDynamicMixer();
|
||||
|
||||
bool armNoThrottle() const
|
||||
{
|
||||
@@ -232,14 +222,10 @@ private:
|
||||
|
||||
unsigned motorTest();
|
||||
|
||||
void updateOutputSlewrateMultirotorMixer();
|
||||
void updateOutputSlewrateSimplemixer();
|
||||
void setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs);
|
||||
void publishMixerStatus(const actuator_outputs_s &actuator_outputs);
|
||||
void updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs);
|
||||
|
||||
static int controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
|
||||
|
||||
void cleanupFunctions();
|
||||
|
||||
void initParamHandles();
|
||||
@@ -263,12 +249,6 @@ private:
|
||||
Betaflight = 1
|
||||
};
|
||||
|
||||
/**
|
||||
* Reorder outputs according to _param_mot_ordering
|
||||
* @param values values to reorder
|
||||
*/
|
||||
inline void reorderOutputs(uint16_t values[MAX_ACTUATORS]);
|
||||
|
||||
void lock() { do {} while (px4_sem_wait(&_lock) != 0); }
|
||||
void unlock() { px4_sem_post(&_lock); }
|
||||
|
||||
@@ -292,30 +272,20 @@ private:
|
||||
const bool _output_ramp_up; ///< if true, motors will ramp up from disarmed to min_output after arming
|
||||
|
||||
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
|
||||
|
||||
uORB::PublicationMulti<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs)};
|
||||
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{};
|
||||
|
||||
hrt_abstime _time_last_dt_update_multicopter{0};
|
||||
hrt_abstime _time_last_dt_update_simple_mixer{0};
|
||||
unsigned _max_topic_update_interval_us{0}; ///< max _control_subs topic update interval (0=unlimited)
|
||||
unsigned _max_topic_update_interval_us{0}; ///< max topic update interval (0=unlimited)
|
||||
|
||||
bool _throttle_armed{false};
|
||||
bool _ignore_lockdown{false}; ///< if true, ignore the _armed.lockdown flag (for HIL outputs)
|
||||
|
||||
MixerGroup *_mixers{nullptr};
|
||||
uint32_t _groups_required{0};
|
||||
uint32_t _groups_subscribed{1u << 31}; ///< initialize to a different value than _groups_required and outside of (1 << NUM_ACTUATOR_CONTROL_GROUPS)
|
||||
|
||||
const SchedulingPolicy _scheduling_policy;
|
||||
const bool _support_esc_calibration;
|
||||
|
||||
bool _wq_switched{false};
|
||||
uint8_t _driver_instance{0}; ///< for boards that supports multiple outputs (e.g. PX4IO + FMU)
|
||||
uint8_t _max_num_outputs;
|
||||
|
||||
struct MotorTest {
|
||||
@@ -334,7 +304,6 @@ private:
|
||||
FunctionProviderBase *_functions[MAX_ACTUATORS] {}; ///< currently assigned functions
|
||||
OutputFunction _function_assignment[MAX_ACTUATORS] {};
|
||||
bool _need_function_update{true};
|
||||
bool _use_dynamic_mixing{false}; ///< set to _param_sys_ctrl_alloc on init (avoid changing after startup)
|
||||
bool _has_backup_schedule{false};
|
||||
const char *const _param_prefix;
|
||||
ParamHandles _param_handles[MAX_ACTUATORS];
|
||||
|
||||
Reference in New Issue
Block a user