diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index d5d9989c86..b187b1b4ea 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -8,7 +8,8 @@ # # Start Control Allocator # -control_allocator start +# control_allocator start +metric_allocator start # # Start Multicopter Rate Controller. diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 1584f93ef6..666041d39f 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -11,7 +11,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_CONTROL_ALLOCATOR=n +CONFIG_MODULES_METRIC_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y diff --git a/src/modules/metric_allocator/CMakeLists.txt b/src/modules/metric_allocator/CMakeLists.txt new file mode 100644 index 0000000000..92664b4d01 --- /dev/null +++ b/src/modules/metric_allocator/CMakeLists.txt @@ -0,0 +1,54 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_module( + MODULE modules__metric_allocator + MAIN metric_allocator + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + STACK_MAIN + 3000 + SRCS + ControlAllocator.cpp + ControlAllocator.hpp + MODULE_CONFIG + module.yaml + DEPENDS + mathlib + ActuatorEffectiveness + ControlAllocation + px4_work_queue + SlewRate +) diff --git a/src/modules/metric_allocator/ControlAllocator.cpp b/src/modules/metric_allocator/ControlAllocator.cpp new file mode 100644 index 0000000000..8155118e13 --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocator.cpp @@ -0,0 +1,879 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocator.cpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#include "ControlAllocator.hpp" + +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; + +ControlAllocator::ControlAllocator() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + _control_allocator_status_pub[0].advertise(); + _control_allocator_status_pub[1].advertise(); + + _actuator_motors_pub.advertise(); + _actuator_servos_pub.advertise(); + _actuator_servos_trim_pub.advertise(); + + for (int i = 0; i < MAX_NUM_MOTORS; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i); + _param_handles.slew_rate_motors[i] = param_find(buffer); + } + + for (int i = 0; i < MAX_NUM_SERVOS; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SV%u_SLEW", i); + _param_handles.slew_rate_servos[i] = param_find(buffer); + } + + parameters_updated(); +} + +ControlAllocator::~ControlAllocator() +{ + for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) { + delete _control_allocation[i]; + } + + delete _actuator_effectiveness; + + perf_free(_loop_perf); +} + +bool +ControlAllocator::init() +{ + if (!_vehicle_torque_setpoint_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + if (!_vehicle_thrust_setpoint_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + +#ifndef ENABLE_LOCKSTEP_SCHEDULER // Backup schedule would interfere with lockstep + ScheduleDelayed(50_ms); +#endif + + return true; +} + +void +ControlAllocator::parameters_updated() +{ + _has_slew_rate = false; + + for (int i = 0; i < MAX_NUM_MOTORS; ++i) { + param_get(_param_handles.slew_rate_motors[i], &_params.slew_rate_motors[i]); + _has_slew_rate |= _params.slew_rate_motors[i] > FLT_EPSILON; + } + + for (int i = 0; i < MAX_NUM_SERVOS; ++i) { + param_get(_param_handles.slew_rate_servos[i], &_params.slew_rate_servos[i]); + _has_slew_rate |= _params.slew_rate_servos[i] > FLT_EPSILON; + } + + // Allocation method & effectiveness source + // Do this first: in case a new method is loaded, it will be configured below + bool updated = update_effectiveness_source(); + update_allocation_method(updated); // must be called after update_effectiveness_source() + + if (_num_control_allocation == 0) { + return; + } + + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->updateParameters(); + } + + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE); +} + +void +ControlAllocator::update_allocation_method(bool force) +{ + AllocationMethod configured_method = (AllocationMethod)_param_ca_method.get(); + + if (!_actuator_effectiveness) { + PX4_ERR("_actuator_effectiveness null"); + return; + } + + if (_allocation_method_id != configured_method || force) { + + matrix::Vector actuator_sp[ActuatorEffectiveness::MAX_NUM_MATRICES]; + + // Cleanup first + for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) { + // Save current state + if (_control_allocation[i] != nullptr) { + actuator_sp[i] = _control_allocation[i]->getActuatorSetpoint(); + } + + delete _control_allocation[i]; + _control_allocation[i] = nullptr; + } + + _num_control_allocation = _actuator_effectiveness->numMatrices(); + + AllocationMethod desired_methods[ActuatorEffectiveness::MAX_NUM_MATRICES]; + _actuator_effectiveness->getDesiredAllocationMethod(desired_methods); + + bool normalize_rpy[ActuatorEffectiveness::MAX_NUM_MATRICES]; + _actuator_effectiveness->getNormalizeRPY(normalize_rpy); + + for (int i = 0; i < _num_control_allocation; ++i) { + AllocationMethod method = configured_method; + + if (configured_method == AllocationMethod::AUTO) { + method = desired_methods[i]; + } + + switch (method) { + case AllocationMethod::PSEUDO_INVERSE: + _control_allocation[i] = new ControlAllocationPseudoInverse(); + break; + + case AllocationMethod::SEQUENTIAL_DESATURATION: + _control_allocation[i] = new ControlAllocationSequentialDesaturation(); + break; + + default: + PX4_ERR("Unknown allocation method"); + break; + } + + if (_control_allocation[i] == nullptr) { + PX4_ERR("alloc failed"); + _num_control_allocation = 0; + + } else { + _control_allocation[i]->setNormalizeRPY(normalize_rpy[i]); + _control_allocation[i]->setActuatorSetpoint(actuator_sp[i]); + } + } + + _allocation_method_id = configured_method; + } +} + +bool +ControlAllocator::update_effectiveness_source() +{ + const EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get(); + + if (_effectiveness_source_id != source) { + + // try to instanciate new effectiveness source + ActuatorEffectiveness *tmp = nullptr; + + switch (source) { + case EffectivenessSource::NONE: + case EffectivenessSource::MULTIROTOR: + tmp = new ActuatorEffectivenessMultirotor(this); + break; + + case EffectivenessSource::STANDARD_VTOL: + tmp = new ActuatorEffectivenessStandardVTOL(this); + break; + + case EffectivenessSource::TILTROTOR_VTOL: + tmp = new ActuatorEffectivenessTiltrotorVTOL(this); + break; + + case EffectivenessSource::TAILSITTER_VTOL: + tmp = new ActuatorEffectivenessTailsitterVTOL(this); + break; + + case EffectivenessSource::ROVER_ACKERMANN: + tmp = new ActuatorEffectivenessRoverAckermann(); + break; + + case EffectivenessSource::ROVER_DIFFERENTIAL: + // rover_differential_control does allocation and publishes directly to actuator_motors topic + break; + + case EffectivenessSource::FIXED_WING: + tmp = new ActuatorEffectivenessFixedWing(this); + break; + + case EffectivenessSource::MOTORS_6DOF: // just a different UI from MULTIROTOR + tmp = new ActuatorEffectivenessUUV(this); + break; + + case EffectivenessSource::MULTIROTOR_WITH_TILT: + tmp = new ActuatorEffectivenessMCTilt(this); + break; + + case EffectivenessSource::CUSTOM: + tmp = new ActuatorEffectivenessCustom(this); + break; + + case EffectivenessSource::HELICOPTER_TAIL_ESC: + tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::MOTORS); + break; + + case EffectivenessSource::HELICOPTER_TAIL_SERVO: + tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::SERVOS); + break; + + case EffectivenessSource::HELICOPTER_COAXIAL: + tmp = new ActuatorEffectivenessHelicopterCoaxial(this); + break; + + default: + PX4_ERR("Unknown airframe"); + break; + } + + // Replace previous source with new one + if (tmp == nullptr) { + // It did not work, forget about it + PX4_ERR("Actuator effectiveness init failed"); + _param_ca_airframe.set((int)_effectiveness_source_id); + + } else { + // Swap effectiveness sources + delete _actuator_effectiveness; + _actuator_effectiveness = tmp; + + // Save source id + _effectiveness_source_id = source; + } + + return true; + } + + return false; +} + +void +ControlAllocator::Run() +{ + if (should_exit()) { + _vehicle_torque_setpoint_sub.unregisterCallback(); + _vehicle_thrust_setpoint_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + +#ifndef ENABLE_LOCKSTEP_SCHEDULER // Backup schedule would interfere with lockstep + // Push backup schedule + ScheduleDelayed(50_ms); +#endif + + // Check if parameters have changed + if (_parameter_update_sub.updated() && !_armed) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + if (_handled_motor_failure_bitmask == 0) { + // We don't update the geometry after an actuator failure, as it could lead to unexpected results + // (e.g. a user could add/remove motors, such that the bitmask isn't correct anymore) + updateParams(); + parameters_updated(); + } + } + + if (_num_control_allocation == 0 || _actuator_effectiveness == nullptr) { + return; + } + + { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + + ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT}; + + // Check if the current flight phase is HOVER or FIXED_WING + if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT; + + } else { + flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT; + } + + // Special cases for VTOL in transition + if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) { + if (vehicle_status.in_transition_to_fw) { + flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF; + + } else { + flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF; + } + } + + // Forward to effectiveness source + _actuator_effectiveness->setFlightPhase(flight_phase); + } + } + + { + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + _publish_controls = vehicle_control_mode.flag_control_allocation_enabled; + } + } + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); + + bool do_update = false; + vehicle_torque_setpoint_s vehicle_torque_setpoint; + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + + // Run allocator on torque changes + if (_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)) { + _torque_sp = matrix::Vector3f(vehicle_torque_setpoint.xyz); + + do_update = true; + _timestamp_sample = vehicle_torque_setpoint.timestamp_sample; + + } + + // Also run allocator on thrust setpoint changes if the torque setpoint + // has not been updated for more than 5ms + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) { + _thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz); + + if (dt > 0.005f) { + do_update = true; + _timestamp_sample = vehicle_thrust_setpoint.timestamp_sample; + } + } + + if (do_update) { + _last_run = now; + + check_for_motor_failures(); + + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE); + + // Set control setpoint vector(s) + matrix::Vector c[ActuatorEffectiveness::MAX_NUM_MATRICES]; + c[0](0) = _torque_sp(0); + c[0](1) = _torque_sp(1); + c[0](2) = _torque_sp(2); + c[0](3) = _thrust_sp(0); + c[0](4) = _thrust_sp(1); + c[0](5) = _thrust_sp(2); + + if (_num_control_allocation > 1) { + if (_vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint)) { + c[1](0) = vehicle_torque_setpoint.xyz[0]; + c[1](1) = vehicle_torque_setpoint.xyz[1]; + c[1](2) = vehicle_torque_setpoint.xyz[2]; + } + + if (_vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_setpoint)) { + c[1](3) = vehicle_thrust_setpoint.xyz[0]; + c[1](4) = vehicle_thrust_setpoint.xyz[1]; + c[1](5) = vehicle_thrust_setpoint.xyz[2]; + } + } + + for (int i = 0; i < _num_control_allocation; ++i) { + + _control_allocation[i]->setControlSetpoint(c[i]); + + // Do allocation + _control_allocation[i]->allocate(); + _actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers + _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, + _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); + + if (_has_slew_rate) { + _control_allocation[i]->applySlewRateLimit(dt); + } + + _control_allocation[i]->clipActuatorSetpoint(); + } + } + + // Publish actuator setpoint and allocator status + publish_actuator_controls(); + + // Publish status at limited rate, as it's somewhat expensive and we use it for slower dynamics + // (i.e. anti-integrator windup) + if (now - _last_status_pub >= 5_ms) { + publish_control_allocator_status(0); + + if (_num_control_allocation > 1) { + publish_control_allocator_status(1); + } + + _last_status_pub = now; + } + + perf_end(_loop_perf); +} + +void +ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason) +{ + ActuatorEffectiveness::Configuration config{}; + + if (reason == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE + && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates + return; + } + + if (_actuator_effectiveness->getEffectivenessMatrix(config, reason)) { + _last_effectiveness_update = hrt_absolute_time(); + + memcpy(_control_allocation_selection_indexes, config.matrix_selection_indexes, + sizeof(_control_allocation_selection_indexes)); + + // Get the minimum and maximum depending on type and configuration + ActuatorEffectiveness::ActuatorVector minimum[ActuatorEffectiveness::MAX_NUM_MATRICES]; + ActuatorEffectiveness::ActuatorVector maximum[ActuatorEffectiveness::MAX_NUM_MATRICES]; + ActuatorEffectiveness::ActuatorVector slew_rate[ActuatorEffectiveness::MAX_NUM_MATRICES]; + int actuator_idx = 0; + int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; + + actuator_servos_trim_s trims{}; + static_assert(actuator_servos_trim_s::NUM_CONTROLS == actuator_servos_s::NUM_CONTROLS, "size mismatch"); + + for (int actuator_type = 0; actuator_type < (int)ActuatorType::COUNT; ++actuator_type) { + _num_actuators[actuator_type] = config.num_actuators[actuator_type]; + + for (int actuator_type_idx = 0; actuator_type_idx < config.num_actuators[actuator_type]; ++actuator_type_idx) { + if (actuator_idx >= NUM_ACTUATORS) { + _num_actuators[actuator_type] = 0; + PX4_ERR("Too many actuators"); + break; + } + + int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; + + if ((ActuatorType)actuator_type == ActuatorType::MOTORS) { + if (actuator_type_idx >= MAX_NUM_MOTORS) { + PX4_ERR("Too many motors"); + _num_actuators[actuator_type] = 0; + break; + } + + if (_param_r_rev.get() & (1u << actuator_type_idx)) { + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f; + + } else { + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 0.f; + } + + slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_motors[actuator_type_idx]; + + } else if ((ActuatorType)actuator_type == ActuatorType::SERVOS) { + if (actuator_type_idx >= MAX_NUM_SERVOS) { + PX4_ERR("Too many servos"); + _num_actuators[actuator_type] = 0; + break; + } + + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f; + slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_servos[actuator_type_idx]; + trims.trim[actuator_type_idx] = config.trim[selected_matrix](actuator_idx_matrix[selected_matrix]); + + } else { + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f; + } + + maximum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 1.f; + + ++actuator_idx_matrix[selected_matrix]; + ++actuator_idx; + } + } + + // Handle failed actuators + if (_handled_motor_failure_bitmask) { + actuator_idx = 0; + memset(&actuator_idx_matrix, 0, sizeof(actuator_idx_matrix)); + + for (int motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; + + if (_handled_motor_failure_bitmask & (1 << motors_idx)) { + ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[selected_matrix]; + + for (int i = 0; i < NUM_AXES; i++) { + matrix(i, actuator_idx_matrix[selected_matrix]) = 0.0f; + } + } + + ++actuator_idx_matrix[selected_matrix]; + ++actuator_idx; + } + } + + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->setActuatorMin(minimum[i]); + _control_allocation[i]->setActuatorMax(maximum[i]); + _control_allocation[i]->setSlewRateLimit(slew_rate[i]); + + // Set all the elements of a row to 0 if that row has weak authority. + // That ensures that the algorithm doesn't try to control axes with only marginal control authority, + // which in turn would degrade the control of the main axes that actually should and can be controlled. + + ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[i]; + + for (int n = 0; n < NUM_AXES; n++) { + bool all_entries_small = true; + + for (int m = 0; m < config.num_actuators_matrix[i]; m++) { + if (fabsf(matrix(n, m)) > 0.05f) { + all_entries_small = false; + } + } + + if (all_entries_small) { + matrix.row(n) = 0.f; + } + } + + // Assign control effectiveness matrix + int total_num_actuators = config.num_actuators_matrix[i]; + _control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i], + config.linearization_point[i], total_num_actuators, reason == EffectivenessUpdateReason::CONFIGURATION_UPDATE); + } + + trims.timestamp = hrt_absolute_time(); + _actuator_servos_trim_pub.publish(trims); + } +} + +void +ControlAllocator::publish_control_allocator_status(int matrix_index) +{ + control_allocator_status_s control_allocator_status{}; + control_allocator_status.timestamp = hrt_absolute_time(); + + // TODO: disabled motors (?) + + // Allocated control + const matrix::Vector &allocated_control = _control_allocation[matrix_index]->getAllocatedControl(); + + // Unallocated control + const matrix::Vector unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() - + allocated_control; + control_allocator_status.unallocated_torque[0] = unallocated_control(0); + control_allocator_status.unallocated_torque[1] = unallocated_control(1); + control_allocator_status.unallocated_torque[2] = unallocated_control(2); + control_allocator_status.unallocated_thrust[0] = unallocated_control(3); + control_allocator_status.unallocated_thrust[1] = unallocated_control(4); + control_allocator_status.unallocated_thrust[2] = unallocated_control(5); + + // override control_allocator_status in customized saturation logic for certain effectiveness types + _actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status); + + // Allocation success flags + control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0], + control_allocator_status.unallocated_torque[1], + control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f); + control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0], + control_allocator_status.unallocated_thrust[1], + control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f); + + // Actuator saturation + const matrix::Vector &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint(); + const matrix::Vector &actuator_min = _control_allocation[matrix_index]->getActuatorMin(); + const matrix::Vector &actuator_max = _control_allocation[matrix_index]->getActuatorMax(); + + for (int i = 0; i < NUM_ACTUATORS; i++) { + if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) { + control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_UPPER; + + } else if (actuator_sp(i) < (actuator_min(i) + FLT_EPSILON)) { + control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_LOWER; + } + } + + // Handled motor failures + control_allocator_status.handled_motor_failure_mask = _handled_motor_failure_bitmask; + + _control_allocator_status_pub[matrix_index].publish(control_allocator_status); +} + +void +ControlAllocator::publish_actuator_controls() +{ + if (!_publish_controls) { + return; + } + + actuator_motors_s actuator_motors; + actuator_motors.timestamp = hrt_absolute_time(); + actuator_motors.timestamp_sample = _timestamp_sample; + + actuator_servos_s actuator_servos; + actuator_servos.timestamp = actuator_motors.timestamp; + actuator_servos.timestamp_sample = _timestamp_sample; + + actuator_motors.reversible_flags = _param_r_rev.get(); + + int actuator_idx = 0; + int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; + + uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask; + + // motors + int motors_idx; + + for (motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; + float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); + actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; + + if (stopped_motors & (1u << motors_idx)) { + actuator_motors.control[motors_idx] = NAN; + } + + ++actuator_idx_matrix[selected_matrix]; + ++actuator_idx; + } + + for (int i = motors_idx; i < actuator_motors_s::NUM_CONTROLS; i++) { + actuator_motors.control[i] = NAN; + } + + _actuator_motors_pub.publish(actuator_motors); + + // servos + if (_num_actuators[1] > 0) { + int servos_idx; + + for (servos_idx = 0; servos_idx < _num_actuators[1] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { + int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; + float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); + actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; + ++actuator_idx_matrix[selected_matrix]; + ++actuator_idx; + } + + for (int i = servos_idx; i < actuator_servos_s::NUM_CONTROLS; i++) { + actuator_servos.control[i] = NAN; + } + + _actuator_servos_pub.publish(actuator_servos); + } +} + +void +ControlAllocator::check_for_motor_failures() +{ + failure_detector_status_s failure_detector_status; + + if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE + && _failure_detector_status_sub.update(&failure_detector_status)) { + if (failure_detector_status.fd_motor) { + + if (_handled_motor_failure_bitmask != failure_detector_status.motor_failure_mask) { + // motor failure bitmask changed + switch ((FailureMode)_param_ca_failure_mode.get()) { + case FailureMode::REMOVE_FIRST_FAILING_MOTOR: { + // Count number of failed motors + const int num_motors_failed = math::countSetBits(failure_detector_status.motor_failure_mask); + + // Only handle if it is the first failure + if (_handled_motor_failure_bitmask == 0 && num_motors_failed == 1) { + _handled_motor_failure_bitmask = failure_detector_status.motor_failure_mask; + PX4_WARN("Removing motor from allocation (0x%x)", _handled_motor_failure_bitmask); + + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->setHadActuatorFailure(true); + } + + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE); + } + } + break; + + default: + break; + } + + } + + } else if (_handled_motor_failure_bitmask != 0) { + // Clear bitmask completely + PX4_INFO("Restoring all motors"); + _handled_motor_failure_bitmask = 0; + + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->setHadActuatorFailure(false); + } + + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE); + } + } +} + +int ControlAllocator::task_spawn(int argc, char *argv[]) +{ + ControlAllocator *instance = new ControlAllocator(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int ControlAllocator::print_status() +{ + PX4_INFO("Running"); + + // Print current allocation method + switch (_allocation_method_id) { + case AllocationMethod::NONE: + PX4_INFO("Method: None"); + break; + + case AllocationMethod::PSEUDO_INVERSE: + PX4_INFO("Method: Pseudo-inverse"); + break; + + case AllocationMethod::SEQUENTIAL_DESATURATION: + PX4_INFO("Method: Sequential desaturation"); + break; + + case AllocationMethod::AUTO: + PX4_INFO("Method: Auto"); + break; + } + + // Print current airframe + if (_actuator_effectiveness != nullptr) { + PX4_INFO("Effectiveness Source: %s", _actuator_effectiveness->name()); + } + + // Print current effectiveness matrix + for (int i = 0; i < _num_control_allocation; ++i) { + const ActuatorEffectiveness::EffectivenessMatrix &effectiveness = _control_allocation[i]->getEffectivenessMatrix(); + + if (_num_control_allocation > 1) { + PX4_INFO("Instance: %i", i); + } + + PX4_INFO(" Effectiveness.T ="); + effectiveness.T().print(); + PX4_INFO(" minimum ="); + _control_allocation[i]->getActuatorMin().T().print(); + PX4_INFO(" maximum ="); + _control_allocation[i]->getActuatorMax().T().print(); + PX4_INFO(" Configured actuators: %i", _control_allocation[i]->numConfiguredActuators()); + } + + if (_handled_motor_failure_bitmask) { + PX4_INFO("Failed motors: %i (0x%x)", math::countSetBits(_handled_motor_failure_bitmask), + _handled_motor_failure_bitmask); + } + + // Print perf + perf_print_counter(_loop_perf); + + return 0; +} + +int ControlAllocator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int ControlAllocator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements control allocation. It takes torque and thrust setpoints +as inputs and outputs actuator setpoint messages. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("control_allocator", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +/** + * Control Allocator app start / stop handling function + */ +extern "C" __EXPORT int metric_allocator_main(int argc, char *argv[]); + +int metric_allocator_main(int argc, char *argv[]) +{ + return ControlAllocator::main(argc, argv); +} diff --git a/src/modules/metric_allocator/ControlAllocator.hpp b/src/modules/metric_allocator/ControlAllocator.hpp new file mode 100644 index 0000000000..303316f1ef --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocator.hpp @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocator.hpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ControlAllocator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + static constexpr int NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; + static constexpr int NUM_AXES = ControlAllocation::NUM_AXES; + + static constexpr int MAX_NUM_MOTORS = actuator_motors_s::NUM_CONTROLS; + static constexpr int MAX_NUM_SERVOS = actuator_servos_s::NUM_CONTROLS; + + using ActuatorVector = ActuatorEffectiveness::ActuatorVector; + + ControlAllocator(); + + virtual ~ControlAllocator(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + bool init(); + +private: + + struct ParamHandles { + param_t slew_rate_motors[MAX_NUM_MOTORS]; + param_t slew_rate_servos[MAX_NUM_SERVOS]; + }; + + struct Params { + float slew_rate_motors[MAX_NUM_MOTORS]; + float slew_rate_servos[MAX_NUM_SERVOS]; + }; + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + void update_allocation_method(bool force); + bool update_effectiveness_source(); + + void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason); + + void check_for_motor_failures(); + + void publish_control_allocator_status(int matrix_index); + + void publish_actuator_controls(); + + AllocationMethod _allocation_method_id{AllocationMethod::NONE}; + ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; ///< class for control allocation calculations + int _num_control_allocation{0}; + hrt_abstime _last_effectiveness_update{0}; + + enum class EffectivenessSource { + NONE = -1, + MULTIROTOR = 0, + FIXED_WING = 1, + STANDARD_VTOL = 2, + TILTROTOR_VTOL = 3, + TAILSITTER_VTOL = 4, + ROVER_ACKERMANN = 5, + ROVER_DIFFERENTIAL = 6, + MOTORS_6DOF = 7, + MULTIROTOR_WITH_TILT = 8, + CUSTOM = 9, + HELICOPTER_TAIL_ESC = 10, + HELICOPTER_TAIL_SERVO = 11, + HELICOPTER_COAXIAL = 12, + }; + + enum class FailureMode { + IGNORE = 0, + REMOVE_FIRST_FAILING_MOTOR = 1, + }; + + EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; + ActuatorEffectiveness *_actuator_effectiveness{nullptr}; ///< class providing actuator effectiveness + + uint8_t _control_allocation_selection_indexes[NUM_ACTUATORS * ActuatorEffectiveness::MAX_NUM_MATRICES] {}; + int _num_actuators[(int)ActuatorType::COUNT] {}; + + // Inputs + uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)}; /**< vehicle torque setpoint subscription */ + uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle thrust setpoint subscription */ + + uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1}; /**< vehicle torque setpoint subscription (2. instance) */ + uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; /**< vehicle thrust setpoint subscription (2. instance) */ + + // Outputs + uORB::PublicationMulti _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)}; + + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + uORB::Publication _actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; + + matrix::Vector3f _torque_sp; + matrix::Vector3f _thrust_sp; + bool _publish_controls{true}; + + // Reflects motor failures that are currently handled, not motor failures that are reported. + // For example, the system might report two motor failures, but only the first one is handled by CA + uint16_t _handled_motor_failure_bitmask{0}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + bool _armed{false}; + hrt_abstime _last_run{0}; + hrt_abstime _timestamp_sample{0}; + hrt_abstime _last_status_pub{0}; + + ParamHandles _param_handles{}; + Params _params{}; + bool _has_slew_rate{false}; + + DEFINE_PARAMETERS( + (ParamInt) _param_ca_airframe, + (ParamInt) _param_ca_method, + (ParamInt) _param_ca_failure_mode, + (ParamInt) _param_r_rev + ) + +}; diff --git a/src/modules/metric_allocator/Kconfig b/src/modules/metric_allocator/Kconfig new file mode 100644 index 0000000000..ea4449854d --- /dev/null +++ b/src/modules/metric_allocator/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_METRIC_ALLOCATOR + bool "metric_allocator" + default n + ---help--- + Enable support for metric_allocator + +menuconfig USER_METRIC_ALLOCATOR + bool "metric_allocator running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_METRIC_ALLOCATOR + ---help--- + Put metric_allocator in userspace memory diff --git a/src/modules/metric_allocator/module.yaml b/src/modules/metric_allocator/module.yaml new file mode 100644 index 0000000000..8cc5e2fab9 --- /dev/null +++ b/src/modules/metric_allocator/module.yaml @@ -0,0 +1,1157 @@ +__max_num_mc_motors: &max_num_mc_motors 12 +__max_num_servos: &max_num_servos 8 +__max_num_tilts: &max_num_tilts 4 + +module_name: Metric Allocation + +parameters: + - group: Geometry + definitions: + CA_AIRFRAME: + description: + short: Airframe selection + long: | + Defines which mixer implementation to use. + Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators. + + 'Custom' should only be used if noting else can be used. + type: enum + values: + 0: Multirotor + 1: Fixed-wing + 2: Standard VTOL + 3: Tiltrotor VTOL + 4: Tailsitter VTOL + 5: Rover (Ackermann) + 6: Rover (Differential) + 7: Motors (6DOF) + 8: Multirotor with Tilt + 9: Custom + 10: Helicopter (tail ESC) + 11: Helicopter (tail Servo) + 12: Helicopter (Coaxial) + 13: Rover (Mecanum) + default: 0 + + CA_METHOD: + description: + short: Control allocation method + long: | + Selects the algorithm and desaturation method. + If set to Automtic, the selection is based on the airframe (CA_AIRFRAME). + type: enum + values: + 0: Pseudo-inverse with output clipping + 1: Pseudo-inverse with sequential desaturation technique + 2: Automatic + default: 2 + + # Motor parameters + CA_R_REV: + description: + short: Bidirectional/Reversible motors + long: | + Configure motors to be bidirectional/reversible. Note that the output driver needs to support this as well. + type: bitmask + bit: + 0: Motor 1 + 1: Motor 2 + 2: Motor 3 + 3: Motor 4 + 4: Motor 5 + 5: Motor 6 + 6: Motor 7 + 7: Motor 8 + 8: Motor 9 + 9: Motor 10 + 10: Motor 11 + 11: Motor 12 + default: 0 + CA_R${i}_SLEW: + description: + short: Motor ${i} slew rate limit + long: | + Minimum time allowed for the motor input signal to pass through + the full output range. A value x means that the motor signal + can only go from 0 to 1 in minimum x seconds (in case of + reversible motors, the range is -1 to 1). + + Zero means that slew rate limiting is disabled. + type: float + decimal: 2 + increment: 0.01 + num_instances: *max_num_mc_motors + min: 0 + max: 10 + default: 0.0 + + # Servo params + CA_SV${i}_SLEW: + description: + short: Servo ${i} slew rate limit + long: | + Minimum time allowed for the servo input signal to pass through + the full output range. A value x means that the servo signal + can only go from -1 to 1 in minimum x seconds. + + Zero means that slew rate limiting is disabled. + type: float + decimal: 2 + increment: 0.05 + num_instances: *max_num_servos + min: 0 + max: 10 + default: 0.0 + + + # (MC) Rotors + CA_ROTOR_COUNT: + description: + short: Total number of rotors + type: enum + values: + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + 5: '5' + 6: '6' + 7: '7' + 8: '8' + 9: '9' + 10: '10' + 11: '11' + 12: '12' + default: 0 + CA_ROTOR${i}_PX: + description: + short: Position of rotor ${i} along X body axis relative to center of gravity + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: 0.0 + CA_ROTOR${i}_PY: + description: + short: Position of rotor ${i} along Y body axis relative to center of gravity + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: 0.0 + CA_ROTOR${i}_PZ: + description: + short: Position of rotor ${i} along Z body axis relative to center of gravity + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: 0.0 + + CA_ROTOR${i}_AX: + description: + short: Axis of rotor ${i} thrust vector, X body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: 0.0 + CA_ROTOR${i}_AY: + description: + short: Axis of rotor ${i} thrust vector, Y body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: 0.0 + CA_ROTOR${i}_AZ: + description: + short: Axis of rotor ${i} thrust vector, Z body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: -1.0 + + CA_ROTOR${i}_CT: + description: + short: Thrust coefficient of rotor ${i} + long: | + The thrust coefficient if defined as Thrust = CT * u^2, + where u (with value between actuator minimum and maximum) + is the output signal sent to the motor controller. + type: float + decimal: 1 + increment: 1 + num_instances: *max_num_mc_motors + min: 0 + max: 100 + default: 6.5 + + CA_ROTOR${i}_KM: + description: + short: Moment coefficient of rotor ${i} + long: | + The moment coefficient if defined as Torque = KM * Thrust. + + Use a positive value for a rotor with CCW rotation. + Use a negative value for a rotor with CW rotation. + type: float + decimal: 3 + increment: 0.01 + num_instances: *max_num_mc_motors + min: -1 + max: 1 + default: 0.05 + CA_ROTOR${i}_TILT: + description: + short: Rotor ${i} tilt assignment + long: If not set to None, this motor is tilted by the configured tilt servo. + type: enum + values: + 0: 'None' + 1: 'Tilt 1' + 2: 'Tilt 2' + 3: 'Tilt 3' + 4: 'Tilt 4' + num_instances: *max_num_mc_motors + instance_start: 0 + default: 0 + + # control surfaces + CA_SV_CS_COUNT: + description: + short: Total number of Control Surfaces + type: enum + values: + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + 5: '5' + 6: '6' + 7: '7' + 8: '8' + default: 0 + CA_SV_CS${i}_TYPE: + description: + short: Control Surface ${i} type + type: enum + values: + 0: (Not set) + 1: Left Aileron + 2: Right Aileron + 3: Elevator + 4: Rudder + 5: Left Elevon + 6: Right Elevon + 7: Left V-Tail + 8: Right V-Tail + 9: Left Flap + 10: Right Flap + 11: Airbrake + 12: Custom + 13: Left A-tail + 14: Right A-tail + 15: Single Channel Aileron + 16: Steering Wheel + 17: Left Spoiler + 18: Right Spoiler + num_instances: *max_num_servos + instance_start: 0 + default: 0 + + CA_SV_CS${i}_TRQ_R: + description: + short: Control Surface ${i} roll torque scaling + type: float + decimal: 2 + num_instances: *max_num_servos + instance_start: 0 + default: 0.0 + + CA_SV_CS${i}_TRQ_P: + description: + short: Control Surface ${i} pitch torque scaling + type: float + decimal: 2 + num_instances: *max_num_servos + instance_start: 0 + default: 0.0 + + CA_SV_CS${i}_TRQ_Y: + description: + short: Control Surface ${i} yaw torque scaling + type: float + decimal: 2 + num_instances: *max_num_servos + instance_start: 0 + default: 0.0 + + CA_SV_CS${i}_TRIM: + description: + short: Control Surface ${i} trim + long: Can be used to add an offset to the servo control. + type: float + decimal: 2 + min: -1.0 + max: 1.0 + num_instances: *max_num_servos + instance_start: 0 + default: 0.0 + + CA_SV_CS${i}_FLAP: + description: + short: Control Surface ${i} configuration as flap + type: float + decimal: 2 + min: -1.0 + max: 1.0 + num_instances: *max_num_servos + instance_start: 0 + default: 0 + + CA_SV_CS${i}_SPOIL: + description: + short: Control Surface ${i} configuration as spoiler + type: float + decimal: 2 + min: -1.0 + max: 1.0 + num_instances: *max_num_servos + instance_start: 0 + default: 0 + + # Tilts + CA_SV_TL_COUNT: + description: + short: Total number of Tilt Servos + type: enum + values: + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + default: 0 + CA_SV_TL${i}_CT: + description: + short: Tilt ${i} is used for control + long: Define if this servo is used for additional control. + type: enum # This could be a bitset, but the enum shows up nicer in the UI + values: + 0: 'None' + 1: 'Yaw' + 2: 'Pitch' + 3: 'Yaw and Pitch' + num_instances: *max_num_tilts + instance_start: 0 + default: 1 + CA_SV_TL${i}_MINA: + description: + short: Tilt Servo ${i} Tilt Angle at Minimum + long: | + Defines the tilt angle when the servo is at the minimum. + An angle of zero means upwards. + type: float + decimal: 0 + unit: 'deg' + num_instances: *max_num_tilts + instance_start: 0 + min: -90.0 + max: 90.0 + default: 0.0 + CA_SV_TL${i}_MAXA: + description: + short: Tilt Servo ${i} Tilt Angle at Maximum + long: | + Defines the tilt angle when the servo is at the maximum. + An angle of zero means upwards. + type: float + decimal: 0 + unit: 'deg' + num_instances: *max_num_tilts + instance_start: 0 + min: -90.0 + max: 90.0 + default: 90.0 + CA_SV_TL${i}_TD: + description: + short: Tilt Servo ${i} Tilt Direction + long: | + Defines the direction the servo tilts towards when moving towards the maximum tilt angle. + For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front', + the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt. + type: enum + values: + 0: 'Towards Front' + 90: 'Towards Right' + min: 0 + max: 359 + num_instances: *max_num_tilts + instance_start: 0 + default: 0 + + # helicopter + CA_SP0_COUNT: + description: + short: Number of swash plates servos + type: enum + values: + 2: '2' + 3: '3' + 4: '4' + default: 3 + CA_SP0_ANG${i}: + description: + short: Angle for swash plate servo ${i} + long: | + The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis). + type: float + decimal: 0 + increment: 10 + unit: deg + num_instances: 4 + min: 0 + max: 360 + default: [0, 140, 220, 0] + CA_SP0_ARM_L${i}: + description: + short: Arm length for swash plate servo ${i} + long: | + This is relative to the other arm lengths. + type: float + decimal: 3 + increment: 0.1 + num_instances: 4 + min: 0 + max: 10 + default: 1.0 + CA_HELI_THR_C${i}: + description: + short: Throttle curve at position ${i} + long: | + Defines the output throttle at the interval position ${i}. + type: float + decimal: 3 + increment: 0.1 + num_instances: 5 + min: 0 + max: 1 + default: [1, 1, 1, 1, 1] + CA_HELI_PITCH_C${i}: + description: + short: Collective pitch curve at position ${i} + long: | + Defines the collective pitch at the interval position ${i} for a given thrust setpoint. + + Use negative values if the swash plate needs to move down to provide upwards thrust. + type: float + decimal: 3 + increment: 0.1 + num_instances: 5 + min: -1 + max: 1 + default: [-0.05, 0.0725, 0.2, 0.325, 0.45] + CA_HELI_YAW_CP_S: + description: + short: Scale for yaw compensation based on collective pitch + long: | + This allows to add a proportional factor of the collective pitch command to the yaw command. + A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction. + + tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O) + type: float + decimal: 3 + increment: 0.1 + min: -2 + max: 2 + default: 0.0 + CA_HELI_YAW_CP_O: + description: + short: Offset for yaw compensation based on collective pitch + long: | + This allows to specify which collective pitch command results in the least amount of rotor drag. + This is used to increase the accuracy of the yaw drag torque compensation based on collective pitch + by aligning the lowest rotor drag with zero compensation. + For symmetric profile blades this is the command that results in exactly 0° collective blade angle. + For lift profile blades this is typically a command resulting in slightly negative collective blade angle. + + tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O) + type: float + decimal: 3 + increment: 0.1 + min: -2 + max: 2 + default: 0.0 + CA_HELI_YAW_TH_S: + description: + short: Scale for yaw compensation based on throttle + long: | + This allows to add a proportional factor of the throttle command to the yaw command. + A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction. + + tail_output += CA_HELI_YAW_TH_S * throttle + type: float + decimal: 3 + increment: 0.1 + min: -2 + max: 2 + default: 0.0 + CA_HELI_YAW_CCW: + description: + short: Main rotor turns counter-clockwise + long: | + Default configuration is for a clockwise turning main rotor and + positive thrust of the tail rotor is expected to rotate the vehicle clockwise. + Set this parameter to true if the tail rotor provides thrust in counter-clockwise direction + which is mostly the case when the main rotor turns counter-clockwise. + type: boolean + default: 0 + + # Others + CA_FAILURE_MODE: + description: + short: Motor failure handling mode + long: | + This is used to specify how to handle motor failures + reported by failure detector. + type: enum + values: + 0: Ignore + 1: Remove first failed motor from effectiveness + default: 0 + +# Mixer +mixer: + actuator_types: + motor: + functions: 'Motor' + actuator_testing_values: + min: 0 + max: 1 + default_is_nan: true + per_item_parameters: + - name: 'CA_R_REV' + label: 'Bidirectional' + show_as: 'bitset' + index_offset: 0 + advanced: true + - name: 'CA_R${i}_SLEW' + label: 'Slew Rate' + index_offset: 0 + advanced: true + servo: + functions: 'Servo' + actuator_testing_values: + min: -1 + max: 1 + default: 0 + per_item_parameters: + - name: 'CA_SV${i}_SLEW' + label: 'Slew Rate' + index_offset: 0 + advanced: true + DEFAULT: + actuator_testing_values: + min: -1 + max: 1 + default: -1 + + rules: + - select_identifier: 'servo-type' # restrict torque based on servo type + apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw', 'servo-scale-flap', 'servo-scale-spoiler'] + items: + # Convention: horizontal surfaces: up=positive, vertical: right=positive, mixed: up=positive. + # By default the scale is set to 1/N, where N is the amount of actuators with an effect on + # the corresponding axis. With that we are using all the available servo moving range. + 0: # Not set + - { 'disabled': True, 'default': 0.0 } # roll + - { 'disabled': True, 'default': 0.0 } # pitch + - { 'disabled': True, 'default': 0.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 1: # Left Aileron + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 2: # Right Aileron + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 3: # Elevator + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 4: # Rudder + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 5: # Left Elevon + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 6: # Right Elevon + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 7: # Left V Tail + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 8: # Right V Tail + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 9: # Left Flap + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 10: # Right Flap + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 11: # Airbrake + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 12: # Custom + - { 'hidden': False, 'default': 0.0 } # roll + - { 'hidden': False, 'default': 0.0 } # pitch + - { 'hidden': False, 'default': 0.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 13: # Left A Tail + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 14: # Right A Tail + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 15: # Single Channel Aileron + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 16: # Steering Wheel + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 17: # Left Spoiler + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # spoiler + 18: # Right Spoiler + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # spoiler + + + - select_identifier: 'servo-type-tailsitter' # restrict torque based on servo type for tailsitters + apply_identifiers: ['servo-torque-roll-tailsitter', 'servo-torque-pitch-tailsitter', 'servo-torque-yaw-tailsitter'] + items: + # Convention: horizontal surfaces: up=positive, vertical: right=positive, mixed: up=positive. + # No rules for control surfaces on tailsitter beside elevons. + 5: # Left Elevon (Tailsitter --> wing vertical, so affects pitch and yaw) + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw + 6: # Right Elevon (Tailsitter -->wing vertical, so affects pitch and yaw) + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw + + config: + param: CA_AIRFRAME + types: + 0: # Multirotor + type: 'multirotor' + title: 'Multirotor' + actuators: + - actuator_type: 'motor' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_KM' + label: 'Direction CCW' + function: 'spin-dir' + show_as: 'true-if-positive' + - name: 'CA_ROTOR${i}_AX' + label: 'Axis X' + function: 'axisx' + advanced: true + - name: 'CA_ROTOR${i}_AY' + label: 'Axis Y' + function: 'axisy' + advanced: true + - name: 'CA_ROTOR${i}_AZ' + label: 'Axis Z' + function: 'axisz' + advanced: true + + 1: # Fixed Wing + title: 'Fixed Wing' + actuators: + - actuator_type: 'motor' + group_label: 'Motors' + count: 'CA_ROTOR_COUNT' + - actuator_type: 'servo' + group_label: 'Control Surfaces' + count: 'CA_SV_CS_COUNT' + per_item_parameters: + extra: + - name: 'CA_SV_CS${i}_TYPE' + label: 'Type' + function: 'type' + identifier: 'servo-type' + - name: 'CA_SV_CS${i}_TRQ_R' + label: 'Roll Torque' + identifier: 'servo-torque-roll' + - name: 'CA_SV_CS${i}_TRQ_P' + label: 'Pitch Torque' + identifier: 'servo-torque-pitch' + - name: 'CA_SV_CS${i}_TRQ_Y' + label: 'Yaw Torque' + identifier: 'servo-torque-yaw' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + - name: 'CA_SV_CS${i}_FLAP' + label: 'Flaps Scale' + advanced: true + identifier: 'servo-scale-flap' + - name: 'CA_SV_CS${i}_SPOIL' + label: 'Spoiler Scale' + advanced: true + identifier: 'servo-scale-spoiler' + + 2: # Standard VTOL + title: 'Standard VTOL' + actuators: + - actuator_type: 'motor' + group_label: 'Motors' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_KM' + label: 'Direction CCW' + function: 'spin-dir' + show_as: 'true-if-positive' + - name: 'CA_ROTOR${i}_AX' + label: 'Axis X' + function: 'axisx' + - name: 'CA_ROTOR${i}_AY' + label: 'Axis Y' + function: 'axisy' + - name: 'CA_ROTOR${i}_AZ' + label: 'Axis Z' + function: 'axisz' + - actuator_type: 'servo' + group_label: 'Control Surfaces' + count: 'CA_SV_CS_COUNT' + per_item_parameters: + extra: + - name: 'CA_SV_CS${i}_TYPE' + label: 'Type' + function: 'type' + identifier: 'servo-type' + - name: 'CA_SV_CS${i}_TRQ_R' + label: 'Roll Scale' + identifier: 'servo-torque-roll' + - name: 'CA_SV_CS${i}_TRQ_P' + label: 'Pitch Scale' + identifier: 'servo-torque-pitch' + - name: 'CA_SV_CS${i}_TRQ_Y' + label: 'Yaw Scale' + identifier: 'servo-torque-yaw' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + - name: 'CA_SV_CS${i}_FLAP' + label: 'Flaps Scale' + advanced: true + identifier: 'servo-scale-flap' + - name: 'CA_SV_CS${i}_SPOIL' + label: 'Spoiler Scale' + advanced: true + identifier: 'servo-scale-spoiler' + parameters: + - label: 'Lock control surfaces in hover' + name: VT_ELEV_MC_LOCK + + 3: # Tiltrotor VTOL + title: 'Tiltrotor VTOL' + actuators: + - actuator_type: 'motor' + group_label: 'MC Motors' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_TILT' + label: 'Tilted by' + - name: 'CA_ROTOR${i}_KM' + label: 'Direction CCW' + function: 'spin-dir' + show_as: 'true-if-positive' + - actuator_type: 'servo' + group_label: 'Control Surfaces' + count: 'CA_SV_CS_COUNT' + item_label_prefix: 'Surface ${i}' + per_item_parameters: + extra: + - name: 'CA_SV_CS${i}_TYPE' + label: 'Type' + function: 'type' + identifier: 'servo-type' + - name: 'CA_SV_CS${i}_TRQ_R' + label: 'Roll Scale' + identifier: 'servo-torque-roll' + - name: 'CA_SV_CS${i}_TRQ_P' + label: 'Pitch Scale' + identifier: 'servo-torque-pitch' + - name: 'CA_SV_CS${i}_TRQ_Y' + label: 'Yaw Scale' + identifier: 'servo-torque-yaw' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + - name: 'CA_SV_CS${i}_FLAP' + label: 'Flaps Scale' + advanced: true + identifier: 'servo-scale-flap' + - name: 'CA_SV_CS${i}_SPOIL' + label: 'Spoiler Scale' + advanced: true + identifier: 'servo-scale-spoiler' + parameters: + - label: 'Lock control surfaces in hover' + name: VT_ELEV_MC_LOCK + - actuator_type: 'servo' + group_label: 'Tilt Servos' + count: 'CA_SV_TL_COUNT' + item_label_prefix: 'Tilt ${i}' + per_item_parameters: + extra: + - name: 'CA_SV_TL${i}_MINA' + label: "Angle at Min\nTilt" + - name: 'CA_SV_TL${i}_MAXA' + label: "Angle at Max\nTilt" + - name: 'CA_SV_TL${i}_TD' + label: 'Tilt Direction' + - name: 'CA_SV_TL${i}_CT' + label: 'Use for Control' + + 4: # Tailsitter VTOL + title: 'Tailsitter VTOL' + actuators: + - actuator_type: 'motor' + group_label: 'MC Motors' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_KM' + label: 'Direction CCW' + function: 'spin-dir' + show_as: 'true-if-positive' + - actuator_type: 'servo' + group_label: 'Control Surfaces' + count: 'CA_SV_CS_COUNT' + item_label_prefix: 'Surface ${i}' + per_item_parameters: + extra: + - name: 'CA_SV_CS${i}_TYPE' + label: 'Type' + function: 'type' + identifier: 'servo-type-tailsitter' + - name: 'CA_SV_CS${i}_TRQ_R' + label: 'Roll Scale' + identifier: 'servo-torque-roll-tailsitter' + - name: 'CA_SV_CS${i}_TRQ_P' + label: 'Pitch Scale' + identifier: 'servo-torque-pitch-tailsitter' + - name: 'CA_SV_CS${i}_TRQ_Y' + label: 'Yaw Scale' + identifier: 'servo-torque-yaw-tailsitter' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + - name: 'CA_SV_CS${i}_FLAP' + label: 'Flaps Scale' + advanced: true + identifier: 'servo-scale-flap' + - name: 'CA_SV_CS${i}_SPOIL' + label: 'Spoiler Scale' + advanced: true + identifier: 'servo-scale-spoiler' + parameters: + - label: 'Lock control surfaces in hover' + name: VT_ELEV_MC_LOCK + + 5: # Rover (Ackermann) + title: 'Rover (Ackermann)' + actuators: + - actuator_type: 'motor' + instances: + - name: 'Throttle' + position: [ -1, 0, 0 ] + - actuator_type: 'servo' + instances: + - name: 'Steering' + position: [ 1, 0, 0 ] + + 6: # Rover (Differential) + title: 'Rover (Differential)' + actuators: + - actuator_type: 'motor' + instances: + - name: 'Right Motor' + position: [ 0, 1, 0 ] + - name: 'Left Motor' + position: [ 0, -1, 0 ] + + 7: # Motors (6DOF) + actuators: + - actuator_type: 'motor' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_AX' + label: 'Axis X' + function: 'axisx' + - name: 'CA_ROTOR${i}_AY' + label: 'Axis Y' + function: 'axisy' + - name: 'CA_ROTOR${i}_AZ' + label: 'Axis Z' + function: 'axisz' + - name: 'CA_ROTOR${i}_KM' + label: "Moment\nCoefficient" + + 8: # Multirotor with Tilt + title: 'Multirotor with Tilt' + actuators: + - actuator_type: 'motor' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_KM' + label: 'Direction CCW' + function: 'spin-dir' + show_as: 'true-if-positive' + - name: 'CA_ROTOR${i}_TILT' + label: 'Tilted by' + - actuator_type: 'servo' + group_label: 'Tilt Servos' + count: 'CA_SV_TL_COUNT' + item_label_prefix: 'Tilt ${i}' + per_item_parameters: + extra: + - name: 'CA_SV_TL${i}_MINA' + label: "Angle at Min\nTilt" + - name: 'CA_SV_TL${i}_MAXA' + label: "Angle at Max\nTilt" + - name: 'CA_SV_TL${i}_TD' + label: 'Tilt Direction' + - name: 'CA_SV_TL${i}_CT' + label: 'Use for Control' + + 9: # Custom + actuators: + - actuator_type: 'motor' + group_label: 'Thrust/Motors' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_AX' + label: 'Axis X' + function: 'axisx' + - name: 'CA_ROTOR${i}_AY' + label: 'Axis Y' + function: 'axisy' + - name: 'CA_ROTOR${i}_AZ' + label: 'Axis Z' + function: 'axisz' + - name: 'CA_ROTOR${i}_CT' + label: "Thrust\nCoefficient" + - name: 'CA_ROTOR${i}_KM' + label: "Moment\nCoefficient" + - actuator_type: 'servo' + group_label: 'Torque' + count: 'CA_SV_CS_COUNT' + item_label_prefix: 'Torque ${i}' + per_item_parameters: + extra: + - name: 'CA_SV_CS${i}_TRQ_R' + label: 'Roll Scale' + - name: 'CA_SV_CS${i}_TRQ_P' + label: 'Pitch Scale' + - name: 'CA_SV_CS${i}_TRQ_Y' + label: 'Yaw Scale' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + - name: 'CA_SV_CS${i}_FLAP' + label: 'Flaps Scale' + advanced: true + identifier: 'servo-scale-flap' + - name: 'CA_SV_CS${i}_SPOIL' + label: 'Spoiler Scale' + advanced: true + identifier: 'servo-scale-spoiler' + + 10: # Helicopter (tail ESC) + actuators: + - actuator_type: 'motor' + count: 1 + item_label_prefix: ['Rotor'] + - actuator_type: 'motor' + item_label_prefix: ['Yaw tail Motor'] + count: 1 + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + parameters: + - label: 'Collective pitch offset to align least amount of rotor drag' + name: CA_HELI_YAW_CP_O + - label: 'Yaw compensation scale based on collective pitch' + name: CA_HELI_YAW_CP_S + - label: 'Yaw compensation scale based on throttle' + name: CA_HELI_YAW_TH_S + - label: 'Main rotor turns counter-clockwise' + name: CA_HELI_YAW_CCW + - label: 'Throttle spoolup time' + name: COM_SPOOLUP_TIME + + 11: # Helicopter (tail Servo) + actuators: + - actuator_type: 'motor' + count: 1 + item_label_prefix: ['Rotor'] + - actuator_type: 'servo' + item_label_prefix: ['Yaw tail Servo'] + count: 1 + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + parameters: + - label: 'Yaw compensation scale based on collective pitch' + name: CA_HELI_YAW_CP_S + - label: 'Yaw compensation scale based on throttle' + name: CA_HELI_YAW_TH_S + - label: 'Main rotor turns counter-clockwise' + name: CA_HELI_YAW_CCW + - label: 'Throttle spoolup time' + name: COM_SPOOLUP_TIME + + 12: # Helicopter (Coaxial) + actuators: + - actuator_type: 'motor' + count: 2 + item_label_prefix: ['Clockwise Rotor', 'Counter-clockwise Rotor'] + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + parameters: + - label: 'Throttle spoolup time' + name: COM_SPOOLUP_TIME + + 13: # Rover (Mecanum) + title: 'Rover (Mecanum)' + actuators: + - actuator_type: 'motor' + instances: + - name: 'Right Motor Front' + position: [ 1, 1, 0 ] + - name: 'Left Motor Front' + position: [ 1, -1, 0 ] + - name: 'Right Motor Back' + position: [ -1, 1, 0 ] + - name: 'Left Motor Back' + position: [ -1, -1, 0 ]