Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-10-06 09:02:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete lib/mixer and mixer_module static mixing

Browse Source
This commit is contained in:
Daniel Agar
2022-09-09 09:14:09 -04:00
parent 0019ffbea6
commit a7bbcd5b04
79 changed files with 219 additions and 6835 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
-1
View File
@@ -93,7 +93,6 @@ rules.ninja
/googletest-*/
/logs
/mavsdk_tests
/test_mixer_multirotor
/unit-*
/uORB/
DartConfiguration.tcl
CMakeLists.txt
-1
View File
@@ -414,7 +414,6 @@ if(BUILD_TESTING)
DEPENDS
px4
examples__dyn_hello
test_mixer_multirotor
USES_TERMINAL
COMMENT "Running tests"
WORKING_DIRECTORY ${PX4_BINARY_DIR})
src/drivers/actuators/modalai_esc/modalai_esc.cpp
+1 -25
View File
@@ -38,7 +38,6 @@
#include "modalai_esc.hpp"
#include "modalai_esc_serial.hpp"
#define MODALAI_ESC_DEVICE_PATH "/dev/uart_esc"
#define MODALAI_ESC_DEFAULT_PORT "/dev/ttyS1"
#define MODALAI_ESC_VOXL_PORT "/dev/ttyS4"
@@ -48,7 +47,6 @@
const char *_device;
ModalaiEsc::ModalaiEsc() :
CDev(MODALAI_ESC_DEVICE_PATH),
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval"))
@@ -96,35 +94,15 @@ ModalaiEsc::~ModalaiEsc()
_uart_port_bridge = nullptr;
}
/* clean up the alternate device node */
unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
perf_free(_cycle_perf);
perf_free(_output_update_perf);
}
int ModalaiEsc::init()
{
/* do regular cdev init */
int ret = CDev::init();
if (ret != OK) {
return ret;
}
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
_class_instance = register_class_devname(MODALAI_ESC_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* lets not be too verbose */
} else if (_class_instance < 0) {
PX4_ERR("FAILED registering class device");
}
_mixing_output.setDriverInstance(_class_instance);
/* Getting initial parameter values */
ret = update_params();
int ret = update_params();
if (ret != OK) {
return ret;
@@ -1135,8 +1113,6 @@ void ModalaiEsc::Run()
return;
}
SmartLock lock_guard(_lock);
perf_begin(_cycle_perf);
/* Open serial port in this thread */
src/drivers/actuators/modalai_esc/modalai_esc.hpp
+1 -7
View File
@@ -34,7 +34,6 @@
#pragma once
#include <drivers/device/device.h>
#include <lib/cdev/CDev.hpp>
#include <lib/led/led.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/perf/perf_counter.h>
@@ -51,7 +50,7 @@
#include "qc_esc_packet.h"
#include "qc_esc_packet_types.h"
class ModalaiEsc : public cdev::CDev, public ModuleBase<ModalaiEsc>, public OutputModuleInterface
class ModalaiEsc : public ModuleBase<ModalaiEsc>, public OutputModuleInterface
{
public:
ModalaiEsc();
@@ -76,9 +75,6 @@ public:
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
/** @see OutputModuleInterface */
void mixerChanged() override;
virtual int init();
typedef enum {
@@ -183,8 +179,6 @@ private:
ch_assign_t _output_map[MODALAI_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
MixingOutput _mixing_output{"MODALAI_ESC", MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
int _class_instance{-1};
perf_counter_t _cycle_perf;
perf_counter_t _output_update_perf;
src/drivers/cyphal/Cyphal.hpp
-2
View File
@@ -91,8 +91,6 @@ public:
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
void mixerChanged() override {};
void printInfo() { _mixing_output.printStatus(); }
MixingOutput &mixingOutput() { return _mixing_output; }
src/drivers/dshot/CMakeLists.txt
-1
View File
@@ -48,7 +48,6 @@ px4_add_module(
DEPENDS
arch_io_pins
arch_dshot
mixer
mixer_module
MODULE_CONFIG
module.yaml
src/drivers/dshot/DShot.cpp
+57 -73
View File
@@ -47,10 +47,8 @@ DShot::DShot() :
_mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE);
_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
if (_mixing_output.useDynamicMixing()) {
// Avoid using the PWM failsafe params
_mixing_output.setAllFailsafeValues(UINT16_MAX);
}
// Avoid using the PWM failsafe params
_mixing_output.setAllFailsafeValues(UINT16_MAX);
}
DShot::~DShot()
@@ -64,8 +62,6 @@ DShot::~DShot()
int DShot::init()
{
_mixing_output.setDriverInstance(0);
_output_mask = (1u << _num_outputs) - 1;
// Getting initial parameter values
@@ -102,75 +98,72 @@ int DShot::task_spawn(int argc, char *argv[])
void DShot::enable_dshot_outputs(const bool enabled)
{
if (enabled && !_outputs_initialized) {
if (_mixing_output.useDynamicMixing()) {
unsigned int dshot_frequency = 0;
uint32_t dshot_frequency_param = 0;
unsigned int dshot_frequency = 0;
uint32_t dshot_frequency_param = 0;
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = io_timer_get_group(timer);
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = io_timer_get_group(timer);
if (channels == 0) {
continue;
}
if (channels == 0) {
continue;
}
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
unsigned int dshot_frequency_request = 0;
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
unsigned int dshot_frequency_request = 0;
if (tim_config == -5) {
dshot_frequency_request = DSHOT150;
if (tim_config == -5) {
dshot_frequency_request = DSHOT150;
} else if (tim_config == -4) {
dshot_frequency_request = DSHOT300;
} else if (tim_config == -4) {
dshot_frequency_request = DSHOT300;
} else if (tim_config == -3) {
dshot_frequency_request = DSHOT600;
} else if (tim_config == -3) {
dshot_frequency_request = DSHOT600;
} else if (tim_config == -2) {
dshot_frequency_request = DSHOT1200;
} else if (tim_config == -2) {
dshot_frequency_request = DSHOT1200;
} else {
_output_mask &= ~channels; // don't use for dshot
}
if (dshot_frequency_request != 0) {
if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) {
PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name);
param_set_no_notification(handle, &dshot_frequency_param);
} else {
_output_mask &= ~channels; // don't use for dshot
}
if (dshot_frequency_request != 0) {
if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) {
PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name);
param_set_no_notification(handle, &dshot_frequency_param);
} else {
dshot_frequency = dshot_frequency_request;
dshot_frequency_param = tim_config;
}
dshot_frequency = dshot_frequency_request;
dshot_frequency_param = tim_config;
}
}
}
int ret = up_dshot_init(_output_mask, dshot_frequency);
int ret = up_dshot_init(_output_mask, dshot_frequency);
if (ret < 0) {
PX4_ERR("up_dshot_init failed (%i)", ret);
return;
if (ret < 0) {
PX4_ERR("up_dshot_init failed (%i)", ret);
return;
}
_output_mask = ret;
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _output_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
_output_mask = ret;
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _output_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
if (_output_mask == 0) {
// exit the module if no outputs used
request_stop();
return;
}
if (_output_mask == 0) {
// exit the module if no outputs used
request_stop();
return;
}
_outputs_initialized = true;
@@ -190,17 +183,10 @@ void DShot::update_telemetry_num_motors()
int motor_count = 0;
if (_mixing_output.useDynamicMixing()) {
for (unsigned i = 0; i < _num_outputs; ++i) {
if (_mixing_output.isFunctionSet(i)) {
_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
++motor_count;
}
}
} else {
if (_mixing_output.mixers()) {
motor_count = _mixing_output.mixers()->get_multirotor_count();
for (unsigned i = 0; i < _num_outputs; ++i) {
if (_mixing_output.isFunctionSet(i)) {
_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
++motor_count;
}
}
@@ -449,9 +435,7 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
_current_command.clear();
}
if (stop_motors || num_control_groups_updated > 0 || _mixing_output.useDynamicMixing()) {
up_dshot_trigger();
}
up_dshot_trigger();
return true;
}
@@ -471,7 +455,7 @@ void DShot::Run()
_mixing_output.update();
// update output status if armed or if mixer is loaded
bool outputs_on = _mixing_output.armed().armed || _mixing_output.initialized();
bool outputs_on = true;
if (_outputs_on != outputs_on) {
enable_dshot_outputs(outputs_on);
src/drivers/linux_pwm_out/linux_pwm_out.cpp
+2 -188
View File
File diff suppressed because it is too large Load Diff
src/drivers/linux_pwm_out/linux_pwm_out.hpp
+2 -11
View File
@@ -33,9 +33,7 @@
#pragma once
#include <drivers/device/device.h>
#include <lib/cdev/CDev.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h>
@@ -51,8 +49,7 @@
using namespace time_literals;
class LinuxPWMOut : public cdev::CDev, public ModuleBase<LinuxPWMOut>, public OutputModuleInterface
class LinuxPWMOut : public ModuleBase<LinuxPWMOut>, public OutputModuleInterface
{
public:
LinuxPWMOut();
@@ -72,9 +69,7 @@ public:
/** @see ModuleBase::print_status() */
int print_status() override;
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
int init() override;
int init();
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
@@ -82,14 +77,10 @@ public:
private:
static constexpr int MAX_ACTUATORS = 8;
void update_params();
MixingOutput _mixing_output{"PWM_MAIN", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
int _class_instance{-1};
pwm_out::PWMOutBase *_pwm_out{nullptr};
perf_counter_t _cycle_perf;
src/drivers/pca9685_pwm_out/CMakeLists.txt
-1
View File
@@ -40,6 +40,5 @@ px4_add_module(
MODULE_CONFIG
module.yaml
DEPENDS
mixer
mixer_module
)
src/drivers/pca9685_pwm_out/main.cpp
-226
View File
File diff suppressed because it is too large Load Diff
src/drivers/pwm_out/CMakeLists.txt
-1
View File
@@ -49,6 +49,5 @@ px4_add_module(
module.yaml
DEPENDS
arch_io_pins
mixer
mixer_module
)
src/drivers/pwm_out/PWMOut.cpp
+65 -490
View File
File diff suppressed because it is too large Load Diff
src/drivers/pwm_out/PWMOut.hpp
-2
View File
@@ -128,8 +128,6 @@ private:
MixingOutput _mixing_output {PARAM_PREFIX, FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
uint32_t _backup_schedule_interval_us{1_s};
unsigned _pwm_default_rate{50};
unsigned _pwm_alt_rate{50};
uint32_t _pwm_alt_rate_channels{0};
src/drivers/px4io/px4io.cpp
+6 -230
View File
File diff suppressed because it is too large Load Diff
src/drivers/tap_esc/CMakeLists.txt
-1
View File
@@ -45,7 +45,6 @@ px4_add_module(
module.yaml
DEPENDS
led
mixer
mixer_module
tunes
)
src/drivers/tap_esc/TAP_ESC.cpp
+1 -5
View File
@@ -36,7 +36,6 @@
#include <px4_platform_common/sem.hpp>
TAP_ESC::TAP_ESC(char const *const device, uint8_t channels_count):
CDev(TAP_ESC_DEVICE_PATH),
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(device)),
_mixing_output{"TAP_ESC", channels_count, *this, MixingOutput::SchedulingPolicy::Auto, true},
_channels_count(channels_count)
@@ -154,8 +153,7 @@ int TAP_ESC::init()
usleep(2000);
}
/* do regular cdev init */
return CDev::init();
return 0;
}
void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const uint8_t motor_cnt)
@@ -337,8 +335,6 @@ void TAP_ESC::Run()
return;
}
SmartLock lock_guard(_lock);
// push backup schedule
ScheduleDelayed(20_ms);
src/drivers/tap_esc/TAP_ESC.hpp
+2 -3
View File
@@ -52,7 +52,6 @@
#include <lib/drivers/device/device.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/cdev/CDev.hpp>
#include <lib/led/led.h>
#include <lib/tunes/tunes.h>
@@ -80,7 +79,7 @@ using namespace time_literals;
/*
* This driver connects to TAP ESCs via serial.
*/
class TAP_ESC : public cdev::CDev, public ModuleBase<TAP_ESC>, public OutputModuleInterface
class TAP_ESC : public ModuleBase<TAP_ESC>, public OutputModuleInterface
{
public:
TAP_ESC(const char *device, uint8_t channels_count);
@@ -98,7 +97,7 @@ public:
/** @see ModuleBase::print_status() */
int print_status() override;
int init() override;
int init();
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
src/drivers/tap_esc/drv_tap_esc.h
-2
View File
@@ -37,8 +37,6 @@
#include <stdint.h>
#define TAP_ESC_DEVICE_PATH "/dev/tap_esc"
/* At the moment the only known use is with a current sensor */
#define ESC_HAVE_CURRENT_SENSOR
src/drivers/uavcan/CMakeLists.txt
-1
View File
@@ -177,7 +177,6 @@ px4_add_module(
button_publisher
drivers_rangefinder
led
mixer
mixer_module
version
src/drivers/uavcan/uavcan_main.cpp
-16
View File
@@ -575,15 +575,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
_mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
// these are configurable with dynamic mixing
_mixing_interface_esc.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT
_mixing_interface_esc.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value());
param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed_param);
enable_idle_throttle_when_armed(true);
}
/* Set up shared service clients */
_param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset));
_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode));
@@ -931,13 +922,6 @@ void
UavcanNode::enable_idle_throttle_when_armed(bool value)
{
value &= _idle_throttle_when_armed_param > 0;
if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
if (value != _idle_throttle_when_armed) {
_mixing_interface_esc.mixingOutput().setAllMinValues(value ? 1 : 0);
_idle_throttle_when_armed = value;
}
}
}
bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
src/lib/CMakeLists.txt
-1
View File
@@ -51,7 +51,6 @@ add_subdirectory(l1)
add_subdirectory(led)
add_subdirectory(matrix)
add_subdirectory(mathlib)
add_subdirectory(mixer)
add_subdirectory(mixer_module)
add_subdirectory(motion_planning)
add_subdirectory(npfg)
src/lib/mixer/.gitignore
-2
View File
@@ -1,2 +0,0 @@
/test_mixer_multirotor
src/lib/mixer/CMakeLists.txt
-56
View File
@@ -1,56 +0,0 @@
############################################################################
#
# Copyright (c) 2015-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.
#
############################################################################
# required by other mixers
add_subdirectory(MixerBase)
add_subdirectory(HelicopterMixer)
add_subdirectory(MultirotorMixer)
add_subdirectory(NullMixer)
add_subdirectory(SimpleMixer)
add_library(mixer
MixerGroup.cpp
MixerGroup.hpp
load_mixer_file.cpp
mixer_load.h
)
target_link_libraries(mixer
PRIVATE
HelicopterMixer
MultirotorMixer
NullMixer
SimpleMixer
)
add_dependencies(mixer prebuild_targets)
src/lib/mixer/HelicopterMixer/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2015-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.
#
############################################################################
add_library(HelicopterMixer
HelicopterMixer.cpp
HelicopterMixer.hpp
)
target_link_libraries(HelicopterMixer PRIVATE MixerBase)
add_dependencies(HelicopterMixer prebuild_targets)
src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp
-240
View File
File diff suppressed because it is too large Load Diff
src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp
-111
View File
@@ -1,111 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-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.
*
****************************************************************************/
#pragma once
#include <mixer/MixerBase/Mixer.hpp>
/** helicopter swash servo mixer */
struct mixer_heli_servo_s {
float angle;
float arm_length;
float scale;
float offset;
float min_output;
float max_output;
};
#define HELI_CURVES_NR_POINTS 5
/** helicopter swash plate mixer */
struct mixer_heli_s {
uint8_t control_count; /**< number of inputs */
float throttle_curve[HELI_CURVES_NR_POINTS];
float pitch_curve[HELI_CURVES_NR_POINTS];
mixer_heli_servo_s servos[4]; /**< up to four inputs */
};
/**
* Generic helicopter mixer for helicopters with swash plate.
*
* Collects four inputs (roll, pitch, yaw, thrust) and mixes them to servo commands
* for swash plate tilting and throttle- and pitch curves.
*/
class HelicopterMixer : public Mixer
{
public:
/**
* Constructor.
*
* @param control_cb Callback invoked to read inputs.
* @param cb_handle Passed to control_cb.
* @param mixer_info Pointer to heli mixer configuration
*/
HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info);
virtual ~HelicopterMixer() = default;
// no copy, assignment, move, move assignment
HelicopterMixer(const HelicopterMixer &) = delete;
HelicopterMixer &operator=(const HelicopterMixer &) = delete;
HelicopterMixer(HelicopterMixer &&) = delete;
HelicopterMixer &operator=(HelicopterMixer &&) = delete;
/**
* Factory method.
*
* Given a pointer to a buffer containing a text description of the mixer,
* returns a pointer to a new instance of the mixer.
*
* @param control_cb The callback to invoke when fetching a
* control value.
* @param cb_handle Handle passed to the control callback.
* @param buf Buffer containing a text description of
* the mixer.
* @param buflen Length of the buffer in bytes, adjusted
* to reflect the bytes consumed.
* @return A new HelicopterMixer instance, or nullptr
* if the text format is bad.
*/
static HelicopterMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
unsigned &buflen);
unsigned mix(float *outputs, unsigned space) override;
void groups_required(uint32_t &groups) override { groups |= (1 << 0); }
unsigned set_trim(float trim) override { return 4; }
unsigned get_trim(float *trim) override { return 4; }
private:
mixer_heli_s _mixer_info;
};
src/lib/mixer/MixerBase/CMakeLists.txt
-38
View File
@@ -1,38 +0,0 @@
############################################################################
#
# Copyright (c) 2015-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.
#
############################################################################
add_library(MixerBase
Mixer.cpp
Mixer.hpp
)
add_dependencies(MixerBase prebuild_targets)
src/lib/mixer/MixerBase/Mixer.cpp
-124
View File
@@ -1,124 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 mixer.cpp
*
* Programmable multi-channel mixer library.
*/
#include "Mixer.hpp"
#include <math.h>
#include <cstring>
#include <ctype.h>
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
float
Mixer::get_control(uint8_t group, uint8_t index)
{
float value;
_control_cb(_cb_handle, group, index, value);
return value;
}
const char *
Mixer::findtag(const char *buf, unsigned &buflen, char tag)
{
while (buflen >= 2) {
if ((buf[0] == tag) && (buf[1] == ':')) {
return buf;
}
buf++;
buflen--;
}
return nullptr;
}
char
Mixer::findnexttag(const char *buf, unsigned buflen)
{
while (buflen >= 2) {
if (isupper(buf[0]) && buf[1] == ':') {
return buf[0];
}
buf++;
buflen--;
}
return 0;
}
const char *
Mixer::skipline(const char *buf, unsigned &buflen)
{
const char *p;
/* if we can find a CR or NL in the buffer, skip up to it */
if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) {
/* skip up to it AND one beyond - could be on the NUL symbol now */
buflen -= (p - buf) + 1;
return p + 1;
}
return nullptr;
}
bool
Mixer::string_well_formed(const char *buf, unsigned &buflen)
{
/* enforce that the mixer ends with a new line */
for (int i = buflen - 1; i >= 0; i--) {
if (buf[i] == '\0') {
continue;
}
/* require a space or newline at the end of the buffer, fail on printable chars */
if (buf[i] == '\n' || buf[i] == '\r') {
/* found a line ending, so no split symbols / numbers. good. */
return true;
}
}
debug("pre-parser rejected: No newline in buf");
return false;
}
src/lib/mixer/MixerBase/Mixer.hpp
-279
View File
File diff suppressed because it is too large Load Diff
src/lib/mixer/MixerGroup.cpp
-253
View File
File diff suppressed because it is too large Load Diff
src/lib/mixer/MixerGroup.hpp
-172
View File
@@ -1,172 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-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.
*
****************************************************************************/
#pragma once
#include "MixerBase/Mixer.hpp"
/**
* Group of mixers, built up from single mixers and processed
* in order when mixing.
*/
class MixerGroup
{
public:
MixerGroup() = default;
~MixerGroup()
{
reset();
}
// no copy, assignment, move, move assignment
MixerGroup(const MixerGroup &) = delete;
MixerGroup &operator=(const MixerGroup &) = delete;
MixerGroup(MixerGroup &&) = delete;
MixerGroup &operator=(MixerGroup &&) = delete;
unsigned mix(float *outputs, unsigned space);
uint16_t get_saturation_status();
void groups_required(uint32_t &groups);
/**
* Add a mixer to the group.
*
* @param mixer The mixer to be added.
*/
void add_mixer(Mixer *mixer) { _mixers.add(mixer); }
/**
* Remove all the mixers from the group.
*/
void reset() { _mixers.clear(); }
/**
* Count the mixers in the group.
*/
unsigned count() const { return _mixers.size(); }
/**
* Adds mixers to the group based on a text description in a buffer.
*
* Mixer definitions begin with a single capital letter and a colon.
* The actual format of the mixer definition varies with the individual
* mixers; they are summarised here, but see ROMFS/mixers/README for
* more details.
*
* Null Mixer
* ..........
*
* The null mixer definition has the form:
*
* Z:
*
* Simple Mixer
* ............
*
* A simple mixer definition begins with:
*
* M: <control count>
* O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
*
* The second line O: can be omitted. In that case 'O: 10000 10000 0 -10000 10000' is used.
* The definition continues with <control count> entries describing the control
* inputs and their scaling, in the form:
*
* S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
*
* Multirotor Mixer
* ................
*
* The multirotor mixer definition is a single line of the form:
*
* R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
*
* Helicopter Mixer
* ................
*
* The helicopter mixer includes throttle and pitch curves
*
* H: <swash plate servo count>
* T: <0> <2500> <5000> <7500> <10000>
* P: <-10000> <-5000> <0> <5000> <10000>
*
* The definition continues with <swash plate servo count> entries describing
* the position of the servo, in the following form:
*
* S: <angle (deg)> <normalized arm length> <scale> <offset> <lower limit> <upper limit>
*
* @param buf The mixer configuration buffer.
* @param buflen The length of the buffer, updated to reflect
* bytes as they are consumed.
* @return Zero on successful load, nonzero otherwise.
*/
int load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen);
/**
* @brief Update slew rate parameter. This tells instances of the class MultirotorMixer
* the maximum allowed change of the output values per cycle.
* The value is only valid for one cycle, in order to have continuous
* slew rate limiting this function needs to be called before every call
* to mix().
*
* @param[in] delta_out_max Maximum delta output.
*
*/
void set_max_delta_out_once(float delta_out_max);
/*
* Invoke the set_offset method of each mixer in the group
* for each value in page r_page_servo_control_trim
*/
unsigned set_trims(int16_t *v, unsigned n);
unsigned get_trims(int16_t *values);
/**
* @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output.
*
* @param[in] val The value
*/
void set_thrust_factor(float val);
void set_airmode(Mixer::Airmode airmode);
unsigned get_multirotor_count();
void set_dt_once(float dt);
private:
List<Mixer *> _mixers; /**< linked list of mixers */
};
src/lib/mixer/MultirotorMixer/CMakeLists.txt
-113
View File
@@ -1,113 +0,0 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
set(geometry_files
dodeca_bottom_cox.toml
dodeca_top_cox.toml
hex_cox.toml
hex_plus.toml
hex_t.toml
hex_x.toml
octa_cox.toml
octa_cox_wide.toml
octa_plus.toml
octa_x.toml
quad_deadcat.toml
quad_h.toml
quad_plus.toml
quad_vtail.toml
quad_wide.toml
quad_x_cw.toml
quad_x.toml
quad_x_pusher.toml
quad_y.toml
tri_y.toml
twin_engine.toml
)
set(geometries_list)
foreach(geom_file ${geometry_files})
list(APPEND geometries_list ${CMAKE_CURRENT_SOURCE_DIR}/geometries/${geom_file})
endforeach()
set(MIXER_TOOLS ${CMAKE_CURRENT_SOURCE_DIR}/geometries/tools)
# generate mixers and normalize
add_custom_command(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h
COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py -f ${geometries_list} -o mixer_multirotor.generated.h
DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list}
)
add_custom_command(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h
COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --normalize -f ${geometries_list} -o mixer_multirotor_normalized.generated.h
DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list}
)
add_custom_target(mixer_gen DEPENDS mixer_multirotor.generated.h ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h)
# 6DOF mixers
add_custom_command(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --sixdof -f ${geometries_list} -o mixer_multirotor_6dof.generated.h
DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list}
)
add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h)
add_library(MultirotorMixer
MultirotorMixer.cpp
MultirotorMixer.hpp
${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h
${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h
${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
)
target_link_libraries(MultirotorMixer PRIVATE MixerBase)
target_include_directories(MultirotorMixer PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
add_dependencies(MultirotorMixer mixer_gen mixer_gen_6dof prebuild_targets)
if(BUILD_TESTING)
add_executable(test_mixer_multirotor
test_mixer_multirotor.cpp
MultirotorMixer.cpp
)
target_compile_definitions(test_mixer_multirotor PRIVATE MIXER_MULTIROTOR_USE_MOCK_GEOMETRY)
target_compile_options(test_mixer_multirotor PRIVATE -Wno-unused-result)
target_link_libraries(test_mixer_multirotor PRIVATE MixerBase)
add_test(NAME mixer_multirotor
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/mixer_multirotor.py --test --mixer-multirotor-binary $<TARGET_FILE:test_mixer_multirotor>
WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
)
endif()
src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp
-498
View File
File diff suppressed because it is too large Load Diff
src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp
-255
View File
File diff suppressed because it is too large Load Diff
src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml
-40
View File
@@ -1,40 +0,0 @@
# Generic Dodecacopter in X coax configuration, bottom half
[info]
key = "6a"
description = "Generic Dodecacopter in X coax configuration, bottom half"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "mid_right_bottom"
position = [0.0, 1.0, 0.1]
direction = "CCW"
[[rotors]]
name = "mid_left_bottom"
position = [0.0, -1.0, 0.1]
direction = "CW"
[[rotors]]
name = "front_left_bottom"
position = [0.866025, -0.5, 0.1]
direction = "CCW"
[[rotors]]
name = "rear_right_bottom"
position = [-0.866025, 0.5, 0.1]
direction = "CW"
[[rotors]]
name = "front_right_bottom"
position = [0.866025, 0.5, 0.1]
direction = "CW"
[[rotors]]
name = "rear_left_bottom"
position = [-0.866025, -0.5, 0.1]
direction = "CCW"
src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml
-40
View File
@@ -1,40 +0,0 @@
# Generic Dodecacopter in X coax configuration, top half
[info]
key = "6m"
description = "Generic Dodecacopter in X coax configuration, top half"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "mid_right_top"
position = [0.0, 1.0, -0.1]
direction = "CW"
[[rotors]]
name = "mid_left_top"
position = [0.0, -1.0, -0.1]
direction = "CCW"
[[rotors]]
name = "front_left_top"
position = [0.866025, -0.5, -0.1]
direction = "CW"
[[rotors]]
name = "rear_right_top"
position = [-0.866025, 0.5, -0.1]
direction = "CCW"
[[rotors]]
name = "front_right_top"
position = [0.866025, 0.5, -0.1]
direction = "CCW"
[[rotors]]
name = "rear_left_top"
position = [-0.866025, -0.5, -0.1]
direction = "CW"
src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml
-40
View File
@@ -1,40 +0,0 @@
# Generic Hexacopter in coaxial configuration
[info]
key = "6c"
description = "Generic Hexacopter in coaxial configuration"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right_top"
position = [0.5, 0.866, -0.1]
direction = "CW"
[[rotors]]
name = "front_right_bottom"
position = [0.5, 0.866, 0.1]
direction = "CCW"
[[rotors]]
name = "rear_top"
position = [-1.0, 0.0, -0.1]
direction = "CW"
[[rotors]]
name = "rear_bottom"
position = [-1.0, 0.0, 0.1]
direction = "CCW"
[[rotors]]
name = "front_left_top"
position = [0.5, -0.866, -0.1]
direction = "CW"
[[rotors]]
name = "front_left_bottom"
position = [0.5, -0.866, 0.1]
direction = "CCW"
src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml
-40
View File
@@ -1,40 +0,0 @@
# Generic Hexacopter in + configuration
[info]
key = "6+"
description = "Generic Hexacopter in + configuration"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front"
position = [1.0, 0.0, 0.0]
direction = "CW"
[[rotors]]
name = "rear"
position = [-1.0, 0.0, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_left"
position = [-0.5, -0.866025, 0.0]
direction = "CW"
[[rotors]]
name = "front_right"
position = [0.5, 0.866025, 0.0]
direction = "CCW"
[[rotors]]
name = "front_left"
position = [0.5, -0.866025, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_right"
position = [-0.5, 0.866025, 0.0]
direction = "CW"
src/lib/mixer/MultirotorMixer/geometries/hex_t.toml
-40
View File
@@ -1,40 +0,0 @@
# Generic Hexacopter in T configuration
[info]
key = "6t"
description = "Generic Hexacopter in T configuration"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right_top"
position = [0.729, 0.684, 0.1]
direction = "CW"
[[rotors]]
name = "front_right_bottom"
position = [0.729, 0.684, -0.1]
direction = "CCW"
[[rotors]]
name = "rear_top"
position = [-1.0, 0.0, 0.1]
direction = "CW"
[[rotors]]
name = "rear_bottom"
position = [-1.0, 0.0, -0.1]
direction = "CCW"
[[rotors]]
name = "front_left_top"
position = [0.729, -0.684, 0.1]
direction = "CW"
[[rotors]]
name = "front_left_bottom"
position = [0.729, -0.684, -0.1]
direction = "CCW"
src/lib/mixer/MultirotorMixer/geometries/hex_x.toml
-40
View File
@@ -1,40 +0,0 @@
# Generic Hexacopter in X configuration
[info]
key = "6x"
description = "Generic Hexacopter in X configuration"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "mid_right"
position = [0.0, 1.0, 0.0]
direction = "CW"
[[rotors]]
name = "mid_left"
position = [0.0, -1.0, 0.0]
direction = "CCW"
[[rotors]]
name = "front_left"
position = [0.866025, -0.5, 0.0]
direction = "CW"
[[rotors]]
name = "rear_right"
position = [-0.866025, 0.5, 0.0]
direction = "CCW"
[[rotors]]
name = "front_right"
position = [0.866025, 0.5, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_left"
position = [-0.866025, -0.5, 0.0]
direction = "CW"
src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml
-50
View File
@@ -1,50 +0,0 @@
# Generic Octacopter in coax configuration
[info]
key = "8c"
description = "GenericOctacopter in coax configuration"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right_top"
position = [0.707107, 0.707107, -0.1]
direction = "CCW"
[[rotors]]
name = "front_left_top"
position = [0.707107, -0.707107, -0.1]
direction = "CW"
[[rotors]]
name = "rear_left_top"
position = [-0.707107, -0.707107, -0.1]
direction = "CCW"
[[rotors]]
name = "rear_right_top"
position = [-0.707107, 0.707107, -0.1]
direction = "CW"
[[rotors]]
name = "front_left_bottom"
position = [0.707107, -0.707107, 0.1]
direction = "CCW"
[[rotors]]
name = "front_right_bottom"
position = [0.707107, 0.707107, 0.1]
direction = "CW"
[[rotors]]
name = "rear_right_bottom"
position = [-0.707107, 0.707107, 0.1]
direction = "CCW"
[[rotors]]
name = "rear_left_bottom"
position = [-0.707107, -0.707107, 0.1]
direction = "CW"
src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml
-50
View File
@@ -1,50 +0,0 @@
# Generic Octacopter in wide coax configuration
[info]
key = "8cw"
description = "Generic Octacopter in wide coax configuration"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right_top"
position = [0.3746066, 0.927184, -0.1]
direction = "CCW"
[[rotors]]
name = "front_left_top"
position = [0.3746066, -0.927184, -0.1]
direction = "CW"
[[rotors]]
name = "rear_left_top"
position = [-0.62932, -0.777146, -0.1]
direction = "CCW"
[[rotors]]
name = "rear_right_top"
position = [-0.62932, 0.777146, -0.1]
direction = "CW"
[[rotors]]
name = "front_left_bottom"
position = [0.3746066, -0.927184, 0.1]
direction = "CCW"
[[rotors]]
name = "front_right_bottom"
position = [0.3746066, 0.927184, 0.1]
direction = "CW"
[[rotors]]
name = "rear_right_bottom"
position = [-0.62932, 0.777146, 0.1]
direction = "CCW"
[[rotors]]
name = "rear_left_bottom"
position = [-0.62932, -0.777146, 0.1]
direction = "CW"
src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml
-50
View File
@@ -1,50 +0,0 @@
# Generic Octacopter in + configuration
[info]
key = "8+"
description = "Generic Octacopter in + configuration"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front"
position = [1.0, 0.0, 0.0]
direction = "CW"
[[rotors]]
name = "rear"
position = [-1.0, 0.0, 0.0]
direction = "CW"
[[rotors]]
name = "front_right"
position = [0.707107, 0.707107, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_right"
position = [-0.707107, 0.707107, 0.0]
direction = "CCW"
[[rotors]]
name = "front_left"
position = [0.707107, -0.707107, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_left"
position = [-0.707107, -0.707107, 0.0]
direction = "CCW"
[[rotors]]
name = "left"
position = [0.0, -1.0, 0.0]
direction = "CW"
[[rotors]]
name = "right"
position = [0.0, 1.0, 0.0]
direction = "CW"
src/lib/mixer/MultirotorMixer/geometries/octa_x.toml
-50
View File
@@ -1,50 +0,0 @@
# Generic Octacopter in X configuration
[info]
key = "8x"
description = "Generic Octacopter in X configuration"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right"
position = [0.9238795, 0.3826834, 0.0]
direction = "CW"
[[rotors]]
name = "rear_left"
position = [-0.9238795, -0.3826834, 0.0]
direction = "CW"
[[rotors]]
name = "mid_front_right"
position = [0.3826834, 0.9238795, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_right"
position = [-0.9238795, 0.3826834, 0.0]
direction = "CCW"
[[rotors]]
name = "front_left"
position = [0.9238795, -0.3826834, 0.0]
direction = "CCW"
[[rotors]]
name = "mid_rear_left"
position = [-0.3826834, -0.9238795, 0.0]
direction = "CCW"
[[rotors]]
name = "mid_front_left"
position = [0.3826834, -0.9238795, 0.0]
direction = "CW"
[[rotors]]
name = "mid_rear_right"
position = [-0.3826834, 0.9238795, 0.0]
direction = "CW"
src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml
-30
View File
@@ -1,30 +0,0 @@
# SK450 DeadCat Quadcopter.
# Same geometry as quad_wide, except CG is located at intersection of rear arms, so front motors are more loaded.
[info]
key = "4dc"
description = "SK450 DeadCat Quadcopter, CG at intersection of rear arms"
[rotor_default]
direction = "CW"
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.01
[[rotors]]
name = "front_right"
position = [0.1155, 0.245, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_left"
position = [-0.1875, -0.1875, 0.0]
direction = "CCW"
[[rotors]]
name = "front_left"
position = [0.1155, -0.245, 0.0]
[[rotors]]
name = "rear_right"
position = [-0.1875, 0.1875, 0.0]
src/lib/mixer/MultirotorMixer/geometries/quad_h.toml
-30
View File
@@ -1,30 +0,0 @@
# Generic Quadcopter in H configuration
[info]
key = "4h"
description = "Generic Quadcopter in H configuration"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right"
position = [0.707107, 0.707107, 0.0]
direction = "CW"
[[rotors]]
name = "rear_left"
position = [-0.707107, -0.707107, 0.0]
direction = "CW"
[[rotors]]
name = "front_left"
position = [0.707107, -0.707107, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_right"
position = [-0.707107, 0.707107, 0.0]
direction = "CCW"
src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml
-29
View File
@@ -1,29 +0,0 @@
# Generic Quadcopter in + configuration
[info]
key = "4+"
description = "Generic Quadcopter in + configuration"
[rotor_default]
direction = "CW"
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "right"
position = [0.0, 1.0, 0.0]
direction = "CCW"
[[rotors]]
name = "left"
position = [0.0, -1.0, 0.0]
direction = "CCW"
[[rotors]]
name = "front"
position = [1.0, 0.0, 0.0]
[[rotors]]
name = "rear"
position = [-1.0, 0.0, 0.0]
src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml
-33
View File
@@ -1,33 +0,0 @@
# Quadcopter in Y configuration with rear props tilted at 45 degrees
[info]
key = "4vt"
description = "Quadcopter in Y configuration with rear props tilted at 45 degrees"
[rotor_default]
direction = "CW"
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right"
position = [0.2, 0.2, 0.0]
direction = "CW"
[[rotors]]
name = "rear_left"
position = [-0.3, -0.1, -0.1]
axis = [0.0, 0.707106, -0.707106]
direction = "CW"
[[rotors]]
name = "front_left"
position = [0.2, -0.2, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_right"
position = [-0.3, 0.1, -0.1]
axis = [0.0, -0.707106, -0.707106]
direction = "CCW"
src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml
-30
View File
@@ -1,30 +0,0 @@
# Generic Quadcopter in wide configuration
[info]
key = "4w"
description = "Quadcopter in wide configuration. Same geometry as SK450 Deadcat except the CG is moved backward to load all motors equally"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right"
position = [0.1515, 0.245, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_left"
position = [-0.1515, -0.1875, 0.0]
direction = "CCW"
[[rotors]]
name = "front_left"
position = [0.1515, -0.245, 0.0]
direction = "CW"
[[rotors]]
name = "rear_right"
position = [-0.1515, 0.1875, 0.0]
direction = "CW"
src/lib/mixer/MultirotorMixer/geometries/quad_x.toml
-29
View File
@@ -1,29 +0,0 @@
# Generic Quadcopter in X configuration
[info]
key = "4x"
description = "Generic Quadcopter in X configuration"
[rotor_default]
direction = "CW"
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right"
position = [0.707107, 0.707107, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_left"
position = [-0.707107, -0.707107, 0.0]
direction = "CCW"
[[rotors]]
name = "front_left"
position = [0.707107, -0.707107, 0.0]
[[rotors]]
name = "rear_right"
position = [-0.707107, 0.707107, 0.0]
src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml
-30
View File
@@ -1,30 +0,0 @@
# Generic Quadcopter in X configuration
# with clock-wise motor numbering
[info]
key = "4xcw"
description = "Quadcopter in X configuration with clock-wise motor numbering"
[rotor_default]
direction = "CW"
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right"
position = [0.707107, 0.707107, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_right"
position = [-0.707107, 0.707107, 0.0]
[[rotors]]
name = "rear_left"
position = [-0.707107, -0.707107, 0.0]
direction = "CCW"
[[rotors]]
name = "front_left"
position = [0.707107, -0.707107, 0.0]
src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml
-37
View File
@@ -1,37 +0,0 @@
# Quadcopter in X configuration,
# with added pusher motor in the back
[info]
key = "4x1p"
description = "Quadcopter in X configuration, with added pusher motor in the back"
[rotor_default]
direction = "CW"
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right"
position = [1.0, 1.0, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_left"
position = [-1.0, -1.0, 0.0]
direction = "CCW"
[[rotors]]
name = "front_left"
position = [1.0, -1.0, 0.0]
[[rotors]]
name = "rear_right"
position = [-1.0, 1.0, 0.0]
[[rotors]]
name = "pusher"
position = [-1.0, 0.0, 0.0]
axis = [1.0, 0.0, 0.0]
Ct = 2.0
Cm = 0.0
src/lib/mixer/MultirotorMixer/geometries/quad_y.toml
-31
View File
@@ -1,31 +0,0 @@
# Quadcopter in Y configuration with coax rear props
[info]
key = "4y"
description = "Quadcopter in Y configuration with coax rear props"
[rotor_default]
direction = "CW"
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.05
[[rotors]]
name = "front_right"
position = [0.2, 0.2, 0.0]
direction = "CCW"
[[rotors]]
name = "rear_top"
position = [-0.2, 0.0, -0.1]
direction = "CCW"
[[rotors]]
name = "front_left"
position = [0.2, -0.2, 0.0]
direction = "CW"
[[rotors]]
name = "rear_bottom"
position = [-0.2, 0.0, 0.1]
direction = "CW"
src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py
-398
View File
File diff suppressed because it is too large Load Diff
src/lib/mixer/MultirotorMixer/geometries/tri_y.toml
-23
View File
@@ -1,23 +0,0 @@
# Tri Y
[info]
key = "3y"
description = "Tri Y"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.0
direction = "CW"
[[rotors]]
name = "front_right"
position = [0.5, 0.866025, 0.0]
[[rotors]]
name = "front_left"
position = [0.5, -0.866025, 0.0]
[[rotors]]
name = "rear"
position = [-1.0, 0.0, 0.0]
src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml
-19
View File
@@ -1,19 +0,0 @@
# Twin engine
[info]
key = "2-"
description = "Twin engine"
[rotor_default]
axis = [0.0, 0.0, -1.0]
Ct = 1.0
Cm = 0.0
direction = "CW"
[[rotors]]
name = "right"
position = [0.0, 1.0, 0.0]
[[rotors]]
name = "left"
position = [0.0, -1.0, 0.0]
src/lib/mixer/MultirotorMixer/mixer_multirotor.py
-389
View File
File diff suppressed because it is too large Load Diff
src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp
-173
View File
@@ -1,173 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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.
*
****************************************************************************/
/**
* testing binary that runs the multirotor mixer through test cases given
* via file or stdin and compares the mixer output against expected values.
*/
#include "MultirotorMixer.hpp"
#include <cstdio>
#include <math.h>
static const unsigned output_max = 16;
static float actuator_controls[output_max] {};
static int mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &control);
static int
mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
{
control = actuator_controls[control_index];
return 0;
}
int main(int argc, char *argv[])
{
FILE *file_in = stdin;
if (argc > 1) {
file_in = fopen(argv[1], "r");
}
unsigned rotor_count = 0;
MultirotorMixer::Rotor rotors[output_max];
float actuator_outputs[output_max];
// read airmode
int airmode;
fscanf(file_in, "%i", &airmode);
// read the motor count & control allocation matrix
fscanf(file_in, "%i", &rotor_count);
if (rotor_count > output_max) {
return -1;
}
for (unsigned i = 0; i < rotor_count; ++i) {
fscanf(file_in, "%f %f %f %f", &rotors[i].roll_scale, &rotors[i].pitch_scale,
&rotors[i].yaw_scale, &rotors[i].thrust_scale);
}
MultirotorMixer mixer(mixer_callback, 0, rotors, rotor_count);
mixer.set_airmode((Mixer::Airmode)airmode);
int test_counter = 0;
int num_failed = 0;
while (!feof(file_in)) {
// read actuator controls
unsigned count = 0;
while (count < 4 && fscanf(file_in, "%f", &actuator_controls[count]) == 1) {
++count;
}
if (count < 4) {
break;
}
// do the mixing
if (mixer.mix(actuator_outputs, output_max) != rotor_count) {
return -1;
}
// Account for MultirotorMixer outputing [-1,1] and python script [0,1]
for (unsigned i = 0; i < rotor_count; i++) {
actuator_outputs[i] = (actuator_outputs[i] + 1.f) * .5f;
}
// read expected outputs
count = 0;
float expected_output[output_max];
bool failed = false;
while (count < rotor_count && fscanf(file_in, "%f", &expected_output[count]) == 1) {
if (fabsf(expected_output[count] - actuator_outputs[count]) > 0.00001f) {
failed = true;
}
++count;
}
if (count < rotor_count) {
break;
}
if (failed) {
printf("test %i failed:\n", test_counter + 1);
printf("control input : %.3f %.3f %.3f %.3f\n", (double)actuator_controls[0], (double)actuator_controls[1],
(double)actuator_controls[2], (double)actuator_controls[3]);
printf("mixer output : ");
for (unsigned i = 0; i < rotor_count; ++i) {
printf("%.3f ", (double)actuator_outputs[i]);
}
printf("\n");
printf("expected output: ");
for (unsigned i = 0; i < rotor_count; ++i) {
printf("%.3f ", (double)expected_output[i]);
}
printf("\n");
printf("diff : ");
for (unsigned i = 0; i < rotor_count; ++i) {
printf("%.3f ", (double)(expected_output[i] - actuator_outputs[i]));
}
printf("\n");
++num_failed;
}
++test_counter;
}
printf("tested %i cases: %i success, %i failed\n", test_counter,
test_counter - num_failed, num_failed);
if (file_in != stdin) {
fclose(file_in);
}
return num_failed > 0 ? -1 : 0;
}
src/lib/mixer/NullMixer/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2015-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.
#
############################################################################
add_library(NullMixer
NullMixer.cpp
NullMixer.hpp
)
target_link_libraries(NullMixer PRIVATE MixerBase)
add_dependencies(NullMixer prebuild_targets)
src/lib/mixer/NullMixer/NullMixer.cpp
-67
View File
@@ -1,67 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-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 "NullMixer.hpp"
#include <math.h>
#include <cstring>
#include <ctype.h>
unsigned
NullMixer::mix(float *outputs, unsigned space)
{
if (space > 0) {
*outputs = NAN;
return 1;
}
return 0;
}
NullMixer *
NullMixer::from_text(const char *buf, unsigned &buflen)
{
NullMixer *nm = nullptr;
/* enforce that the mixer ends with a new line */
if (!string_well_formed(buf, buflen)) {
return nullptr;
}
if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
nm = new NullMixer;
buflen -= 2;
}
return nm;
}
src/lib/mixer/NullMixer/NullMixer.hpp
-75
View File
@@ -1,75 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-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.
*
****************************************************************************/
#pragma once
#include <mixer/MixerBase/Mixer.hpp>
/**
* Null mixer; returns zero.
*
* Used as a placeholder for output channels that are unassigned in groups.
*/
class NullMixer : public Mixer
{
public:
NullMixer() : Mixer(nullptr, 0) {}
virtual ~NullMixer() = default;
// no copy, assignment, move, move assignment
NullMixer(const NullMixer &) = delete;
NullMixer &operator=(const NullMixer &) = delete;
NullMixer(NullMixer &&) = delete;
NullMixer &operator=(NullMixer &&) = delete;
/**
* Factory method.
*
* Given a pointer to a buffer containing a text description of the mixer,
* returns a pointer to a new instance of the mixer.
*
* @param buf Buffer containing a text description of
* the mixer.
* @param buflen Length of the buffer in bytes, adjusted
* to reflect the bytes consumed.
* @return A new NullMixer instance, or nullptr
* if the text format is bad.
*/
static NullMixer *from_text(const char *buf, unsigned &buflen);
unsigned mix(float *outputs, unsigned space) override;
unsigned set_trim(float trim) override { return 1; }
unsigned get_trim(float *trim) override { return 1; }
};
src/lib/mixer/SimpleMixer/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2015-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.
#
############################################################################
add_library(SimpleMixer
SimpleMixer.cpp
SimpleMixer.hpp
)
target_link_libraries(SimpleMixer PRIVATE MixerBase)
add_dependencies(SimpleMixer prebuild_targets)
src/lib/mixer/SimpleMixer/SimpleMixer.cpp
-390
View File
File diff suppressed because it is too large Load Diff
src/lib/mixer/SimpleMixer/SimpleMixer.hpp
-153
View File
@@ -1,153 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-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.
*
****************************************************************************/
#pragma once
#include <mixer/MixerBase/Mixer.hpp>
/** simple channel scaler */
struct mixer_scaler_s {
float negative_scale{1.0f};
float positive_scale{1.0f};
float offset{0.0f};
float min_output{-1.0f};
float max_output{1.0f};
};
/** mixer input */
struct mixer_control_s {
uint8_t control_group; /**< group from which the input reads */
uint8_t control_index; /**< index within the control group */
mixer_scaler_s scaler; /**< scaling applied to the input before use */
};
#define MIXER_SIMPLE_SIZE(_icount) (sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_control_s))
/** simple mixer */
struct mixer_simple_s {
uint8_t control_count; /**< number of inputs */
mixer_scaler_s output_scaler; /**< scaling for the output */
float slew_rate_rise_time{0.0f}; /**< output max rise time (slew rate limit)*/
mixer_control_s controls[]; /**< actual size of the array is set by control_count */
};
/**
* Simple summing mixer.
*
* Collects zero or more inputs and mixes them to a single output.
*/
class SimpleMixer : public Mixer
{
public:
/**
* Constructor
*
* @param mixinfo Mixer configuration. The pointer passed
* becomes the property of the mixer and
* will be freed when the mixer is deleted.
*/
SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo);
virtual ~SimpleMixer();
// no copy, assignment, move, move assignment
SimpleMixer(const SimpleMixer &) = delete;
SimpleMixer &operator=(const SimpleMixer &) = delete;
SimpleMixer(SimpleMixer &&) = delete;
SimpleMixer &operator=(SimpleMixer &&) = delete;
/**
* Factory method with full external configuration.
*
* Given a pointer to a buffer containing a text description of the mixer,
* returns a pointer to a new instance of the mixer.
*
* @param control_cb The callback to invoke when fetching a
* control value.
* @param cb_handle Handle passed to the control callback.
* @param buf Buffer containing a text description of
* the mixer.
* @param buflen Length of the buffer in bytes, adjusted
* to reflect the bytes consumed.
* @return A new SimpleMixer instance, or nullptr
* if the text format is bad.
*/
static SimpleMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
unsigned &buflen);
unsigned mix(float *outputs, unsigned space) override;
void groups_required(uint32_t &groups) override;
/**
* Check that the mixer configuration as loaded is sensible.
*
* Note that this function will call control_cb, but only cares about
* error returns, not the input value.
*
* @return Zero if the mixer makes sense, nonzero otherwise.
*/
int check();
unsigned set_trim(float trim) override;
unsigned get_trim(float *trim) override;
void set_dt_once(float dt) override;
private:
/**
* Perform simpler linear scaling.
*
* @param scaler The scaler configuration.
* @param input The value to be scaled.
* @return The scaled value.
*/
static float scale(const mixer_scaler_s &scaler, float input);
/**
* Validate a scaler
*
* @param scaler The scaler to be validated.
* @return Zero if good, nonzero otherwise.
*/
static int scale_check(struct mixer_scaler_s &scaler);
static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time);
static int parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
uint8_t &control_index);
float _output_prev{0.f};
float _dt{0.f};
mixer_simple_s *_pinfo;
};
src/lib/mixer/load_mixer_file.cpp
-112
View File
@@ -1,112 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 mixer_load.c
*
* Programmable multi-channel mixer library.
*/
#include <string.h>
#include <stdio.h>
#include <ctype.h>
#include "mixer_load.h"
int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
{
FILE *fp;
char line[120];
/* open the mixer definition file */
fp = fopen(fname, "r");
if (fp == nullptr) {
printf("file not found\n");
return -1;
}
/* read valid lines from the file into a buffer */
buf[0] = '\0';
for (;;) {
/* get a line, bail on error/EOF */
line[0] = '\0';
if (fgets(line, sizeof(line), fp) == nullptr) {
break;
}
/* if the line doesn't look like a mixer definition line, skip it */
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) {
continue;
}
/* compact whitespace in the buffer */
char *t, *f;
for (f = line; *f != '\0'; f++) {
/* scan for space characters */
if (*f == ' ') {
/* look for additional spaces */
t = f + 1;
while (*t == ' ') {
t++;
}
if (*t == '\0') {
/* strip trailing whitespace */
*f = '\0';
} else if (t > (f + 1)) {
memmove(f + 1, t, strlen(t) + 1);
}
}
}
/* if the line is too long to fit in the buffer, bail */
if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
printf("line too long\n");
fclose(fp);
return -1;
}
/* add the line to the buffer */
strcat(buf, line);
}
fclose(fp);
return 0;
}
src/lib/mixer/mixer_load.h
-51
View File
@@ -1,51 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 mixer_load.h
*
*/
#ifndef _SYSTEMLIB_MIXER_LOAD_H
#define _SYSTEMLIB_MIXER_LOAD_H value
#include <px4_platform_common/px4_config.h>
__BEGIN_DECLS
__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
__END_DECLS
#endif
src/lib/mixer_module/CMakeLists.txt
+1 -2
View File
@@ -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)
src/lib/mixer_module/mixer_module.cpp
+60 -480
View File
File diff suppressed because it is too large Load Diff
src/lib/mixer_module/mixer_module.hpp
+12 -43
View File
@@ -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];
src/modules/control_allocator/CMakeLists.txt
-1
View File
@@ -51,6 +51,5 @@ px4_add_module(
mathlib
ActuatorEffectiveness
ControlAllocation
mixer
px4_work_queue
)
src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp
+3 -92
View File
@@ -57,19 +57,12 @@ public:
private:
explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink)
{
int32_t sys_ctrl_alloc = 0;
param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
_use_dynamic_mixing = sys_ctrl_alloc >= 1;
if (_use_dynamic_mixing) {
_act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
}
_act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
}
uORB::Subscription _act_sub{ORB_ID(actuator_outputs)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
bool _use_dynamic_mixing{false};
bool send() override
{
@@ -79,90 +72,8 @@ private:
mavlink_hil_actuator_controls_t msg{};
msg.time_usec = act.timestamp;
if (_use_dynamic_mixing) {
for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
msg.controls[i] = act.output[i];
}
} else {
static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
unsigned system_type = _mavlink->get_system_type();
/* scale outputs depending on system type */
if (system_type == MAV_TYPE_QUADROTOR ||
system_type == MAV_TYPE_HEXAROTOR ||
system_type == MAV_TYPE_OCTOROTOR ||
system_type == MAV_TYPE_VTOL_TAILSITTER_DUOROTOR ||
system_type == MAV_TYPE_VTOL_TAILSITTER_QUADROTOR ||
system_type == MAV_TYPE_VTOL_FIXEDROTOR) {
/* multirotors: set number of rotor outputs depending on type */
unsigned n;
switch (system_type) {
case MAV_TYPE_QUADROTOR:
n = 4;
break;
case MAV_TYPE_HEXAROTOR:
n = 6;
break;
case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR:
n = 2;
break;
case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR:
n = 4;
break;
case MAV_TYPE_VTOL_FIXEDROTOR:
n = 8;
break;
default:
n = 8;
break;
}
for (unsigned i = 0; i < 16; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i < n) {
/* scale PWM out 900..2100 us to 0..1 for rotors */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
} else {
/* scale PWM out 900..2100 us to -1..1 for other channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
}
} else {
/* send 0 when disarmed and for disabled channels */
msg.controls[i] = 0.0f;
}
}
} else {
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
for (unsigned i = 0; i < 16; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i != 3) {
/* scale PWM out 900..2100 us to -1..1 for normal channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
} else {
/* scale PWM out 900..2100 us to 0..1 for throttle */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
}
} else {
/* set 0 for disabled channels */
msg.controls[i] = 0.0f;
}
}
}
for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
msg.controls[i] = act.output[i];
}
// mode (MAV_MODE_FLAG)
src/modules/mc_att_control/mc_att_control.hpp
-1
View File
@@ -33,7 +33,6 @@
#pragma once
#include <lib/mixer/MixerBase/Mixer.hpp> // Airmode
#include <matrix/matrix/math.hpp>
#include <perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
src/modules/mc_att_control/mc_att_control_main.cpp
+1 -1
View File
@@ -117,7 +117,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
_man_yaw_sp = yaw;
} else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f
|| _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) {
|| _param_mc_airmode.get() == 2) {
const float yaw_rate = math::radians(_param_mpc_man_y_max.get());
attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;
src/modules/simulation/pwm_out_sim/CMakeLists.txt
-1
View File
@@ -45,6 +45,5 @@ px4_add_module(
MODULE_CONFIG
${module_config}
DEPENDS
mixer
mixer_module
)
src/modules/simulation/pwm_out_sim/PWMSim.cpp
+2 -2
View File
@@ -139,7 +139,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (i < OutputModuleInterface::MAX_ACTUATORS && !_mixing_output.useDynamicMixing()) {
if (i < OutputModuleInterface::MAX_ACTUATORS && false) {
_mixing_output.minValue(i) = pwm->values[i];
}
}
@@ -151,7 +151,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (i < OutputModuleInterface::MAX_ACTUATORS && !_mixing_output.useDynamicMixing()) {
if (i < OutputModuleInterface::MAX_ACTUATORS && false) {
_mixing_output.maxValue(i) = pwm->values[i];
}
}
src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp
-2
View File
@@ -84,8 +84,6 @@ private:
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
void mixerChanged() override {}
void Run() override;
bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec);
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
+3 -94
View File
@@ -91,10 +91,6 @@ static constexpr vehicle_odometry_s vehicle_odometry_empty {
SimulatorMavlink::SimulatorMavlink() :
ModuleParams(nullptr)
{
int32_t sys_ctrl_alloc = 0;
param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
_use_dynamic_mixing = sys_ctrl_alloc >= 1;
for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) {
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_%s%d", "PWM_MAIN", "FUNC", i + 1);
@@ -123,91 +119,9 @@ void SimulatorMavlink::actuator_controls_from_outputs(mavlink_hil_actuator_contr
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
int _system_type = _param_mav_type.get();
if (_use_dynamic_mixing) {
if (armed) {
for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
msg->controls[i] = _actuator_outputs.output[i];
}
}
} else {
/* 'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust)
all other motors are configured for -1..1 range */
unsigned pos_thrust_motors_count;
bool is_fixed_wing;
switch (_system_type) {
case MAV_TYPE_AIRSHIP:
case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR:
case MAV_TYPE_COAXIAL:
pos_thrust_motors_count = 2;
is_fixed_wing = false;
break;
case MAV_TYPE_TRICOPTER:
pos_thrust_motors_count = 3;
is_fixed_wing = false;
break;
case MAV_TYPE_QUADROTOR:
case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR:
case MAV_TYPE_VTOL_TILTROTOR:
pos_thrust_motors_count = 4;
is_fixed_wing = false;
break;
case MAV_TYPE_VTOL_FIXEDROTOR:
pos_thrust_motors_count = 5;
is_fixed_wing = false;
break;
case MAV_TYPE_HEXAROTOR:
pos_thrust_motors_count = 6;
is_fixed_wing = false;
break;
case MAV_TYPE_OCTOROTOR:
pos_thrust_motors_count = 8;
is_fixed_wing = false;
break;
case MAV_TYPE_SUBMARINE:
pos_thrust_motors_count = 0;
is_fixed_wing = false;
break;
case MAV_TYPE_FIXED_WING:
pos_thrust_motors_count = 0;
is_fixed_wing = true;
break;
default:
pos_thrust_motors_count = 0;
is_fixed_wing = false;
break;
}
if (armed) {
for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
if (!armed) {
/* send 0 when disarmed and for disabled channels */
msg->controls[i] = 0.0f;
} else if ((is_fixed_wing && i == 4) ||
(!is_fixed_wing && i < pos_thrust_motors_count)) { //multirotor, rotor channel
/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */
msg->controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
msg->controls[i] = math::constrain(msg->controls[i], 0.f, 1.f);
} else {
const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
const float pwm_delta = (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2;
/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */
msg->controls[i] = (_actuator_outputs.output[i] - pwm_center) / pwm_delta;
msg->controls[i] = math::constrain(msg->controls[i], -1.f, 1.f);
}
msg->controls[i] = _actuator_outputs.output[i];
}
}
@@ -1065,12 +979,7 @@ void SimulatorMavlink::send()
// Subscribe to topics.
// Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS.
if (_use_dynamic_mixing) {
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs_sim), 0);
} else {
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
}
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs_sim), 0);
// Before starting, we ought to send a heartbeat to initiate the SITL
// simulator to start sending sensor data which will set the time and
src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
-1
View File
@@ -296,7 +296,6 @@ private:
float _last_magx{0.0f};
float _last_magy{0.0f};
float _last_magz{0.0f};
bool _use_dynamic_mixing{false};
float _last_baro_pressure{0.0f};
float _last_baro_temperature{0.0f};