fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 03:57:34 +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-08-24 16:50:02 -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
@@ -41,22 +41,14 @@
using namespace pwm_out;
LinuxPWMOut::LinuxPWMOut() :
CDev("/dev/pwm_out"),
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
{
if (!_mixing_output.useDynamicMixing()) {
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
}
LinuxPWMOut::~LinuxPWMOut()
{
/* clean up the alternate device node */
unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
perf_free(_cycle_perf);
perf_free(_interval_perf);
delete _pwm_out;
@@ -64,21 +56,9 @@ LinuxPWMOut::~LinuxPWMOut()
int LinuxPWMOut::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(PWM_OUTPUT_BASE_DEVICE_PATH);
_mixing_output.setDriverInstance(_class_instance);
_pwm_out = new BOARD_PWM_OUT_IMPL(MAX_ACTUATORS);
ret = _pwm_out->init();
int ret = _pwm_out->init();
if (ret != 0) {
PX4_ERR("PWM output init failed");
@@ -87,8 +67,6 @@ int LinuxPWMOut::init()
return ret;
}
update_params();
ScheduleNow();
return ret;
@@ -134,8 +112,6 @@ void LinuxPWMOut::Run()
return;
}
SmartLock lock_guard(_lock);
perf_begin(_cycle_perf);
perf_count(_interval_perf);
@@ -148,7 +124,7 @@ void LinuxPWMOut::Run()
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
update_params();
updateParams();
}
_mixing_output.updateSubscriptions(false);
@@ -156,168 +132,6 @@ void LinuxPWMOut::Run()
perf_end(_cycle_perf);
}
void LinuxPWMOut::update_params()
{
updateParams();
// skip update when armed or dynamic mixing enabled
if (_mixing_output.armed().armed || _mixing_output.useDynamicMixing()) {
return;
}
int32_t pwm_min_default = PWM_DEFAULT_MIN;
int32_t pwm_max_default = PWM_DEFAULT_MAX;
int32_t pwm_disarmed_default = 0;
int32_t pwm_default_channels = 0;
const char *prefix;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
prefix = "PWM_MAIN";
param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default);
param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels);
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
prefix = "PWM_AUX";
param_get(param_find("PWM_AUX_MIN"), &pwm_min_default);
param_get(param_find("PWM_AUX_MAX"), &pwm_max_default);
param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_AUX_OUT"), &pwm_default_channels);
} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
prefix = "PWM_EXTRA";
param_get(param_find("PWM_EXTRA_MIN"), &pwm_min_default);
param_get(param_find("PWM_EXTRA_MAX"), &pwm_max_default);
param_get(param_find("PWM_EXTRA_DISARM"), &pwm_disarmed_default);
} else {
PX4_ERR("invalid class instance %d", _class_instance);
return;
}
uint32_t single_ch = 0;
uint32_t pwm_default_channel_mask = 0;
while ((single_ch = pwm_default_channels % 10)) {
pwm_default_channel_mask |= 1 << (single_ch - 1);
pwm_default_channels /= 10;
}
char str[17];
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
// PWM_MAIN_MINx
{
sprintf(str, "%s_MIN%u", prefix, i + 1);
int32_t pwm_min = -1;
if (param_get(param_find(str), &pwm_min) == PX4_OK && pwm_min >= 0) {
_mixing_output.minValue(i) = math::constrain(pwm_min, PWM_LOWEST_MIN, PWM_HIGHEST_MIN);
if (pwm_min != _mixing_output.minValue(i)) {
int32_t pwm_min_new = _mixing_output.minValue(i);
param_set(param_find(str), &pwm_min_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.minValue(i) = pwm_min_default;
}
}
// PWM_MAIN_MAXx
{
sprintf(str, "%s_MAX%u", prefix, i + 1);
int32_t pwm_max = -1;
if (param_get(param_find(str), &pwm_max) == PX4_OK && pwm_max >= 0) {
_mixing_output.maxValue(i) = math::constrain(pwm_max, PWM_LOWEST_MAX, PWM_HIGHEST_MAX);
if (pwm_max != _mixing_output.maxValue(i)) {
int32_t pwm_max_new = _mixing_output.maxValue(i);
param_set(param_find(str), &pwm_max_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.maxValue(i) = pwm_max_default;
}
}
// PWM_MAIN_DISx
{
sprintf(str, "%s_DIS%u", prefix, i + 1);
int32_t pwm_dis = -1;
if (param_get(param_find(str), &pwm_dis) == PX4_OK && pwm_dis >= 0) {
_mixing_output.disarmedValue(i) = math::constrain(pwm_dis, 0, PWM_HIGHEST_MAX);
if (pwm_dis != _mixing_output.disarmedValue(i)) {
int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
param_set(param_find(str), &pwm_dis_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.disarmedValue(i) = pwm_disarmed_default;
}
}
// PWM_MAIN_FAILx
{
sprintf(str, "%s_FAIL%u", prefix, i + 1);
int32_t pwm_failsafe = -1;
if (param_get(param_find(str), &pwm_failsafe) == PX4_OK && pwm_failsafe >= 0) {
_mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, 0, PWM_HIGHEST_MAX);
if (pwm_failsafe != _mixing_output.failsafeValue(i)) {
int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
param_set(param_find(str), &pwm_fail_new);
}
} else {
// if no channel specific failsafe value is configured, use the disarmed value
_mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i);
}
}
// PWM_MAIN_REVx
{
sprintf(str, "%s_REV%u", prefix, i + 1);
int32_t pwm_rev = 0;
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
if (pwm_rev >= 1) {
reverse_pwm_mask = reverse_pwm_mask | (2 << i);
} else {
reverse_pwm_mask = reverse_pwm_mask & ~(2 << i);
}
}
}
}
if (_mixing_output.mixers()) {
int16_t values[MAX_ACTUATORS] {};
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
sprintf(str, "%s_TRIM%u", prefix, i + 1);
float pval = 0.0f;
param_get(param_find(str), &pval);
values[i] = roundf(10000 * pval);
}
// copy the trim values to the mixer offsets
_mixing_output.mixers()->set_trims(values, MAX_ACTUATORS);
}
}
int LinuxPWMOut::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
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
@@ -59,14 +59,11 @@ using namespace time_literals;
class PCA9685Wrapper : public ModuleBase<PCA9685Wrapper>, public OutputModuleInterface
{
public:
PCA9685Wrapper(int schd_rate_limit = 400);
~PCA9685Wrapper() override ;
int init();
void mixerChanged() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
@@ -85,8 +82,6 @@ public:
private:
perf_counter_t _cycle_perf;
int _class_instance{-1};
enum class STATE : uint8_t {
INIT,
WAIT_FOR_OSC,
@@ -102,12 +97,6 @@ private:
void Run() override;
protected:
void updateParams() override;
void updatePWMParams();
void updatePWMParamTrim();
int _schd_rate_limit = 400;
PCA9685 *pca9685 = nullptr; // driver handle.
@@ -122,10 +111,6 @@ PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) :
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_schd_rate_limit(schd_rate_limit)
{
if (!_mixing_output.useDynamicMixing()) {
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
}
PCA9685Wrapper::~PCA9685Wrapper()
@@ -157,205 +142,6 @@ int PCA9685Wrapper::init()
return PX4_OK;
}
void PCA9685Wrapper::updateParams()
{
updatePWMParams();
ModuleParams::updateParams();
}
void PCA9685Wrapper::updatePWMParams()
{
if (_mixing_output.useDynamicMixing()) {
return;
}
// update pwm params
const char *pname_format_pwm_ch_max[2] = {"PWM_MAIN_MAX%d", "PWM_AUX_MAX%d"};
const char *pname_format_pwm_ch_min[2] = {"PWM_MAIN_MIN%d", "PWM_AUX_MIN%d"};
const char *pname_format_pwm_ch_fail[2] = {"PWM_MAIN_FAIL%d", "PWM_AUX_FAIL%d"};
const char *pname_format_pwm_ch_dis[2] = {"PWM_MAIN_DIS%d", "PWM_AUX_DIS%d"};
const char *pname_format_pwm_ch_rev[2] = {"PWM_MAIN_REV%d", "PWM_AUX_REV%d"};
int32_t default_pwm_max = PWM_DEFAULT_MAX,
default_pwm_min = PWM_DEFAULT_MIN,
default_pwm_fail = PWM_DEFAULT_MIN,
default_pwm_dis = PWM_MOTOR_OFF;
param_t param_h = param_find("PWM_MAIN_MAX");
if (param_h != PARAM_INVALID) {
param_get(param_h, &default_pwm_max);
} else {
PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_MAX");
}
param_h = param_find("PWM_MAIN_MIN");
if (param_h != PARAM_INVALID) {
param_get(param_h, &default_pwm_min);
} else {
PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_MIN");
}
param_h = param_find("PWM_MAIN_RATE");
if (param_h != PARAM_INVALID) {
int32_t pval = 0;
param_get(param_h, &pval);
if (_last_fetched_Freq != pval) {
_last_fetched_Freq = pval;
_targetFreq = (float)pval; // update only if changed
}
} else {
PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_RATE");
}
for (int i = 0; i < PCA9685_PWM_CHANNEL_COUNT; i++) {
char pname[16];
uint8_t param_group, param_index;
if (i <= 7) { // Main channel
param_group = 0;
param_index = i + 1;
} else { // AUX
param_group = 1;
param_index = i - 8 + 1;
}
sprintf(pname, pname_format_pwm_ch_max[param_group], param_index);
param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
int32_t pval = 0;
param_get(param_h, &pval);
if (pval != -1) {
_mixing_output.maxValue(i) = pval;
} else {
_mixing_output.maxValue(i) = default_pwm_max;
}
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
sprintf(pname, pname_format_pwm_ch_min[param_group], param_index);
param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
int32_t pval = 0;
param_get(param_h, &pval);
if (pval != -1) {
_mixing_output.minValue(i) = pval;
} else {
_mixing_output.minValue(i) = default_pwm_min;
}
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
sprintf(pname, pname_format_pwm_ch_fail[param_group], param_index);
param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
int32_t pval = 0;
param_get(param_h, &pval);
if (pval != -1) {
_mixing_output.failsafeValue(i) = pval;
} else {
_mixing_output.failsafeValue(i) = default_pwm_fail;
}
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
sprintf(pname, pname_format_pwm_ch_dis[param_group], param_index);
param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
int32_t pval = 0;
param_get(param_h, &pval);
if (pval != -1) {
_mixing_output.disarmedValue(i) = pval;
} else {
_mixing_output.disarmedValue(i) = default_pwm_dis;
}
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
sprintf(pname, pname_format_pwm_ch_rev[param_group], param_index);
param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
int32_t pval = 0;
param_get(param_h, &pval);
reverse_pwm_mask &= (0xfffe << i); // clear this bit
reverse_pwm_mask |= (((uint16_t)(pval != 0)) << i); // set to new val
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
}
if (_mixing_output.mixers()) { // only update trims if mixer loaded
updatePWMParamTrim();
}
}
void PCA9685Wrapper::updatePWMParamTrim()
{
const char *pname_format_pwm_ch_trim[2] = {"PWM_MAIN_TRIM%d", "PWM_AUX_TRIM%d"};
int16_t trim_values[PCA9685_PWM_CHANNEL_COUNT] = {};
for (int i = 0; i < PCA9685_PWM_CHANNEL_COUNT; i++) {
char pname[16];
uint8_t param_group, param_index;
if (i <= 7) { // Main channel
param_group = 0;
param_index = i + 1;
} else { // AUX
param_group = 1;
param_index = i - 8 + 1;
}
sprintf(pname, pname_format_pwm_ch_trim[param_group], param_index);
param_t param_h = param_find(pname);
int32_t val;
if (param_h != PARAM_INVALID) {
param_get(param_h, &val);
trim_values[i] = (int16_t)val;
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
}
unsigned n_out = _mixing_output.mixers()->set_trims(trim_values, PCA9685_PWM_CHANNEL_COUNT);
PX4_DEBUG("set %d trims", n_out);
}
bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs,
unsigned num_control_groups_updated)
{
@@ -381,7 +167,6 @@ void PCA9685Wrapper::Run()
switch (_state) {
case STATE::INIT:
pca9685->initReg();
updatePWMParams(); // target frequency fetched, immediately apply it
if (_targetFreq > 0.0f) {
if (pca9685->setFreq(_targetFreq) != PX4_OK) {
@@ -564,17 +349,6 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) {
return PX4_ERROR;
}
void PCA9685Wrapper::mixerChanged()
{
OutputModuleInterface::mixerChanged();
if (_mixing_output.mixers()) { // only update trims if mixer loaded
updatePWMParamTrim();
}
_mixing_output.updateSubscriptions(false);
}
extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){
return PCA9685Wrapper::main(argc, argv);
}
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
@@ -58,10 +58,6 @@ PWMOut::PWMOut(int instance, uint8_t output_base) :
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
{
if (!_mixing_output.useDynamicMixing()) {
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
}
PWMOut::~PWMOut()
@@ -94,14 +90,7 @@ int PWMOut::init()
PX4_ERR("FAILED registering class device");
}
_mixing_output.setDriverInstance(_class_instance);
if (_mixing_output.useDynamicMixing()) {
_num_outputs = FMU_MAX_ACTUATORS;
} else {
_num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE);
}
_num_outputs = FMU_MAX_ACTUATORS;
_pwm_mask = ((1u << _num_outputs) - 1) << _output_base;
_mixing_output.setMaxNumOutputs(_num_outputs);
@@ -114,116 +103,6 @@ int PWMOut::init()
return 0;
}
/* When set_pwm_rate is called from either of the 2 IOCTLs:
*
* PWM_SERVO_SET_UPDATE_RATE - Sets the "alternate" channel's rate to the callers's rate specified
* and the non "alternate" channels to the _pwm_default_rate.
*
* rate_map = _pwm_alt_rate_channels
* default_rate = _pwm_default_rate
* alt_rate = arg of IOCTL (see rates)
*
* PWM_SERVO_SET_SELECT_UPDATE_RATE - The caller's specified rate map selects the "alternate" channels
* to be set to the alt rate. (_pwm_alt_rate)
* All other channels are set to the default rate. (_pwm_default_rate)
*
* rate_map = arg of IOCTL
* default_rate = _pwm_default_rate
* alt_rate = _pwm_alt_rate
* rate_map - A mask of 1's for the channels to be set to the
* alternate rate.
* N.B. All channels is a given group must be set
* to the same rate/mode. (default or alt)
* rates:
* alt_rate, default_rate For PWM is 25 or 400Hz
* For Oneshot there is no rate, 0 is therefore used
* to select Oneshot mode
*/
int PWMOut::set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate)
{
if (_mixing_output.useDynamicMixing()) {
return -EINVAL;
}
PX4_DEBUG("pwm_out%u set_pwm_rate %x %u %u", _instance, rate_map, default_rate, alt_rate);
for (unsigned pass = 0; pass < 2; pass++) {
/* We should note that group is iterated over from 0 to FMU_MAX_ACTUATORS.
* This allows for the ideal worlds situation: 1 channel per group
* configuration.
*
* This is typically not what HW supports. A group represents a timer
* and channels belongs to a timer.
* Therefore all channels in a group are dependent on the timer's
* common settings and can not be independent in terms of count frequency
* (granularity of pulse width) and rate (period of repetition).
*
* To say it another way, all channels in a group must have the same
* rate and mode. (See rates above.)
*/
for (unsigned group = 0; group < FMU_MAX_ACTUATORS; group++) {
// get the channel mask for this rate group
uint32_t mask = _pwm_mask & up_pwm_servo_get_rate_group(group);
if (mask == 0) {
continue;
}
// all channels in the group must be either default or alt-rate
uint32_t alt = rate_map & mask;
if (pass == 0) {
// preflight
if ((alt != 0) && (alt != mask)) {
PX4_WARN("rate group %u mask %" PRIx32 " bad overlap %" PRIx32, group, mask, alt);
// not a legal map, bail
return -EINVAL;
}
} else {
// set it - errors here are unexpected
if (alt != 0) {
if (up_pwm_servo_set_rate_group_update(group, alt_rate) != OK) {
PX4_WARN("rate group set alt failed");
return -EINVAL;
}
} else {
if (up_pwm_servo_set_rate_group_update(group, default_rate) != OK) {
PX4_WARN("rate group set default failed");
return -EINVAL;
}
}
}
}
}
_pwm_alt_rate_channels = rate_map;
_pwm_default_rate = default_rate;
_pwm_alt_rate = alt_rate;
// minimum rate for backup schedule
unsigned backup_schedule_rate_hz = math::min(_pwm_default_rate, _pwm_alt_rate);
if (backup_schedule_rate_hz == 0) {
// OneShot rate is 0
backup_schedule_rate_hz = 50;
}
// constrain reasonably (1 to 50 Hz)
backup_schedule_rate_hz = math::constrain(backup_schedule_rate_hz, 1u, 50u);
_backup_schedule_interval_us = roundf(1e6f / backup_schedule_rate_hz);
_current_update_rate = 0; // force update
return OK;
}
void PWMOut::update_current_rate()
{
/*
@@ -274,7 +153,8 @@ int PWMOut::task_spawn(int argc, char *argv[])
}
// only start one instance with dynamic mixing
if (dev->_mixing_output.useDynamicMixing()) {
//if (dev->_mixing_output.useDynamicMixing()) {
if (true) {
break;
}
@@ -297,110 +177,65 @@ bool PWMOut::update_pwm_out_state(bool on)
{
if (on && !_pwm_initialized && _pwm_mask != 0) {
if (_mixing_output.useDynamicMixing()) {
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
_timer_rates[timer] = -1;
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
_timer_rates[timer] = -1;
uint32_t channels = io_timer_get_group(timer);
uint32_t channels = io_timer_get_group(timer);
if (channels == 0) {
continue;
}
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);
if (tim_config > 0) {
_timer_rates[timer] = tim_config;
} else if (tim_config == -1) { // OneShot
_timer_rates[timer] = 0;
} else {
_pwm_mask &= ~channels; // don't use for pwm
}
if (channels == 0) {
continue;
}
int ret = up_pwm_servo_init(_pwm_mask);
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
if (ret < 0) {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
return false;
}
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
_pwm_mask = ret;
if (tim_config > 0) {
_timer_rates[timer] = tim_config;
// set the timer rates
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
if (channels == 0) {
continue;
}
ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]);
if (ret != 0) {
PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret);
_timer_rates[timer] = -1;
_pwm_mask &= ~channels;
}
}
_pwm_initialized = true;
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _pwm_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
} else {
// Collect all PWM masks from all instances
uint32_t pwm_mask_new = 0;
// Collect the PWM alt rate channels across all instances
uint32_t pwm_alt_rate_channels_new = 0;
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
if (_objects[i].load()) {
pwm_mask_new |= _objects[i].load()->get_pwm_mask();
pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels();
}
}
// Initialize the PWM output state for all instances
// this is re-done once per instance, but harmless
int ret = up_pwm_servo_init(pwm_mask_new);
if (ret >= 0) {
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
if (_objects[i].load()) {
_objects[i].load()->set_pwm_mask(_objects[i].load()->get_pwm_mask() & ret);
}
}
// Set rate is not affecting non-masked channels, so can be called
// individually
set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate());
_pwm_initialized = true;
// Other instances need to call up_pwm_servo_arm again after we initialized
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
if (i != _instance) {
_require_arming[i].store(true);
}
}
} else if (tim_config == -1) { // OneShot
_timer_rates[timer] = 0;
} else {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
_pwm_mask &= ~channels; // don't use for pwm
}
}
int ret = up_pwm_servo_init(_pwm_mask);
if (ret < 0) {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
return false;
}
_pwm_mask = ret;
// set the timer rates
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
if (channels == 0) {
continue;
}
ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]);
if (ret != 0) {
PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret);
_timer_rates[timer] = -1;
_pwm_mask &= ~channels;
}
}
_pwm_initialized = true;
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _pwm_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
}
@@ -452,16 +287,10 @@ void PWMOut::Run()
perf_begin(_cycle_perf);
perf_count(_interval_perf);
if (!_mixing_output.useDynamicMixing()) {
// push backup schedule
ScheduleDelayed(_backup_schedule_interval_us);
}
_mixing_output.update();
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing()
|| _mixing_output.armed().in_esc_calibration_mode;
bool pwm_on = true;
if (_pwm_on != pwm_on || _require_arming[_instance].load()) {
@@ -477,13 +306,7 @@ void PWMOut::Run()
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings)
update_params();
}
}
if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) {
update_current_rate();
update_params();
}
// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
@@ -503,7 +326,7 @@ void PWMOut::update_params()
updateParams();
// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
if (_mixing_output.useDynamicMixing() && !_first_param_update) {
if (!_first_param_update) {
for (size_t i = 0; i < _num_outputs; i++) {
if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
int32_t output_function;
@@ -544,203 +367,6 @@ void PWMOut::update_params()
}
_first_param_update = false;
if (_mixing_output.useDynamicMixing()) {
return;
}
int32_t pwm_min_default = PWM_DEFAULT_MIN;
int32_t pwm_max_default = PWM_DEFAULT_MAX;
int32_t pwm_disarmed_default = 0;
int32_t pwm_rate_default = 50;
int32_t pwm_default_channels = 0;
const char *prefix;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
prefix = "PWM_MAIN";
param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default);
param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default);
param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels);
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
prefix = "PWM_AUX";
param_get(param_find("PWM_AUX_MIN"), &pwm_min_default);
param_get(param_find("PWM_AUX_MAX"), &pwm_max_default);
param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_AUX_RATE"), &pwm_rate_default);
param_get(param_find("PWM_AUX_OUT"), &pwm_default_channels);
} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
prefix = "PWM_EXTRA";
param_get(param_find("PWM_EXTRA_MIN"), &pwm_min_default);
param_get(param_find("PWM_EXTRA_MAX"), &pwm_max_default);
param_get(param_find("PWM_EXTRA_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_EXTRA_RATE"), &pwm_rate_default);
} else {
PX4_ERR("invalid class instance %d", _class_instance);
return;
}
uint32_t single_ch = 0;
uint32_t pwm_default_channel_mask = 0;
while ((single_ch = pwm_default_channels % 10)) {
pwm_default_channel_mask |= 1 << (single_ch - 1);
pwm_default_channels /= 10;
}
// update the counter
// this is needed to decide if disarmed PWM output should be turned on or not
int num_disarmed_set = 0;
char str[17];
for (unsigned i = 0; i < _num_outputs; i++) {
// PWM_MAIN_MINx
{
sprintf(str, "%s_MIN%u", prefix, i + 1);
int32_t pwm_min = -1;
if (param_get(param_find(str), &pwm_min) == PX4_OK) {
if (pwm_min >= 0 && pwm_min != 1000) {
_mixing_output.minValue(i) = math::constrain(pwm_min, (int32_t) PWM_LOWEST_MIN, (int32_t) PWM_HIGHEST_MIN);
if (pwm_min != _mixing_output.minValue(i)) {
int32_t pwm_min_new = _mixing_output.minValue(i);
param_set(param_find(str), &pwm_min_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.minValue(i) = pwm_min_default;
}
} else {
PX4_ERR("param %s not found", str);
}
}
// PWM_MAIN_MAXx
{
sprintf(str, "%s_MAX%u", prefix, i + 1);
int32_t pwm_max = -1;
if (param_get(param_find(str), &pwm_max) == PX4_OK) {
if (pwm_max >= 0 && pwm_max != 2000) {
_mixing_output.maxValue(i) = math::constrain(pwm_max, (int32_t) PWM_LOWEST_MAX, (int32_t) PWM_HIGHEST_MAX);
if (pwm_max != _mixing_output.maxValue(i)) {
int32_t pwm_max_new = _mixing_output.maxValue(i);
param_set(param_find(str), &pwm_max_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.maxValue(i) = pwm_max_default;
}
} else {
PX4_ERR("param %s not found", str);
}
}
// PWM_MAIN_DISx
{
sprintf(str, "%s_DIS%u", prefix, i + 1);
int32_t pwm_dis = -1;
if (param_get(param_find(str), &pwm_dis) == PX4_OK) {
if (pwm_dis >= 0 && pwm_dis != 900) {
_mixing_output.disarmedValue(i) = math::constrain(pwm_dis, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX);
if (pwm_dis != _mixing_output.disarmedValue(i)) {
int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
param_set(param_find(str), &pwm_dis_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.disarmedValue(i) = pwm_disarmed_default;
}
} else {
PX4_ERR("param %s not found", str);
}
if (_mixing_output.disarmedValue(i) > 0) {
num_disarmed_set++;
}
}
// PWM_MAIN_FAILx
{
sprintf(str, "%s_FAIL%u", prefix, i + 1);
int32_t pwm_failsafe = -1;
if (param_get(param_find(str), &pwm_failsafe) == PX4_OK) {
if (pwm_failsafe >= 0) {
_mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX);
if (pwm_failsafe != _mixing_output.failsafeValue(i)) {
int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
param_set(param_find(str), &pwm_fail_new);
}
} else {
// if no channel specific failsafe value is configured, use the disarmed value
_mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i);
}
} else {
PX4_ERR("param %s not found", str);
}
}
// PWM_MAIN_REVx
{
sprintf(str, "%s_REV%u", prefix, i + 1);
int32_t pwm_rev = 0;
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
if (pwm_rev >= 1) {
reverse_pwm_mask = reverse_pwm_mask | (1 << i);
} else {
reverse_pwm_mask = reverse_pwm_mask & ~(1 << i);
}
} else {
PX4_ERR("param %s not found", str);
}
}
}
if (_mixing_output.mixers()) {
int16_t values[FMU_MAX_ACTUATORS] {};
for (unsigned i = 0; i < _num_outputs; i++) {
sprintf(str, "%s_TRIM%u", prefix, i + 1);
float pval = 0.0f;
if (param_get(param_find(str), &pval) != PX4_OK) {
PX4_ERR("param %s not found", str);
}
values[i] = roundf(10000 * pval);
}
// copy the trim values to the mixer offsets
_mixing_output.mixers()->set_trims(values, _num_outputs);
}
_num_disarmed_set = num_disarmed_set;
}
int PWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
@@ -786,7 +412,7 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_UPDATE_RATE:
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
ret = -EINVAL;
break;
case PWM_SERVO_GET_UPDATE_RATE:
@@ -794,7 +420,7 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
ret = -EINVAL;
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
@@ -823,38 +449,9 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_mixing_output.minValue(i) = PWM_HIGHEST_MIN;
}
#if PWM_LOWEST_MIN > 0
else if (pwm->values[i] < PWM_LOWEST_MIN) {
_mixing_output.minValue(i) = PWM_LOWEST_MIN;
}
#endif
else {
_mixing_output.minValue(i) = pwm->values[i];
}
}
break;
}
case PWM_SERVO_SET_MIN_PWM:
ret = -EINVAL;
break;
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
@@ -868,31 +465,9 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_mixing_output.maxValue(i) = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_mixing_output.maxValue(i) = PWM_HIGHEST_MAX;
} else {
_mixing_output.maxValue(i) = pwm->values[i];
}
}
break;
}
case PWM_SERVO_SET_MAX_PWM:
ret = -EINVAL;
break;
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
@@ -997,7 +572,7 @@ int PWMOut::print_status()
perf_print_counter(_interval_perf);
_mixing_output.printStatus();
if (_mixing_output.useDynamicMixing() && _pwm_initialized) {
if (_pwm_initialized) {
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
if (_timer_rates[timer] >= 0) {
PX4_INFO_RAW("Timer %i: rate: %3i", timer, _timer_rates[timer]);
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
@@ -306,11 +306,6 @@ private:
*/
int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits);
/**
* Send mixer definition text to IO
*/
int mixer_send(const char *buf, unsigned buflen, unsigned retries = 3);
/**
* Handle a status update from IO.
*
@@ -355,11 +350,6 @@ PX4IO::PX4IO(device::Device *interface) :
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(PX4IO_SERIAL_DEVICE)),
_interface(interface)
{
if (!_mixing_output.useDynamicMixing()) {
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
_mixing_output.setLowrateSchedulingInterval(20_ms);
}
@@ -487,7 +477,6 @@ int PX4IO::init()
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
_mixing_output.setDriverInstance(_class_instance);
_mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL);
}
@@ -712,7 +701,7 @@ void PX4IO::update_params()
updateParams();
if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) {
if (!_mixing_output.armed().armed) {
// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
if (!_first_param_update) {
@@ -764,177 +753,6 @@ void PX4IO::update_params()
}
_first_param_update = false;
// skip update when armed or PWM disabled
if (_mixing_output.armed().armed || _class_instance == -1 || _mixing_output.useDynamicMixing()) {
return;
}
int32_t pwm_min_default = PWM_DEFAULT_MIN;
int32_t pwm_max_default = PWM_DEFAULT_MAX;
int32_t pwm_disarmed_default = 0;
int32_t pwm_rate_default = 50;
int32_t pwm_default_channels = 0;
const char *prefix = "PWM_MAIN";
param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default);
param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default);
param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels);
uint32_t single_ch = 0;
uint32_t pwm_default_channel_mask = 0;
while ((single_ch = pwm_default_channels % 10)) {
pwm_default_channel_mask |= 1 << (single_ch - 1);
pwm_default_channels /= 10;
}
char str[17];
// PWM_MAIN_MINx
if (!_pwm_min_configured) {
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_MIN%u", prefix, i + 1);
int32_t pwm_min = -1;
if (param_get(param_find(str), &pwm_min) == PX4_OK) {
if (pwm_min >= 0 && pwm_min != 1000) {
_mixing_output.minValue(i) = math::constrain(pwm_min, static_cast<int32_t>(PWM_LOWEST_MIN),
static_cast<int32_t>(PWM_HIGHEST_MIN));
if (pwm_min != _mixing_output.minValue(i)) {
int32_t pwm_min_new = _mixing_output.minValue(i);
param_set(param_find(str), &pwm_min_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.minValue(i) = pwm_min_default;
}
}
}
_pwm_min_configured = true;
}
// PWM_MAIN_MAXx
if (!_pwm_max_configured) {
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_MAX%u", prefix, i + 1);
int32_t pwm_max = -1;
if (param_get(param_find(str), &pwm_max) == PX4_OK) {
if (pwm_max >= 0 && pwm_max != 2000) {
_mixing_output.maxValue(i) = math::constrain(pwm_max, static_cast<int32_t>(PWM_LOWEST_MAX),
static_cast<int32_t>(PWM_HIGHEST_MAX));
if (pwm_max != _mixing_output.maxValue(i)) {
int32_t pwm_max_new = _mixing_output.maxValue(i);
param_set(param_find(str), &pwm_max_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.maxValue(i) = pwm_max_default;
}
}
}
_pwm_max_configured = true;
}
// PWM_MAIN_DISx
if (!_pwm_dis_configured) {
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_DIS%u", prefix, i + 1);
int32_t pwm_dis = -1;
if (param_get(param_find(str), &pwm_dis) == PX4_OK) {
if (pwm_dis >= 0 && pwm_dis != 900) {
_mixing_output.disarmedValue(i) = math::constrain(pwm_dis, static_cast<int32_t>(0),
static_cast<int32_t>(PWM_HIGHEST_MAX));
if (pwm_dis != _mixing_output.disarmedValue(i)) {
int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
param_set(param_find(str), &pwm_dis_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.disarmedValue(i) = pwm_disarmed_default;
}
}
}
_pwm_dis_configured = true;
updateDisarmed();
}
// PWM_MAIN_FAILx
if (!_pwm_fail_configured) {
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_FAIL%u", prefix, i + 1);
int32_t pwm_fail = -1;
if (param_get(param_find(str), &pwm_fail) == PX4_OK) {
if (pwm_fail >= 0) {
_mixing_output.failsafeValue(i) = math::constrain(pwm_fail, static_cast<int32_t>(0),
static_cast<int32_t>(PWM_HIGHEST_MAX));
if (pwm_fail != _mixing_output.failsafeValue(i)) {
int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
param_set(param_find(str), &pwm_fail_new);
}
} else {
// if no channel specific failsafe value is configured, use the disarmed value
_mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i);
}
}
}
_pwm_fail_configured = true;
updateFailsafe();
}
// PWM_MAIN_REVx
if (!_pwm_rev_configured) {
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
reverse_pwm_mask = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_REV%u", prefix, i + 1);
int32_t pwm_rev = -1;
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
if (pwm_rev >= 1) {
reverse_pwm_mask |= (1 << i);
}
}
}
_pwm_rev_configured = true;
}
// PWM_MAIN_TRIMx
{
int16_t values[8] {};
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_TRIM%u", prefix, i + 1);
float pwm_trim = 0.f;
if (param_get(param_find(str), &pwm_trim) == PX4_OK) {
values[i] = roundf(10000 * pwm_trim);
}
}
if (_mixing_output.mixers()) {
// copy the trim values to the mixer offsets
_mixing_output.mixers()->set_trims(values, _max_actuators);
}
}
}
void PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result)
@@ -1187,30 +1005,13 @@ int PX4IO::io_get_status()
status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i);
}
if (_mixing_output.useDynamicMixing()) {
{
int i = 0;
for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) {
// This is a bit different than below, setting the groups, not the channels
status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset);
}
} else {
// PWM rates, 0 = low rate, 1 = high rate
const uint16_t pwm_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
const int pwm_low_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
const int pwm_high_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
for (unsigned i = 0; i < _max_actuators; i++) {
if (pwm_rate & (1 << i)) {
status.pwm_rate_hz[i] = pwm_high_rate;
} else {
status.pwm_rate_hz[i] = pwm_low_rate;
}
}
}
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
@@ -1544,13 +1345,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case PWM_SERVO_SET_UPDATE_RATE:
PX4_DEBUG("PWM_SERVO_SET_UPDATE_RATE");
if (_mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
/* set the requested alternate rate */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
ret = -EINVAL;
break;
case PWM_SERVO_GET_UPDATE_RATE:
@@ -1562,26 +1357,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case PWM_SERVO_SET_SELECT_UPDATE_RATE: {
PX4_DEBUG("PWM_SERVO_SET_SELECT_UPDATE_RATE");
if (_mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
/* blindly clear the PWM update alarm - might be set for some other reason */
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
/* attempt to set the rate map */
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg);
/* check that the changes took */
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) {
ret = -EINVAL;
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
PX4_ERR("failed setting PWM rate on IO");
}
ret = -EINVAL;
break;
}
@@ -1624,7 +1400,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
if (pwm->values[i] != 0 && false) {
_mixing_output.minValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MIN);
}
}
@@ -1654,7 +1430,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
if (pwm->values[i] != 0 && false) {
_mixing_output.maxValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MAX, (uint16_t)PWM_HIGHEST_MAX);
}
}
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
@@ -1,240 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 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_helicopter.cpp
*
* Helicopter mixers.
*/
#include "HelicopterMixer.hpp"
#include <mathlib/mathlib.h>
#include <cstdio>
#include <px4_platform_common/defines.h>
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
//#include <debug.h>
//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args)
using math::constrain;
HelicopterMixer::HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info) :
Mixer(control_cb, cb_handle),
_mixer_info(mixer_info)
{
}
HelicopterMixer *
HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
{
mixer_heli_s mixer_info;
unsigned swash_plate_servo_count = 0;
unsigned u[5];
int s[5];
int used;
/* enforce that the mixer ends with a new line */
if (!string_well_formed(buf, buflen)) {
return nullptr;
}
if (sscanf(buf, "H: %u%n", &swash_plate_servo_count, &used) != 1) {
debug("helicopter parse failed on '%s'", buf);
return nullptr;
}
if (swash_plate_servo_count < 3 || swash_plate_servo_count > 4) {
debug("only supporting swash plate with 3 or 4 servos");
return nullptr;
}
if (used > (int)buflen) {
debug("OVERFLOW: helicopter spec used %d of %u", used, buflen);
return nullptr;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return nullptr;
}
buf = findtag(buf, buflen, 'T');
if ((buf == nullptr) || (buflen < 12)) {
debug("control parser failed finding tag, ret: '%s'", buf);
return nullptr;
}
if (sscanf(buf, "T: %u %u %u %u %u",
&u[0], &u[1], &u[2], &u[3], &u[4]) != 5) {
debug("control parse failed on '%s'", buf);
return nullptr;
}
for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) {
mixer_info.throttle_curve[i] = ((float) u[i]) / 10000.0f;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return nullptr;
}
buf = findtag(buf, buflen, 'P');
if ((buf == nullptr) || (buflen < 12)) {
debug("control parser failed finding tag, ret: '%s'", buf);
return nullptr;
}
if (sscanf(buf, "P: %d %d %d %d %d",
&s[0], &s[1], &s[2], &s[3], &s[4]) != 5) {
debug("control parse failed on '%s'", buf);
return nullptr;
}
for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) {
mixer_info.pitch_curve[i] = ((float) s[i]) / 10000.0f;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return nullptr;
}
mixer_info.control_count = swash_plate_servo_count;
/* Now loop through the servos */
for (unsigned i = 0; i < mixer_info.control_count; i++) {
buf = findtag(buf, buflen, 'S');
if ((buf == nullptr) || (buflen < 12)) {
debug("control parser failed finding tag, ret: '%s'", buf);
return nullptr;
}
if (sscanf(buf, "S: %u %u %d %d %d %d",
&u[0],
&u[1],
&s[0],
&s[1],
&s[2],
&s[3]) != 6) {
debug("control parse failed on '%s'", buf);
return nullptr;
}
mixer_info.servos[i].angle = ((float) u[0]) * M_PI_F / 180.0f;
mixer_info.servos[i].arm_length = ((float) u[1]) / 10000.0f;
mixer_info.servos[i].scale = ((float) s[0]) / 10000.0f;
mixer_info.servos[i].offset = ((float) s[1]) / 10000.0f;
mixer_info.servos[i].min_output = ((float) s[2]) / 10000.0f;
mixer_info.servos[i].max_output = ((float) s[3]) / 10000.0f;
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return nullptr;
}
}
debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
HelicopterMixer *hm = new HelicopterMixer(control_cb, cb_handle, mixer_info);
if (hm != nullptr) {
debug("loaded heli mixer with %d swash plate input(s)", mixer_info.control_count);
} else {
debug("could not allocate memory for mixer");
}
return hm;
}
unsigned
HelicopterMixer::mix(float *outputs, unsigned space)
{
if (space < _mixer_info.control_count + 1u) {
return 0;
}
/* Find index to use for curves */
float thrust_cmd = get_control(0, 3);
int idx = (thrust_cmd / 0.25f);
/* Make sure idx is in range */
if (idx < 0) {
idx = 0;
} else if (idx > HELI_CURVES_NR_POINTS - 2) {
/* We access idx + 1 below, so max legal index is (size - 2) */
idx = HELI_CURVES_NR_POINTS - 2;
}
/* Local throttle curve gradient and offset */
float tg = (_mixer_info.throttle_curve[idx + 1] - _mixer_info.throttle_curve[idx]) / 0.25f;
float to = (_mixer_info.throttle_curve[idx]) - (tg * idx * 0.25f);
float throttle = constrain(2.0f * (tg * thrust_cmd + to) - 1.0f, -1.0f, 1.0f);
/* Local pitch curve gradient and offset */
float pg = (_mixer_info.pitch_curve[idx + 1] - _mixer_info.pitch_curve[idx]) / 0.25f;
float po = (_mixer_info.pitch_curve[idx]) - (pg * idx * 0.25f);
float collective_pitch = constrain((pg * thrust_cmd + po), -0.5f, 0.5f);
float roll_cmd = get_control(0, 0);
float pitch_cmd = get_control(0, 1);
outputs[0] = throttle;
for (unsigned i = 0; i < _mixer_info.control_count; i++) {
outputs[i + 1] = collective_pitch
+ cosf(_mixer_info.servos[i].angle) * pitch_cmd * _mixer_info.servos[i].arm_length
- sinf(_mixer_info.servos[i].angle) * roll_cmd * _mixer_info.servos[i].arm_length;
outputs[i + 1] *= _mixer_info.servos[i].scale;
outputs[i + 1] += _mixer_info.servos[i].offset;
outputs[i + 1] = constrain(outputs[i + 1], _mixer_info.servos[i].min_output, _mixer_info.servos[i].max_output);
}
return _mixer_info.control_count + 1;
}
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
@@ -1,279 +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.
*
****************************************************************************/
/**
* @file Mixer.hpp
*
* Generic, programmable, procedural control signal mixers.
*
* This library implements a generic mixer interface that can be used
* by any driver or subsytem that wants to combine several control signals
* into a single output.
*
* Terminology
* ===========
*
* control value
* A mixer input value, typically provided by some controlling
* component of the system.
*
* control group
* A collection of controls provided by a single controlling component.
*
* actuator
* The mixer output value.
*
*
* Mixing basics
* =============
*
* An actuator derives its value from the combination of one or more
* control values. Each of the control values is scaled according to
* the actuator's configuration and then combined to produce the
* actuator value, which may then be further scaled to suit the specific
* output type.
*
* Internally, all scaling is performed using floating point values.
* Inputs and outputs are clamped to the range -1.0 to 1.0.
*
* control control control
* | | |
* v v v
* scale scale scale
* | | |
* | v |
* +-------> mix <------+
* |
* scale
* |
* v
* out
*
* Scaling
* -------
*
* Each scaler allows the input value to be scaled independently for
* inputs greater/less than zero. An offset can be applied to the output,
* as well as lower and upper boundary constraints.
* Negative scaling factors cause the output to be inverted (negative input
* produces positive output).
*
* Scaler pseudocode:
*
* if (input < 0)
* output = (input * NEGATIVE_SCALE) + OFFSET
* else
* output = (input * POSITIVE_SCALE) + OFFSET
*
* if (output < LOWER_LIMIT)
* output = LOWER_LIMIT
* if (output > UPPER_LIMIT)
* output = UPPER_LIMIT
*
*
* Mixing
* ------
*
* Mixing behaviour varies based on the specific mixer class; each
* mixer class describes its behaviour in more detail.
*
*
* Controls
* --------
*
* The precise assignment of controls may vary depending on the
* application, but the following assignments should be used
* when appropriate. Some mixer classes have specific assumptions
* about the assignment of controls.
*
* control | standard meaning
* --------+-----------------------
* 0 | roll
* 1 | pitch
* 2 | yaw
* 3 | primary thrust
*/
#pragma once
#include <containers/List.hpp>
#include <mathlib/mathlib.h>
/**
* Abstract class defining a mixer mixing zero or more inputs to
* one or more outputs.
*/
class Mixer : public ListNode<Mixer *>
{
public:
enum class Airmode : int32_t {
disabled = 0,
roll_pitch = 1,
roll_pitch_yaw = 2
};
/**
* Fetch a control value.
*
* @param handle Token passed when the callback is registered.
* @param control_group The group to fetch the control from.
* @param control_index The group-relative index to fetch the control from.
* @param control The returned control
* @return Zero if the value was fetched, nonzero otherwise.
*/
typedef int (* ControlCallback)(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
/**
* Constructor.
*
* @param control_cb Callback invoked when reading controls.
*/
Mixer(ControlCallback control_cb, uintptr_t cb_handle) : _control_cb(control_cb), _cb_handle(cb_handle) {}
virtual ~Mixer() = default;
// no copy, assignment, move, move assignment
Mixer(const Mixer &) = delete;
Mixer &operator=(const Mixer &) = delete;
Mixer(Mixer &&) = delete;
Mixer &operator=(Mixer &&) = delete;
/**
* Perform the mixing function.
*
* @param outputs Array into which mixed output(s) should be placed.
* @param space The number of available entries in the output array;
* @return The number of entries in the output array that were populated.
*/
virtual unsigned mix(float *outputs, unsigned space) = 0;
/**
* Get the saturation status.
*
* @return Integer bitmask containing saturation_status from control_allocator_status.msg.
*/
virtual uint16_t get_saturation_status() { return 0; }
/**
* Analyses the mix configuration and updates a bitmask of groups
* that are required.
*
* @param groups A bitmask of groups (0-31) that the mixer requires.
*/
virtual void groups_required(uint32_t &groups) {};
/**
* @brief Empty method, only implemented for MultirotorMixer and MixerGroup class.
*
* @param[in] delta_out_max Maximum delta output.
*
*/
virtual void set_max_delta_out_once(float delta_out_max) {}
/**
* @brief Set trim offset for this mixer
*
* @return the number of outputs this mixer feeds to
*/
virtual unsigned set_trim(float trim) { return 0; }
/**
* @brief Get trim offset for this mixer
*
* @return the number of outputs this mixer feeds to
*/
virtual unsigned get_trim(float *trim) { return 0; }
/*
* @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output.
*
* @param[in] val The value
*/
virtual void set_thrust_factor(float val) {}
/**
* @brief Set airmode. Airmode allows the mixer to increase the total thrust in order to unsaturate the motors.
*
* @param[in] airmode Select airmode type (0 = disabled, 1 = roll/pitch, 2 = roll/pitch/yaw)
*/
virtual void set_airmode(Airmode airmode) {};
virtual unsigned get_multirotor_count() { return 0; }
virtual void set_dt_once(float dt) {}
protected:
/** client-supplied callback used when fetching control values */
ControlCallback _control_cb;
uintptr_t _cb_handle;
/**
* Invoke the client callback to fetch a control value.
*
* @param group Control group to fetch from.
* @param index Control index to fetch.
* @return The control value.
*/
float get_control(uint8_t group, uint8_t index);
/**
* Find a tag
*
* @param buf The buffer to operate on.
* @param buflen length of the buffer.
* @param tag character to search for.
*/
static const char *findtag(const char *buf, unsigned &buflen, char tag);
/**
* Find next tag and return it (0 is returned if no tag is found)
*
* @param buf The buffer to operate on.
* @param buflen length of the buffer.
*/
static char findnexttag(const char *buf, unsigned buflen);
/**
* Skip a line
*
* @param buf The buffer to operate on.
* @param buflen length of the buffer.
* @return 0 / OK if a line could be skipped, 1 else
*/
static const char *skipline(const char *buf, unsigned &buflen);
/**
* Check wether the string is well formed and suitable for parsing
*/
static bool string_well_formed(const char *buf, unsigned &buflen);
};
src/lib/mixer/MixerGroup.cpp
-253
View File
@@ -1,253 +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.
*
****************************************************************************/
/**
* @file mixer_group.cpp
*
* Mixer collection.
*/
#include "MixerGroup.hpp"
#include "HelicopterMixer/HelicopterMixer.hpp"
#include "MultirotorMixer/MultirotorMixer.hpp"
#include "NullMixer/NullMixer.hpp"
#include "SimpleMixer/SimpleMixer.hpp"
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
//#include <debug.h>
//#define debug(fmt, args...) syslog(fmt "\n", ##args)
unsigned
MixerGroup::mix(float *outputs, unsigned space)
{
unsigned index = 0;
for (auto mixer : _mixers) {
index += mixer->mix(outputs + index, space - index);
if (index >= space) {
break;
}
}
return index;
}
/*
* set_trims() has no effect except for the SimpleMixer implementation for which set_trim()
* always returns the value one.
* The only other existing implementation is MultirotorMixer, which ignores the trim value
* and returns _rotor_count.
*/
unsigned
MixerGroup::set_trims(int16_t *values, unsigned n)
{
unsigned index = 0;
for (auto mixer : _mixers) {
// convert from integer to float
// to be safe, clamp offset to range of [-500, 500] usec
float offset = math::constrain((float)values[index] / 10000, -1.0f, 1.0f);
debug("set trim: %d, offset: %5.3f", values[index], (double)offset);
index += mixer->set_trim(offset);
if (index >= n) {
break;
}
}
return index;
}
/*
* get_trims() has no effect except for the SimpleMixer implementation for which get_trim()
* always returns the value one and sets the trim value.
* The only other existing implementation is MultirotorMixer, which ignores the trim value
* and returns _rotor_count.
*/
unsigned
MixerGroup::get_trims(int16_t *values)
{
unsigned index_mixer = 0;
unsigned index = 0;
for (auto mixer : _mixers) {
float trim = 0;
index_mixer += mixer->get_trim(&trim);
// MultirotorMixer returns the number of motors so we
// loop through index_mixer and set the same trim value for all motors
while (index < index_mixer) {
values[index] = trim * 10000;
index++;
}
}
return index;
}
void
MixerGroup::set_thrust_factor(float val)
{
for (auto mixer : _mixers) {
mixer->set_thrust_factor(val);
}
}
void
MixerGroup::set_airmode(Mixer::Airmode airmode)
{
for (auto mixer : _mixers) {
mixer->set_airmode(airmode);
}
}
unsigned
MixerGroup::get_multirotor_count()
{
for (auto mixer : _mixers) {
unsigned rotor_count = mixer->get_multirotor_count();
if (rotor_count > 0) {
return rotor_count;
}
}
return 0;
}
uint16_t
MixerGroup::get_saturation_status()
{
uint16_t sat = 0;
for (auto mixer : _mixers) {
sat |= mixer->get_saturation_status();
}
return sat;
}
void
MixerGroup::groups_required(uint32_t &groups)
{
for (auto mixer : _mixers) {
mixer->groups_required(groups);
}
}
int
MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
{
int ret = -1;
const char *end = buf + buflen;
/*
* Loop until either we have emptied the buffer, or we have failed to
* allocate something when we expected to.
*/
while (buflen > 0) {
Mixer *m = nullptr;
const char *p = end - buflen;
unsigned resid = buflen;
/*
* Use the next character as a hint to decide which mixer class to construct.
*/
switch (*p) {
case 'Z':
m = NullMixer::from_text(p, resid);
break;
case 'M':
m = SimpleMixer::from_text(control_cb, cb_handle, p, resid);
break;
case 'R':
m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid);
break;
case 'H':
m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid);
break;
default:
/* it's probably junk or whitespace, skip a byte and retry */
buflen--;
continue;
}
/*
* If we constructed something, add it to the group.
*/
if (m != nullptr) {
add_mixer(m);
/* we constructed something */
ret = 0;
/* only adjust buflen if parsing was successful */
buflen = resid;
debug("SUCCESS - buflen: %d", buflen);
} else {
/*
* There is data in the buffer that we expected to parse, but it didn't,
* so give up for now.
*/
break;
}
}
/* nothing more in the buffer for us now */
return ret;
}
void MixerGroup::set_max_delta_out_once(float delta_out_max)
{
for (auto mixer : _mixers) {
mixer->set_max_delta_out_once(delta_out_max);
}
}
void
MixerGroup::set_dt_once(float dt)
{
for (auto mixer : _mixers) {
mixer->set_dt_once(dt);
}
}
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
@@ -1,498 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-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.
*
****************************************************************************/
/**
* @file mixer_multirotor.cpp
*
* Multi-rotor mixers.
*/
#include "MultirotorMixer.hpp"
#include <float.h>
#include <cstring>
#include <cstdio>
#include <mathlib/mathlib.h>
#ifdef MIXER_MULTIROTOR_USE_MOCK_GEOMETRY
enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {
QUAD_X,
MAX_GEOMETRY
};
namespace
{
const MultirotorMixer::Rotor _config_quad_x[] = {
{ -0.707107, 0.707107, 1.000000, 1.000000 },
{ 0.707107, -0.707107, 1.000000, 1.000000 },
{ 0.707107, 0.707107, -1.000000, 1.000000 },
{ -0.707107, -0.707107, -1.000000, 1.000000 },
};
const MultirotorMixer::Rotor *_config_index[] = {
&_config_quad_x[0]
};
const unsigned _config_rotor_count[] = {4};
const char *_config_key[] = {"4x"};
}
#else
// This file is generated by the px_generate_mixers.py script which is invoked during the build process
// #include "mixer_multirotor.generated.h"
#include "mixer_multirotor_normalized.generated.h"
#endif /* MIXER_MULTIROTOR_USE_MOCK_GEOMETRY */
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
//#include <debug.h>
//#define debug(fmt, args...) syslog(fmt "\n", ##args)
MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry) :
MultirotorMixer(control_cb, cb_handle, _config_index[(int)geometry], _config_rotor_count[(int)geometry])
{
}
MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors,
unsigned rotor_count) :
Mixer(control_cb, cb_handle),
_rotor_count(rotor_count),
_rotors(rotors),
_outputs_prev(new float[_rotor_count]),
_tmp_array(new float[_rotor_count])
{
for (unsigned i = 0; i < _rotor_count; ++i) {
_outputs_prev[i] = -1.f;
}
}
MultirotorMixer::~MultirotorMixer()
{
delete[] _outputs_prev;
delete[] _tmp_array;
}
MultirotorMixer *
MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
{
MultirotorGeometry geometry = MultirotorGeometry::MAX_GEOMETRY;
char geomname[16];
/* enforce that the mixer ends with a new line */
if (!string_well_formed(buf, buflen)) {
return nullptr;
}
if (sscanf(buf, "R: %15s", geomname) != 1) {
debug("multirotor parse failed on '%s'", buf);
return nullptr;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return nullptr;
}
debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
for (MultirotorGeometryUnderlyingType i = 0; i < (MultirotorGeometryUnderlyingType)MultirotorGeometry::MAX_GEOMETRY;
i++) {
if (!strcmp(geomname, _config_key[i])) {
geometry = (MultirotorGeometry)i;
break;
}
}
if (geometry == MultirotorGeometry::MAX_GEOMETRY) {
debug("unrecognised geometry '%s'", geomname);
return nullptr;
}
debug("adding multirotor mixer '%s'", geomname);
return new MultirotorMixer(control_cb, cb_handle, geometry);
}
float
MultirotorMixer::compute_desaturation_gain(const float *desaturation_vector, const float *outputs,
saturation_status &sat_status, float min_output, float max_output) const
{
float k_min = 0.f;
float k_max = 0.f;
for (unsigned i = 0; i < _rotor_count; i++) {
// Avoid division by zero. If desaturation_vector[i] is zero, there's nothing we can do to unsaturate anyway
if (fabsf(desaturation_vector[i]) < FLT_EPSILON) {
continue;
}
if (outputs[i] < min_output) {
float k = (min_output - outputs[i]) / desaturation_vector[i];
if (k < k_min) { k_min = k; }
if (k > k_max) { k_max = k; }
sat_status.flags.motor_neg = true;
}
if (outputs[i] > max_output) {
float k = (max_output - outputs[i]) / desaturation_vector[i];
if (k < k_min) { k_min = k; }
if (k > k_max) { k_max = k; }
sat_status.flags.motor_pos = true;
}
}
// Reduce the saturation as much as possible
return k_min + k_max;
}
void
MultirotorMixer::minimize_saturation(const float *desaturation_vector, float *outputs,
saturation_status &sat_status, float min_output, float max_output, bool reduce_only) const
{
float k1 = compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output);
if (reduce_only && k1 > 0.f) {
return;
}
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] += k1 * desaturation_vector[i];
}
// Compute the desaturation gain again based on the updated outputs.
// In most cases it will be zero. It won't be if max(outputs) - min(outputs) > max_output - min_output.
// In that case adding 0.5 of the gain will equilibrate saturations.
float k2 = 0.5f * compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output);
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] += k2 * desaturation_vector[i];
}
}
void
MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs)
{
// Airmode for roll and pitch, but not yaw
// Mix without yaw
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
thrust * _rotors[i].thrust_scale;
// Thrust will be used to unsaturate if needed
_tmp_array[i] = _rotors[i].thrust_scale;
}
minimize_saturation(_tmp_array, outputs, _saturation_status);
// Mix yaw independently
mix_yaw(yaw, outputs);
}
void
MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs)
{
// Airmode for roll, pitch and yaw
// Do full mixing
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale +
thrust * _rotors[i].thrust_scale;
// Thrust will be used to unsaturate if needed
_tmp_array[i] = _rotors[i].thrust_scale;
}
minimize_saturation(_tmp_array, outputs, _saturation_status);
// Unsaturate yaw (in case upper and lower bounds are exceeded)
// to prioritize roll/pitch over yaw.
for (unsigned i = 0; i < _rotor_count; i++) {
_tmp_array[i] = _rotors[i].yaw_scale;
}
minimize_saturation(_tmp_array, outputs, _saturation_status);
}
void
MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs)
{
// Airmode disabled: never allow to increase the thrust to unsaturate a motor
// Mix without yaw
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
thrust * _rotors[i].thrust_scale;
// Thrust will be used to unsaturate if needed
_tmp_array[i] = _rotors[i].thrust_scale;
}
// only reduce thrust
minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
// Reduce roll/pitch acceleration if needed to unsaturate
for (unsigned i = 0; i < _rotor_count; i++) {
_tmp_array[i] = _rotors[i].roll_scale;
}
minimize_saturation(_tmp_array, outputs, _saturation_status);
for (unsigned i = 0; i < _rotor_count; i++) {
_tmp_array[i] = _rotors[i].pitch_scale;
}
minimize_saturation(_tmp_array, outputs, _saturation_status);
// Mix yaw independently
mix_yaw(yaw, outputs);
}
void MultirotorMixer::mix_yaw(float yaw, float *outputs)
{
// Add yaw to outputs
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] += yaw * _rotors[i].yaw_scale;
// Yaw will be used to unsaturate if needed
_tmp_array[i] = _rotors[i].yaw_scale;
}
// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
// and allow some yaw response at maximum thrust
minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f);
for (unsigned i = 0; i < _rotor_count; i++) {
_tmp_array[i] = _rotors[i].thrust_scale;
}
// reduce thrust only
minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
}
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
if (space < _rotor_count) {
return 0;
}
float roll = math::constrain(get_control(0, 0), -1.0f, 1.0f);
float pitch = math::constrain(get_control(0, 1), -1.0f, 1.0f);
float yaw = math::constrain(get_control(0, 2), -1.0f, 1.0f);
float thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f);
// clean out class variable used to capture saturation
_saturation_status.value = 0;
// Do the mixing using the strategy given by the current Airmode configuration
switch (_airmode) {
case Airmode::roll_pitch:
mix_airmode_rp(roll, pitch, yaw, thrust, outputs);
break;
case Airmode::roll_pitch_yaw:
mix_airmode_rpy(roll, pitch, yaw, thrust, outputs);
break;
case Airmode::disabled:
default: // just in case: default to disabled
mix_airmode_disabled(roll, pitch, yaw, thrust, outputs);
break;
}
// Apply thrust model and scale outputs to range [idle_speed, 1].
// At this point the outputs are expected to be in [0, 1], but they can be outside, for example
// if a roll command exceeds the motor band limit.
for (unsigned i = 0; i < _rotor_count; i++) {
// Implement simple model for static relationship between applied motor pwm and motor thrust
// model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2
if (_thrust_factor > 0.0f) {
outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) *
(1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] /
_thrust_factor));
}
outputs[i] = math::constrain((2.f * outputs[i] - 1.f), -1.f, 1.f);
}
// Slew rate limiting and saturation checking
for (unsigned i = 0; i < _rotor_count; i++) {
bool clipping_high = false;
bool clipping_low_roll_pitch = false;
bool clipping_low_yaw = false;
// Check for saturation against static limits.
// We only check for low clipping if airmode is disabled (or yaw
// clipping if airmode==roll/pitch), since in all other cases thrust will
// be reduced or boosted and we can keep the integrators enabled, which
// leads to better tracking performance.
if (outputs[i] < -0.99f) {
if (_airmode == Airmode::disabled) {
clipping_low_roll_pitch = true;
clipping_low_yaw = true;
} else if (_airmode == Airmode::roll_pitch) {
clipping_low_yaw = true;
}
}
// check for saturation against slew rate limits
if (_delta_out_max > 0.0f) {
float delta_out = outputs[i] - _outputs_prev[i];
if (delta_out > _delta_out_max) {
outputs[i] = _outputs_prev[i] + _delta_out_max;
clipping_high = true;
} else if (delta_out < -_delta_out_max) {
outputs[i] = _outputs_prev[i] - _delta_out_max;
clipping_low_roll_pitch = true;
clipping_low_yaw = true;
}
}
_outputs_prev[i] = outputs[i];
// update the saturation status report
update_saturation_status(i, clipping_high, clipping_low_roll_pitch, clipping_low_yaw);
}
// this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen
_delta_out_max = 0.0f;
return _rotor_count;
}
/*
* This function update the control saturation status report using the following inputs:
*
* index: 0 based index identifying the motor that is saturating
* clipping_high: true if the motor demand is being limited in the positive direction
* clipping_low_roll_pitch: true if the motor demand is being limited in the negative direction (roll/pitch)
* clipping_low_yaw: true if the motor demand is being limited in the negative direction (yaw)
*/
void
MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch,
bool clipping_low_yaw)
{
// The motor is saturated at the upper limit
// check which control axes and which directions are contributing
if (clipping_high) {
if (_rotors[index].roll_scale > 0.0f) {
// A positive change in roll will increase saturation
_saturation_status.flags.roll_pos = true;
} else if (_rotors[index].roll_scale < 0.0f) {
// A negative change in roll will increase saturation
_saturation_status.flags.roll_neg = true;
}
// check if the pitch input is saturating
if (_rotors[index].pitch_scale > 0.0f) {
// A positive change in pitch will increase saturation
_saturation_status.flags.pitch_pos = true;
} else if (_rotors[index].pitch_scale < 0.0f) {
// A negative change in pitch will increase saturation
_saturation_status.flags.pitch_neg = true;
}
// check if the yaw input is saturating
if (_rotors[index].yaw_scale > 0.0f) {
// A positive change in yaw will increase saturation
_saturation_status.flags.yaw_pos = true;
} else if (_rotors[index].yaw_scale < 0.0f) {
// A negative change in yaw will increase saturation
_saturation_status.flags.yaw_neg = true;
}
// A positive change in thrust will increase saturation (Z neg is up)
_saturation_status.flags.thrust_neg = true;
}
// The motor is saturated at the lower limit
// check which control axes and which directions are contributing
if (clipping_low_roll_pitch) {
// check if the roll input is saturating
if (_rotors[index].roll_scale > 0.0f) {
// A negative change in roll will increase saturation
_saturation_status.flags.roll_neg = true;
} else if (_rotors[index].roll_scale < 0.0f) {
// A positive change in roll will increase saturation
_saturation_status.flags.roll_pos = true;
}
// check if the pitch input is saturating
if (_rotors[index].pitch_scale > 0.0f) {
// A negative change in pitch will increase saturation
_saturation_status.flags.pitch_neg = true;
} else if (_rotors[index].pitch_scale < 0.0f) {
// A positive change in pitch will increase saturation
_saturation_status.flags.pitch_pos = true;
}
// A negative change in thrust will increase saturation (Z pos is down)
_saturation_status.flags.thrust_pos = true;
}
if (clipping_low_yaw) {
// check if the yaw input is saturating
if (_rotors[index].yaw_scale > 0.0f) {
// A negative change in yaw will increase saturation
_saturation_status.flags.yaw_neg = true;
} else if (_rotors[index].yaw_scale < 0.0f) {
// A positive change in yaw will increase saturation
_saturation_status.flags.yaw_pos = true;
}
}
_saturation_status.flags.valid = true;
}
src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp
-255
View File
@@ -1,255 +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>
/**
* Supported multirotor geometries.
*
* Values are generated by the px_generate_mixers.py script and placed to mixer_multirotor_normalized.generated.h
*/
typedef uint8_t MultirotorGeometryUnderlyingType;
enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
/**
* Multi-rotor mixer for pre-defined vehicle geometries.
*
* Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
* a set of outputs based on the configured geometry.
*/
class MultirotorMixer : public Mixer
{
public:
/**
* Precalculated rotor mix.
*/
struct Rotor {
float roll_scale; /**< scales roll for this rotor */
float pitch_scale; /**< scales pitch for this rotor */
float yaw_scale; /**< scales yaw for this rotor */
float thrust_scale; /**< scales thrust for this rotor */
};
/**
* Constructor.
*
* @param control_cb Callback invoked to read inputs.
* @param cb_handle Passed to control_cb.
* @param geometry The selected geometry.
* @param roll_scale Scaling factor applied to roll inputs
* compared to thrust.
* @param pitch_scale Scaling factor applied to pitch inputs
* compared to thrust.
* @param yaw_wcale Scaling factor applied to yaw inputs compared
* to thrust.
* @param idle_speed Minimum rotor control output value; usually
* tuned to ensure that rotors never stall at the
* low end of their control range.
*/
MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry);
/**
* Constructor (for testing).
*
* @param control_cb Callback invoked to read inputs.
* @param cb_handle Passed to control_cb.
* @param rotors control allocation matrix
* @param rotor_count length of rotors array (= number of motors)
*/
MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count);
virtual ~MultirotorMixer();
// no copy, assignment, move, move assignment
MultirotorMixer(const MultirotorMixer &) = delete;
MultirotorMixer &operator=(const MultirotorMixer &) = delete;
MultirotorMixer(MultirotorMixer &&) = delete;
MultirotorMixer &operator=(MultirotorMixer &&) = 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 MultirotorMixer instance, or nullptr
* if the text format is bad.
*/
static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
unsigned &buflen);
unsigned mix(float *outputs, unsigned space) override;
uint16_t get_saturation_status() override { return _saturation_status.value; }
void groups_required(uint32_t &groups) override { groups |= (1 << 0); }
/**
* @brief Update slew rate parameter. This tells the multicopter mixer
* 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) override { _delta_out_max = delta_out_max; }
unsigned set_trim(float trim) override { return _rotor_count; }
unsigned get_trim(float *trim) override { return _rotor_count; }
/**
* @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) override { _thrust_factor = math::constrain(val, 0.0f, 1.0f); }
void set_airmode(Airmode airmode) override { _airmode = airmode; }
unsigned get_multirotor_count() override { return _rotor_count; }
union saturation_status {
struct {
uint16_t valid : 1; // 0 - true when the saturation status is used
uint16_t motor_pos : 1; // 1 - true when any motor has saturated in the positive direction
uint16_t motor_neg : 1; // 2 - true when any motor has saturated in the negative direction
uint16_t roll_pos : 1; // 3 - true when a positive roll demand change will increase saturation
uint16_t roll_neg : 1; // 4 - true when a negative roll demand change will increase saturation
uint16_t pitch_pos : 1; // 5 - true when a positive pitch demand change will increase saturation
uint16_t pitch_neg : 1; // 6 - true when a negative pitch demand change will increase saturation
uint16_t yaw_pos : 1; // 7 - true when a positive yaw demand change will increase saturation
uint16_t yaw_neg : 1; // 8 - true when a negative yaw demand change will increase saturation
uint16_t thrust_pos : 1; // 9 - true when a positive thrust demand change will increase saturation
uint16_t thrust_neg : 1; //10 - true when a negative thrust demand change will increase saturation
} flags;
uint16_t value;
};
private:
/**
* Computes the gain k by which desaturation_vector has to be multiplied
* in order to unsaturate the output that has the greatest saturation.
* @see also minimize_saturation().
*
* @return desaturation gain
*/
float compute_desaturation_gain(const float *desaturation_vector, const float *outputs, saturation_status &sat_status,
float min_output, float max_output) const;
/**
* Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
* desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
* acceleration on a specific axis.
* For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the
* saturation will be minimized by shifting the vertical thrust setpoint, without changing the
* roll/pitch/yaw accelerations.
*
* Note that as we only slide along the given axis, in extreme cases outputs can still contain values
* outside of [min_output, max_output].
*
* @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale
* @param outputs output vector that is modified
* @param sat_status saturation status output
* @param min_output minimum desired value in outputs
* @param max_output maximum desired value in outputs
* @param reduce_only if true, only allow to reduce (substract) a fraction of desaturation_vector
*/
void minimize_saturation(const float *desaturation_vector, float *outputs, saturation_status &sat_status,
float min_output = 0.f, float max_output = 1.f, bool reduce_only = false) const;
/**
* Mix roll, pitch, yaw, thrust and set the outputs vector.
*
* Desaturation behavior: airmode for roll/pitch:
* thrust is increased/decreased as much as required to meet the demanded roll/pitch.
* Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
*/
inline void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs);
/**
* Mix roll, pitch, yaw, thrust and set the outputs vector.
*
* Desaturation behavior: full airmode for roll/pitch/yaw:
* thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw,
* while giving priority to roll and pitch over yaw.
*/
inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs);
/**
* Mix roll, pitch, yaw, thrust and set the outputs vector.
*
* Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
* roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
* Thrust can be reduced to unsaturate the upper side.
* @see mix_yaw() for the exact yaw behavior.
*/
inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs);
/**
* Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust).
*
* Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
* some yaw control on the upper end. On the lower end thrust will never be increased,
* but yaw is decreased as much as required.
*
* @param yaw demanded yaw
* @param outputs output vector that is updated
*/
inline void mix_yaw(float yaw, float *outputs);
void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw);
float _delta_out_max{0.0f};
float _thrust_factor{0.0f};
Airmode _airmode{Airmode::disabled};
saturation_status _saturation_status{};
unsigned _rotor_count;
const Rotor *_rotors;
float *_outputs_prev{nullptr};
float *_tmp_array{nullptr};
};
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
@@ -1,398 +0,0 @@
#!/usr/bin/env python
#############################################################################
#
# Copyright (C) 2013-2016 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.
#
#############################################################################
"""
px_generate_mixers.py
Generates c/cpp header/source files for multirotor mixers
from geometry descriptions files (.toml format)
"""
import sys
try:
import toml
except ImportError as e:
print("Failed to import toml: " + str(e))
print("")
print("You may need to install it using:")
print(" pip3 install --user toml")
print("")
sys.exit(1)
try:
import numpy as np
except ImportError as e:
print("Failed to import numpy: " + str(e))
print("")
print("You may need to install it using:")
print(" pip3 install --user numpy")
print("")
sys.exit(1)
__author__ = "Julien Lecoeur"
__copyright__ = "Copyright (C) 2013-2017 PX4 Development Team."
__license__ = "BSD"
__email__ = "julien.lecoeur@gmail.com"
def parse_geometry_toml(filename):
'''
Parses toml geometry file and returns a dictionary with curated list of rotors
'''
import os
# Load toml file
d = toml.load(filename)
# Check info section
if 'info' not in d:
raise AttributeError('{}: Error, missing info section'.format(filename))
# Check info section
for field in ['key', 'description']:
if field not in d['info']:
raise AttributeError('{}: Error, unspecified info field "{}"'.format(filename, field))
# Use filename as mixer name
d['info']['name'] = os.path.basename(filename).split('.')[0].lower()
# Store filename
d['info']['filename'] = filename
# Check default rotor config
if 'rotor_default' in d:
default = d['rotor_default']
else:
default = {}
# Convert rotors
rotor_list = []
if 'rotors' in d:
for r in d['rotors']:
# Make sure all fields are defined, fill missing with default
for field in ['name', 'position', 'axis', 'direction', 'Ct', 'Cm']:
if field not in r:
if field in default:
r[field] = default[field]
else:
raise AttributeError('{}: Error, unspecified field "{}" for rotor "{}"'
.format(filename, field, r['name']))
# Check direction field
r['direction'] = r['direction'].upper()
if r['direction'] not in ['CW', 'CCW']:
raise AttributeError('{}: Error, invalid direction value "{}" for rotor "{}"'
.format(filename, r['direction'], r['name']))
# Check vector3 fields
for field in ['position', 'axis']:
if len(r[field]) != 3:
raise AttributeError('{}: Error, field "{}" for rotor "{}"'
.format(filename, field, r['name']) +
' must be an array of length 3')
# Add rotor to list
rotor_list.append(r)
# Clean dictionary
geometry = {'info': d['info'],
'rotors': rotor_list}
return geometry
def torque_matrix(center, axis, dirs, Ct, Cm):
'''
Compute torque generated by rotors
'''
# normalize rotor axis
ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis]
torque = Ct * np.cross(center, ax) - Cm * ax * dirs
return torque
def geometry_to_torque_matrix(geometry):
'''
Compute torque matrix Am and Bm from geometry dictionnary
Am is a 3xN matrix where N is the number of rotors
Each column is the torque generated by one rotor
'''
Am = torque_matrix(center=np.array([rotor['position'] for rotor in geometry['rotors']]),
axis=np.array([rotor['axis'] for rotor in geometry['rotors']]),
dirs=np.array([[1.0 if rotor['direction'] == 'CCW' else -1.0]
for rotor in geometry['rotors']]),
Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']]),
Cm=np.array([[rotor['Cm']] for rotor in geometry['rotors']])).T
return Am
def thrust_matrix(axis, Ct):
'''
Compute thrust generated by rotors
'''
# Normalize rotor axis
ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis]
thrust = Ct * ax
return thrust
def geometry_to_thrust_matrix(geometry):
'''
Compute thrust matrix At from geometry dictionnary
At is a 3xN matrix where N is the number of rotors
Each column is the thrust generated by one rotor
'''
At = thrust_matrix(axis=np.array([rotor['axis'] for rotor in geometry['rotors']]),
Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']])).T
return At
def geometry_to_mix(geometry):
'''
Compute combined torque & thrust matrix A and mix matrix B from geometry dictionnary
A is a 6xN matrix where N is the number of rotors
Each column is the torque and thrust generated by one rotor
B is a Nx6 matrix where N is the number of rotors
Each column is the command to apply to the servos to get
roll torque, pitch torque, yaw torque, x thrust, y thrust, z thrust
'''
# Combined torque & thrust matrix
At = geometry_to_thrust_matrix(geometry)
Am = geometry_to_torque_matrix(geometry)
A = np.vstack([Am, At])
# Mix matrix computed as pseudoinverse of A
B = np.linalg.pinv(A)
return A, B
def normalize_mix_px4(B):
'''
Normalize mix for PX4
This is for compatibility only and should ideally not be used
'''
B_norm = np.linalg.norm(B, axis=0)
B_max = np.abs(B).max(axis=0)
B_sum = np.sum(B, axis=0)
# Same scale on roll and pitch
B_norm[0] = max(B_norm[0], B_norm[1]) / np.sqrt(B.shape[0] / 2.0)
B_norm[1] = B_norm[0]
# Scale yaw separately
B_norm[2] = B_max[2]
# Same scale on x, y
B_norm[3] = max(B_max[3], B_max[4])
B_norm[4] = B_norm[3]
# Scale z thrust separately
B_norm[5] = - B_sum[5] / np.count_nonzero(B[:,5])
# Normalize
B_norm[np.abs(B_norm) < 1e-3] = 1
B_px = (B / B_norm)
return B_px
def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, use_6dof=False):
'''
Generate C header file with same format as multi_tables.py
TODO: rewrite using templates (see generation of uORB headers)
'''
from io import StringIO
buf = StringIO()
# Print Header
buf.write(u"/*\n")
buf.write(u"* This file is automatically generated by px_generate_mixers.py - do not edit.\n")
buf.write(u"*/\n")
buf.write(u"\n")
buf.write(u"#ifndef _MIXER_MULTI_TABLES\n")
buf.write(u"#define _MIXER_MULTI_TABLES\n")
buf.write(u"\n")
# Print enum
buf.write(u"enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {\n")
for i, geometry in enumerate(geometries_list):
buf.write(u"\t{},{}// {} (text key {})\n".format(
geometry['info']['name'].upper(), ' ' * (max(0, 30 - len(geometry['info']['name']))),
geometry['info']['description'], geometry['info']['key']))
buf.write(u"\n\tMAX_GEOMETRY\n")
buf.write(u"}; // enum class MultirotorGeometry\n\n")
# Print mixer gains
buf.write(u"namespace {\n")
for geometry in geometries_list:
# Get desired mix matrix
if use_normalized_mix:
mix = geometry['mix']['B_px']
else:
mix = geometry['mix']['B']
buf.write(u"static constexpr MultirotorMixer::Rotor _config_{}[] {{\n".format(geometry['info']['name']))
for row in mix:
if use_6dof:
# 6dof mixer
buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
row[0], row[1], row[2],
row[3], row[4], row[5]))
else:
# 4dof mixer
buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
row[0], row[1], row[2],
-row[5])) # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly
buf.write(u"};\n\n")
# Print geometry indeces
buf.write(u"static constexpr const MultirotorMixer::Rotor *_config_index[] {\n")
for geometry in geometries_list:
buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name']))
buf.write(u"};\n\n")
# Print geometry rotor counts
buf.write(u"static constexpr unsigned _config_rotor_count[] {\n")
for geometry in geometries_list:
buf.write(u"\t{}, /* {} */\n".format(len(geometry['rotors']), geometry['info']['name']))
buf.write(u"};\n\n")
# Print geometry key
buf.write(u"const char* _config_key[] {\n")
for geometry in geometries_list:
buf.write(u"\t\"{}\",\t/* {} */\n".format(geometry['info']['key'], geometry['info']['name']))
buf.write(u"};\n\n")
# Print footer
buf.write(u"} // anonymous namespace\n\n")
buf.write(u"#endif /* _MIXER_MULTI_TABLES */\n\n")
return buf.getvalue()
if __name__ == '__main__':
import argparse
import glob
import os
# Parse arguments
parser = argparse.ArgumentParser(
description='Convert geometry .toml files to mixer headers')
parser.add_argument('-d', dest='dir',
help='directory with geometry files')
parser.add_argument('-f', dest='files',
help="files to convert (use only without -d)",
nargs="+")
parser.add_argument('-o', dest='outputfile',
help='output header file')
parser.add_argument('--verbose', help='Print details on standard output',
action='store_true')
parser.add_argument('--normalize', help='Use normalized mixers (compatibility mode)',
action='store_true')
parser.add_argument('--sixdof', help='Use 6dof mixers',
action='store_true')
args = parser.parse_args()
# Find toml files
if args.files is not None:
filenames = args.files
elif args.dir is not None:
filenames = glob.glob(os.path.join(args.dir, '*.toml'))
else:
parser.print_usage()
raise Exception("Missing input directory (-d) or list of geometry files (-f)")
# List of geometries
geometries_list = []
for filename in filenames:
# Parse geometry file
geometry = parse_geometry_toml(filename)
# Compute torque and thrust matrices
A, B = geometry_to_mix(geometry)
# Normalize mixer
B_px = normalize_mix_px4(B)
# Store matrices in geometry
geometry['mix'] = {'A': A, 'B': B, 'B_px': B_px}
# Add to list
geometries_list.append(geometry)
if args.verbose:
print('\nFilename')
print(filename)
print('\nGeometry')
print(geometry)
print('\nA:')
print(A.round(2))
print('\nB:')
print(B.round(2))
print('\nNormalized Mix (as in PX4):')
print(B_px.round(2))
print('\n-----------------------------')
# Check that there are no duplicated mixer names or keys
for i in range(len(geometries_list)):
name_i = geometries_list[i]['info']['name']
key_i = geometries_list[i]['info']['key']
for j in range(i + 1, len(geometries_list)):
name_j = geometries_list[j]['info']['name']
key_j = geometries_list[j]['info']['key']
# Mixers cannot share the same name
if name_i == name_j:
raise ValueError('Duplicated mixer name "{}" in files {} and {}'.format(
name_i,
geometries_list[i]['info']['filename'],
geometries_list[j]['info']['filename']))
# Mixers cannot share the same key
if key_i == key_j:
raise ValueError('Duplicated mixer key "{}" for mixers "{}" and "{}"'.format(
key_i, name_i, name_j))
# Generate header file
header = generate_mixer_multirotor_header(geometries_list,
use_normalized_mix=args.normalize,
use_6dof=args.sixdof)
if args.outputfile is not None:
# Write header file
with open(args.outputfile, 'w') as fd:
fd.write(header)
else:
# Print to standard output
print(header)
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
@@ -1,389 +0,0 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Mixer multirotor test and prototyping script.
Author: Mathieu Bresciani <brescianimathieu@gmail.com>, Beat Kueng <beat-kueng@gmx.net>
Description: This script can be used to prototype new mixer algorithms and test
it against the C++ implementation.
"""
from __future__ import print_function
from argparse import ArgumentParser
import numpy as np
import numpy.matlib
import subprocess
# --------------------------------------------------
# mixing algorithms
# --------------------------------------------------
def compute_desaturation_gain(u, u_min, u_max, desaturation_vector):
"""
Computes the gain k by which desaturation_vector has to be multiplied
in order to unsaturate the output that has the greatest saturation
"""
d_u_sat_plus = u_max - u
d_u_sat_minus = u_min - u
k = np.zeros(u.size*2)
for i in range(u.size):
if abs(desaturation_vector[i]) < 0.000001:
# avoid division by zero
continue
if d_u_sat_minus[i] > 0.0:
k[2*i] = d_u_sat_minus[i] / desaturation_vector[i]
if d_u_sat_plus[i] < 0.0:
k[2*i+1] = d_u_sat_plus[i] / desaturation_vector[i]
k_min = min(k)
k_max = max(k)
# Reduce the saturation as much as possible
k = k_min + k_max
return k
def minimize_sat(u, u_min, u_max, desaturation_vector):
"""
Minimize the saturation of the actuators by
adding or substracting a fraction of desaturation_vector.
desaturation_vector is the vector that added to the output u,
modifies the thrust or angular acceleration on a
specific axis.
For example, if desaturation_vector is given
to slide along the vertical thrust axis, the saturation will
be minimized by shifting the vertical thrust setpoint,
without changing the roll/pitch/yaw accelerations.
"""
k_1 = compute_desaturation_gain(u, u_min, u_max, desaturation_vector)
u_1 = u + k_1 * desaturation_vector # Try to unsaturate
# Compute the desaturation gain again based on the updated outputs.
# In most cases it will be zero. It won't be if max(outputs) - min(outputs)
# > max_output - min_output.
# In that case adding 0.5 of the gain will equilibrate saturations.
k_2 = compute_desaturation_gain(u_1, u_min, u_max, desaturation_vector)
k_opt = k_1 + 0.5 * k_2
u_prime = u + k_opt * desaturation_vector
return u_prime
def mix_yaw(m_sp, u, P, u_min, u_max):
"""
Mix yaw by adding it to an existing output vector u
Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
some yaw control on the upper end. On the lower end thrust will never be increased,
but yaw is decreased as much as required.
"""
m_sp_yaw_only = np.matlib.zeros(m_sp.size).T
m_sp_yaw_only[2, 0] = m_sp[2, 0]
u_p = u + P * m_sp_yaw_only
# Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
# and allow some yaw response at maximum thrust
u_r_dot = P[:,2]
u_pp = minimize_sat(u_p, u_min, u_max+0.15, u_r_dot)
u_T = P[:, 3]
u_ppp = minimize_sat(u_pp, 0, u_max, u_T)
# reduce thrust only
if (u_ppp > (u_pp)).any():
u_ppp = u_pp
return u_ppp
def airmode_rp(m_sp, P, u_min, u_max):
"""
Mix roll, pitch, yaw and thrust.
Desaturation behavior: airmode for roll/pitch:
thrust is increased/decreased as much as required to meet the demanded roll/pitch.
Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
"""
# Mix without yaw
m_sp_no_yaw = m_sp.copy()
m_sp_no_yaw[2, 0] = 0.0
u = P * m_sp_no_yaw
# Use thrust to unsaturate the outputs if needed
u_T = P[:, 3]
u_prime = minimize_sat(u, u_min, u_max, u_T)
# Mix yaw axis independently
u_final = mix_yaw(m_sp, u_prime, P, u_min, u_max)
return (u, u_final)
def airmode_rpy(m_sp, P, u_min, u_max):
"""
Mix roll, pitch, yaw and thrust.
Desaturation behavior: full airmode for roll/pitch/yaw:
thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw.
"""
# Mix with yaw
u = P * m_sp
# Use thrust to unsaturate the outputs if needed
u_T = P[:, 3]
u_prime = minimize_sat(u, u_min, u_max, u_T)
# Unsaturate yaw (in case upper and lower bounds are exceeded)
# to prioritize roll/pitch over yaw.
u_T = P[:, 2]
u_prime_yaw = minimize_sat(u_prime, u_min, u_max, u_T)
return (u, u_prime_yaw)
def normal_mode(m_sp, P, u_min, u_max):
"""
Mix roll, pitch, yaw and thrust.
Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
Thrust can be reduced to unsaturate the upper side.
@see mix_yaw() for the exact yaw behavior.
"""
# Mix without yaw
m_sp_no_yaw = m_sp.copy()
m_sp_no_yaw[2, 0] = 0.0
u = P * m_sp_no_yaw
# Use thrust to unsaturate the outputs if needed
# by reducing the thrust only
u_T = P[:, 3]
u_prime = minimize_sat(u, u_min, u_max, u_T)
if (u_prime > (u)).any():
u_prime = u
# Reduce roll/pitch acceleration if needed to unsaturate
u_p_dot = P[:, 0]
u_p2 = minimize_sat(u_prime, u_min, u_max, u_p_dot)
u_q_dot = P[:, 1]
u_p3 = minimize_sat(u_p2, u_min, u_max, u_q_dot)
# Mix yaw axis independently
u_final = mix_yaw(m_sp, u_p3, P, u_min, u_max)
return (u, u_final)
# --------------------------------------------------
# test cases
# --------------------------------------------------
# normalized control allocation test matrices (B_px from px_generate_mixers.py)
# quad_x
P1 = np.matrix([
[-0.71, 0.71, 1., 1. ],
[ 0.71, -0.71, 1., 1. ],
[ 0.71, 0.71, -1., 1. ],
[-0.71, -0.71, -1., 1. ]])
# quad_wide
P2 = np.matrix([
[-0.5, 0.71, 0.77, 1. ],
[ 0.5, -0.71, 1., 1. ],
[ 0.5, 0.71, -0.77, 1. ],
[-0.5, -0.71, -1., 1. ]])
# hex_x
P3 = np.matrix([
[-1., 0., -1., 1. ],
[ 1., -0., 1., 1. ],
[ 0.5, 0.87, -1., 1. ],
[-0.5, -0.87, 1., 1. ],
[-0.5, 0.87, 1., 1. ],
[ 0.5, -0.87, -1., 1. ]])
# hex_cox
P4 = np.matrix([
[-0.87, 0.5, -1., 1. ],
[-0.87, 0.5, 1., 1. ],
[ 0., -1., -1., 1. ],
[ 0., -1., 1., 1. ],
[ 0.87, 0.5, -1., 1. ],
[ 0.87, 0.5, 1., 1. ]])
# octa_plus
P5 = np.matrix([
[-0., 1., -1., 1. ],
[ 0., -1., -1., 1. ],
[-0.71, 0.71, 1., 1. ],
[-0.71, -0.71, 1., 1. ],
[ 0.71, 0.71, 1., 1. ],
[ 0.71, -0.71, 1., 1. ],
[ 1., 0., -1., 1. ],
[-1., -0., -1., 1. ]])
P_tests = [ P1, P2, P3, P4, P5 ]
test_cases_input = np.matrix([
# desired accelerations (must be within [-1, 1]):
#roll pitch yaw thrust
[ 0.0, 0.0, 0.0, 0.0],
[-0.05, 0.0, 0.0, 0.0],
[ 0.05, -0.05, 0.0, 0.0],
[ 0.05, 0.05, -0.025, 0.0],
[ 0.0, 0.2, -0.025, 0.0],
[ 0.2, 0.05, 0.09, 0.0],
[-0.125, 0.02, 0.04, 0.0],
# extreme cases
[ 1.0, 0.0, 0.0, 0.0],
[ 0.0, -1.0, 0.0, 0.0],
[ 0.0, 0.0, 1.0, 0.0],
[ 1.0, 1.0, -1.0, 0.0],
[-1.0, 0.9, -0.9, 0.0],
[-1.0, 0.9, 0.0, 0.0],
])
# use the following thrust values for all test cases (must be within [0, 1])
thrust_values = [0, 0.1, 0.45, 0.9, 1.0]
test_cases = np.zeros((test_cases_input.shape[0] * len(thrust_values), 4))
for i in range(test_cases_input.shape[0]):
for k in range(len(thrust_values)):
test_case = test_cases_input[i]
test_case[0, 3] = thrust_values[k]
test_cases[i * len(thrust_values) + k, :] = test_case
def run_tests(mixer_cb, P, test_mixer_binary, test_index=None):
"""
Run all (or a specific) tests for a certain mixer method an control
allocation matrix P
"""
B = np.linalg.pinv(P)
proc = subprocess.Popen(
test_mixer_binary,
#'cat > /tmp/test_'+str(mode_idx), shell=True, # just to test the output
stdout=subprocess.PIPE,
stdin=subprocess.PIPE)
proc.stdin.write("{:}\n".format(mode_idx).encode('utf-8')) # airmode
motor_count = P.shape[0]
proc.stdin.write("{:}\n".format(motor_count).encode('utf-8')) # motor count
# control allocation matrix
for row in P.getA():
for col in row:
proc.stdin.write("{:.8f} ".format(col).encode('utf-8'))
proc.stdin.write("\n".encode('utf-8'))
proc.stdin.write("\n".encode('utf-8'))
failed = False
try:
if test_index is None:
# go through all test cases
test_indices = range(test_cases.shape[0])
else:
test_indices = [test_index]
for i in test_indices:
actuator_controls = test_cases[[i], :].T
proc.stdin.write("{:.8f} {:.8f} {:.8f} {:.8f}\n"
.format(actuator_controls[0, 0], actuator_controls[1, 0],
actuator_controls[2, 0], actuator_controls[3, 0]).encode('utf-8'))
(u, u_new) = mixer_cb(actuator_controls, P, 0.0, 1.0)
# Saturate the outputs between 0 and 1
u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T)
u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T)
# write expected outputs
for j in range(motor_count):
proc.stdin.write("{:.8f} ".format(u_new_sat[j, 0]).encode('utf-8'))
proc.stdin.write("\n".encode('utf-8'))
proc.stdin.close()
except IOError as e:
failed = True
result = proc.stdout.read()
proc.wait()
if proc.returncode != 0: failed = True
if failed:
print("Error: test failed")
print("B:\n{}".format(B))
print("P:\n{}".format(P))
print(result)
raise Exception('Test failed')
parser = ArgumentParser(description=__doc__)
parser.add_argument('--test', action='store_true', default=False, help='Run tests')
parser.add_argument("--mixer-multirotor-binary",
help="select test_mixer_multirotor binary file name",
default='./test_mixer_multirotor')
parser.add_argument("--mode", "-m", dest="mode",
help="mixer mode: none, rp, rpy", default=None)
parser.add_argument("-i", dest="index", type=int,
help="Select a single test to run (starting at 1)", default=None)
args = parser.parse_args()
mixer_mode = args.mode
if args.test:
mixer_binary = args.mixer_multirotor_binary
test_index = args.index
if test_index is not None: test_index -= 1
for mode_idx, (airmode, mixer_cb) in enumerate([
('none', normal_mode),
('rp', airmode_rp),
('rpy', airmode_rpy)]):
if mixer_mode is not None and mixer_mode != airmode:
continue
print('Testing mode: '+airmode)
for P in P_tests:
run_tests(mixer_cb, P, mixer_binary, test_index)
exit(0)
# --------------------------------------------------
# Prototyping and corner case testing playground
# --------------------------------------------------
# Compute the control allocation matrix
# u = P * m
P = P1 # normal quad
#P = P2 # wide quad
# Normalized actuator effectiveness matrix using the pseudo inverse of P
# m = B * u
B = np.linalg.pinv(P)
# Desired accelerations (actuator controls, in [-1, 1])
p_dot_sp = 0.0 # roll acceleration (p is the roll rate)
q_dot_sp = 0.1 # pitch acceleration
r_dot_sp = 0.1 # yaw acceleration
T_sp = 0.0 # vertical thrust
m_sp = np.matrix([p_dot_sp, q_dot_sp, r_dot_sp, T_sp]).T # Vector of desired "accelerations"
# Airmode type (none/rp/rpy)
airmode = mixer_mode
if airmode is None: airmode = "none"
# Actuators output saturations
u_max = 1.0
u_min = 0.0
if airmode == "none":
(u, u_new) = normal_mode(m_sp, P, u_min, u_max)
elif airmode == "rp":
(u, u_new) = airmode_rp(m_sp, P, u_min, u_max)
elif airmode == "rpy":
(u, u_new) = airmode_rpy(m_sp, P, u_min, u_max)
else:
u = 0.0
u_new = 0.0
# Saturate the outputs between 0 and 1
u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T)
u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T)
np.set_printoptions(suppress=True)
# Display some results
print("u = \n{}\n".format(u))
print("u_new = \n{}\n".format(u_new))
print("u_new_sat = \n{}\n".format(u_new_sat))
print("Desired accelerations = \n{}\n".format(m_sp))
# Compute back the allocated accelerations
m_new = B * u_new_sat
print("Allocated accelerations = \n{}\n".format(m_new))
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
@@ -1,390 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-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.
*
****************************************************************************/
/**
* @file mixer_simple.cpp
*
* Simple summing mixer.
*/
#include "SimpleMixer.hpp"
#include <stdio.h>
#include <stdlib.h>
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo) :
Mixer(control_cb, cb_handle),
_pinfo(mixinfo)
{
}
SimpleMixer::~SimpleMixer()
{
if (_pinfo != nullptr) {
free(_pinfo);
}
}
unsigned SimpleMixer::set_trim(float trim)
{
_pinfo->output_scaler.offset = trim;
return 1;
}
unsigned SimpleMixer::get_trim(float *trim)
{
*trim = _pinfo->output_scaler.offset;
return 1;
}
void
SimpleMixer::set_dt_once(float dt)
{
_dt = dt;
}
int
SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time)
{
int ret;
int s[6];
int n = -1;
buf = findtag(buf, buflen, 'O');
if ((buf == nullptr) || (buflen < 12)) {
debug("output parser failed finding tag, ret: '%s'", buf);
return -1;
}
if ((ret = sscanf(buf, "O: %d %d %d %d %d %d %n",
&s[0], &s[1], &s[2], &s[3], &s[4], &s[5], &n)) < 5) {
debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
return -1;
}
// set slew rate limit to 0 if no 6th number is specified in mixer file
if (ret == 5) {
s[5] = 0;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return -1;
}
scaler.negative_scale = s[0] / 10000.0f;
scaler.positive_scale = s[1] / 10000.0f;
scaler.offset = s[2] / 10000.0f;
scaler.min_output = s[3] / 10000.0f;
scaler.max_output = s[4] / 10000.0f;
slew_rate_rise_time = s[5] / 10000.0f;
return 0;
}
int
SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
uint8_t &control_index)
{
unsigned u[2];
int s[5];
buf = findtag(buf, buflen, 'S');
if ((buf == nullptr) || (buflen < 16)) {
debug("control parser failed finding tag, ret: '%s'", buf);
return -1;
}
if (sscanf(buf, "S: %u %u %d %d %d %d %d",
&u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
debug("control parse failed on '%s'", buf);
return -1;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return -1;
}
control_group = u[0];
control_index = u[1];
scaler.negative_scale = s[0] / 10000.0f;
scaler.positive_scale = s[1] / 10000.0f;
scaler.offset = s[2] / 10000.0f;
scaler.min_output = s[3] / 10000.0f;
scaler.max_output = s[4] / 10000.0f;
return 0;
}
SimpleMixer *
SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
{
SimpleMixer *sm = nullptr;
mixer_simple_s *mixinfo = nullptr;
unsigned inputs;
int used;
const char *end = buf + buflen;
char next_tag;
/* enforce that the mixer ends with a new line */
if (!string_well_formed(buf, buflen)) {
return nullptr;
}
/* get the base info for the mixer */
if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
debug("simple parse failed on '%s'", buf);
goto out;
}
/* at least 1 input is required */
if (inputs == 0) {
debug("simple parse got 0 inputs");
goto out;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
goto out;
}
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
if (mixinfo == nullptr) {
debug("could not allocate memory for mixer info");
goto out;
}
mixinfo->control_count = inputs;
/* find the next tag */
next_tag = findnexttag(end - buflen, buflen);
if (next_tag == 'S') {
/* No output scalers specified. Use default values.
* Corresponds to:
* O: 10000 10000 0 -10000 10000 0
*/
mixinfo->output_scaler.negative_scale = 1.0f;
mixinfo->output_scaler.positive_scale = 1.0f;
mixinfo->output_scaler.offset = 0.f;
mixinfo->output_scaler.min_output = -1.0f;
mixinfo->output_scaler.max_output = 1.0f;
mixinfo->slew_rate_rise_time = 0.0f;
} else {
if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler, mixinfo->slew_rate_rise_time)) {
debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
goto out;
}
}
for (unsigned i = 0; i < inputs; i++) {
if (parse_control_scaler(end - buflen, buflen,
mixinfo->controls[i].scaler,
mixinfo->controls[i].control_group,
mixinfo->controls[i].control_index)) {
debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
goto out;
}
}
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
if (sm != nullptr) {
mixinfo = nullptr;
debug("loaded mixer with %d input(s)", inputs);
} else {
debug("could not allocate memory for mixer");
}
out:
if (mixinfo != nullptr) {
free(mixinfo);
}
return sm;
}
unsigned
SimpleMixer::mix(float *outputs, unsigned space)
{
float sum = 0.0f;
if (_pinfo == nullptr) {
return 0;
}
if (space < 1) {
return 0;
}
for (unsigned i = 0; i < _pinfo->control_count; i++) {
float input = 0.0f;
_control_cb(_cb_handle,
_pinfo->controls[i].control_group,
_pinfo->controls[i].control_index,
input);
sum += scale(_pinfo->controls[i].scaler, input);
}
*outputs = scale(_pinfo->output_scaler, sum);
if (_dt > FLT_EPSILON && _pinfo->slew_rate_rise_time > FLT_EPSILON) {
// factor 2 is needed because actuator outputs are in the range [-1,1]
const float output_delta_max = 2.0f * _dt / _pinfo->slew_rate_rise_time;
float delta_out = *outputs - _output_prev;
if (delta_out > output_delta_max) {
*outputs = _output_prev + output_delta_max;
} else if (delta_out < -output_delta_max) {
*outputs = _output_prev - output_delta_max;
}
}
// this will force the caller of the mixer to always supply dt values, otherwise no slew rate limiting will happen
_dt = 0.f;
_output_prev = *outputs;
return 1;
}
void
SimpleMixer::groups_required(uint32_t &groups)
{
for (unsigned i = 0; i < _pinfo->control_count; i++) {
groups |= 1 << _pinfo->controls[i].control_group;
}
}
int
SimpleMixer::check()
{
float junk;
/* sanity that presumes that a mixer includes a control no more than once */
/* max of 32 groups due to groups_required API */
if (_pinfo->control_count > 32) {
return -2;
}
/* validate the output scaler */
int ret = scale_check(_pinfo->output_scaler);
if (ret != 0) {
return ret;
}
/* validate input scalers */
for (unsigned i = 0; i < _pinfo->control_count; i++) {
/* verify that we can fetch the control */
if (_control_cb(_cb_handle,
_pinfo->controls[i].control_group,
_pinfo->controls[i].control_index,
junk) != 0) {
return -3;
}
/* validate the scaler */
ret = scale_check(_pinfo->controls[i].scaler);
if (ret != 0) {
return (10 * i + ret);
}
}
return 0;
}
float
SimpleMixer::scale(const mixer_scaler_s &scaler, float input)
{
float output;
if (input < 0.0f) {
output = (input * scaler.negative_scale) + scaler.offset;
} else {
output = (input * scaler.positive_scale) + scaler.offset;
}
return math::constrain(output, scaler.min_output, scaler.max_output);
}
int
SimpleMixer::scale_check(mixer_scaler_s &scaler)
{
if (scaler.offset > 1.001f) {
return 1;
}
if (scaler.offset < -1.001f) {
return 2;
}
if (scaler.min_output > scaler.max_output) {
return 3;
}
if (scaler.min_output < -1.001f) {
return 4;
}
if (scaler.max_output > 1.001f) {
return 5;
}
return 0;
}
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
@@ -33,8 +33,6 @@
#include "mixer_module.hpp"
#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
#include <uORB/Publication.hpp>
#include <px4_platform_common/log.h>
@@ -68,21 +66,15 @@ static const FunctionProvider all_function_providers[] = {
};
MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface,
SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up)
: ModuleParams(&interface),
_output_ramp_up(ramp_up),
_control_subs{
{&interface, ORB_ID(actuator_controls_0)},
{&interface, ORB_ID(actuator_controls_1)},
{&interface, ORB_ID(actuator_controls_2)},
{&interface, ORB_ID(actuator_controls_3)},
},
_scheduling_policy(scheduling_policy),
_support_esc_calibration(support_esc_calibration),
_max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS),
_interface(interface),
_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")),
_param_prefix(param_prefix)
SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up) :
ModuleParams(&interface),
_output_ramp_up(ramp_up),
_scheduling_policy(scheduling_policy),
_support_esc_calibration(support_esc_calibration),
_max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS),
_interface(interface),
_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")),
_param_prefix(param_prefix)
{
/* Safely initialize armed flags */
_armed.armed = false;
@@ -94,28 +86,20 @@ _param_prefix(param_prefix)
px4_sem_init(&_lock, 0, 1);
_use_dynamic_mixing = _param_sys_ctrl_alloc.get();
initParamHandles();
if (_use_dynamic_mixing) {
initParamHandles();
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_failsafe_value[i] = UINT16_MAX;
}
updateParams();
} else {
_control_allocator_status_pub.advertise();
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_failsafe_value[i] = UINT16_MAX;
}
updateParams();
_outputs_pub.advertise();
}
MixingOutput::~MixingOutput()
{
perf_free(_control_latency_perf);
delete _mixers;
px4_sem_destroy(&_lock);
cleanupFunctions();
@@ -153,28 +137,12 @@ void MixingOutput::printStatus() const
PX4_INFO("Switched to rate_ctrl work queue");
}
if (!_use_dynamic_mixing) {
PX4_INFO("Mixer loaded: %s", _mixers ? "yes" : "no");
PX4_INFO("Driver instance: %i", _driver_instance);
}
PX4_INFO_RAW("Channel Configuration:\n");
if (_use_dynamic_mixing) {
for (unsigned i = 0; i < _max_num_outputs; i++) {
PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i,
(int)_function_assignment[i], _current_output_value[i],
actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i]);
}
} else {
for (unsigned i = 0; i < _max_num_outputs; i++) {
int reordered_i = reorderedMotorIndex(i);
PX4_INFO_RAW("Channel %i: value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", reordered_i,
_current_output_value[i],
_failsafe_value[reordered_i], _disarmed_value[reordered_i], _min_value[reordered_i], _max_value[reordered_i]);
}
for (unsigned i = 0; i < _max_num_outputs; i++) {
PX4_INFO_RAW("Channel %i: func: %3i, value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d\n", i,
(int)_function_assignment[i], _current_output_value[i],
actualFailsafeValue(i), _disarmed_value[i], _min_value[i], _max_value[i]);
}
}
@@ -182,154 +150,54 @@ void MixingOutput::updateParams()
{
ModuleParams::updateParams();
// update mixer if we have one
if (_mixers) {
if (_param_mot_slew_max.get() <= FLT_EPSILON) {
_mixers->set_max_delta_out_once(0.f);
_param_mot_ordering.set(0); // not used with dynamic mixing
bool function_changed = false;
for (unsigned i = 0; i < _max_num_outputs; i++) {
int32_t val;
if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &val) == 0) {
if (val != (int32_t)_function_assignment[i]) {
function_changed = true;
}
// we set _function_assignment[i] later to ensure _functions[i] is updated at the same time
}
_mixers->set_thrust_factor(_param_thr_mdl_fac.get());
_mixers->set_airmode((Mixer::Airmode)_param_mc_airmode.get());
}
if (_use_dynamic_mixing) {
_param_mot_ordering.set(0); // not used with dynamic mixing
bool function_changed = false;
for (unsigned i = 0; i < _max_num_outputs; i++) {
int32_t val;
if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &val) == 0) {
if (val != (int32_t)_function_assignment[i]) {
function_changed = true;
}
// we set _function_assignment[i] later to ensure _functions[i] is updated at the same time
}
if (_param_handles[i].disarmed != PARAM_INVALID && param_get(_param_handles[i].disarmed, &val) == 0) {
_disarmed_value[i] = val;
}
if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &val) == 0) {
_min_value[i] = val;
}
if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) {
_max_value[i] = val;
}
if (_min_value[i] > _max_value[i]) {
uint16_t tmp = _min_value[i];
_min_value[i] = _max_value[i];
_max_value[i] = tmp;
}
if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) {
_failsafe_value[i] = val;
}
if (_param_handles[i].disarmed != PARAM_INVALID && param_get(_param_handles[i].disarmed, &val) == 0) {
_disarmed_value[i] = val;
}
_reverse_output_mask = 0;
int32_t rev_range_param;
if (_param_handle_rev_range != PARAM_INVALID && param_get(_param_handle_rev_range, &rev_range_param) == 0) {
_reverse_output_mask = rev_range_param;
if (_param_handles[i].min != PARAM_INVALID && param_get(_param_handles[i].min, &val) == 0) {
_min_value[i] = val;
}
if (function_changed) {
_need_function_update = true;
}
}
}
bool MixingOutput::updateSubscriptions(bool allow_wq_switch, bool limit_callbacks_to_primary)
{
if (_use_dynamic_mixing) {
return updateSubscriptionsDynamicMixer(allow_wq_switch, limit_callbacks_to_primary);
} else {
return updateSubscriptionsStaticMixer(allow_wq_switch, limit_callbacks_to_primary);
}
}
bool MixingOutput::updateSubscriptionsStaticMixer(bool allow_wq_switch, bool limit_callbacks_to_primary)
{
if (_groups_subscribed == _groups_required) {
return false;
}
// must be locked to potentially change WorkQueue
lock();
if (_scheduling_policy == SchedulingPolicy::Auto) {
// first clear everything
unregister();
_interface.ScheduleClear();
// if subscribed to control group 0 or 1 then move to the rate_ctrl WQ
const bool sub_group_0 = (_groups_required & (1 << 0));
const bool sub_group_1 = (_groups_required & (1 << 1));
if (allow_wq_switch && !_wq_switched && (sub_group_0 || sub_group_1)) {
if (_interface.ChangeWorkQueue(px4::wq_configurations::rate_ctrl)) {
// let the new WQ handle the subscribe update
_wq_switched = true;
_interface.ScheduleNow();
unlock();
return false;
}
if (_param_handles[i].max != PARAM_INVALID && param_get(_param_handles[i].max, &val) == 0) {
_max_value[i] = val;
}
bool sub_group_0_callback_registered = false;
bool sub_group_1_callback_registered = false;
// register callback to all required actuator control groups
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (limit_callbacks_to_primary) {
// don't register additional callbacks if actuator_controls_0 or actuator_controls_1 are already registered
if ((i > 1) && (sub_group_0_callback_registered || sub_group_1_callback_registered)) {
break;
}
}
if (_groups_required & (1 << i)) {
if (_control_subs[i].registerCallback()) {
PX4_DEBUG("subscribed to actuator_controls_%d", i);
if (limit_callbacks_to_primary) {
if (i == 0) {
sub_group_0_callback_registered = true;
} else if (i == 1) {
sub_group_1_callback_registered = true;
}
}
} else {
PX4_ERR("actuator_controls_%d register callback failed!", i);
}
}
if (_min_value[i] > _max_value[i]) {
uint16_t tmp = _min_value[i];
_min_value[i] = _max_value[i];
_max_value[i] = tmp;
}
// if nothing required keep periodic schedule (so the module can update other things)
if (_groups_required == 0) {
// TODO: this might need to be configurable depending on the module
_interface.ScheduleOnInterval(100_ms);
if (_param_handles[i].failsafe != PARAM_INVALID && param_get(_param_handles[i].failsafe, &val) == 0) {
_failsafe_value[i] = val;
}
}
_groups_subscribed = _groups_required;
setMaxTopicUpdateRate(_max_topic_update_interval_us);
_reverse_output_mask = 0;
int32_t rev_range_param;
PX4_DEBUG("_groups_required 0x%08" PRIx32, _groups_required);
PX4_DEBUG("_groups_subscribed 0x%08" PRIx32, _groups_subscribed);
if (_param_handle_rev_range != PARAM_INVALID && param_get(_param_handle_rev_range, &rev_range_param) == 0) {
_reverse_output_mask = rev_range_param;
}
unlock();
return true;
if (function_changed) {
_need_function_update = true;
}
}
void MixingOutput::cleanupFunctions()
@@ -346,7 +214,7 @@ void MixingOutput::cleanupFunctions()
}
}
bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool limit_callbacks_to_primary)
bool MixingOutput::updateSubscriptions(bool allow_wq_switch, bool limit_callbacks_to_primary)
{
if (!_need_function_update || _armed.armed) {
return false;
@@ -491,17 +359,8 @@ void MixingOutput::setMaxTopicUpdateRate(unsigned max_topic_update_interval_us)
{
_max_topic_update_interval_us = max_topic_update_interval_us;
if (_use_dynamic_mixing) {
if (_subscription_callback) {
_subscription_callback->set_interval_us(_max_topic_update_interval_us);
}
} else {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_groups_subscribed & (1 << i)) {
_control_subs[i].set_interval_us(_max_topic_update_interval_us);
}
}
if (_subscription_callback) {
_subscription_callback->set_interval_us(_max_topic_update_interval_us);
}
}
@@ -539,45 +398,18 @@ void MixingOutput::setAllDisarmedValues(uint16_t value)
void MixingOutput::unregister()
{
for (auto &control_sub : _control_subs) {
control_sub.unregisterCallback();
}
if (_subscription_callback) {
_subscription_callback->unregisterCallback();
}
}
void MixingOutput::updateOutputSlewrateMultirotorMixer()
{
const hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain((now - _time_last_dt_update_multicopter) / 1e6f, 0.0001f, 0.02f);
_time_last_dt_update_multicopter = now;
// maximum value the outputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator outputs are in the range [-1,1]
const float delta_out_max = 2.0f * 1000.0f * dt / (_max_value[0] - _min_value[0]) / _param_mot_slew_max.get();
_mixers->set_max_delta_out_once(delta_out_max);
}
void MixingOutput::updateOutputSlewrateSimplemixer()
{
const hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain((now - _time_last_dt_update_simple_mixer) / 1e6f, 0.0001f, 0.02f);
_time_last_dt_update_simple_mixer = now;
// set dt for slew rate limiter in SimpleMixer (is reset internally after usig it, so needs to be set on every update)
_mixers->set_dt_once(dt);
}
unsigned MixingOutput::motorTest()
{
test_motor_s test_motor;
bool had_update = false;
while (_motor_test.test_motor_sub.update(&test_motor)) {
if (test_motor.driver_instance != _driver_instance ||
if (test_motor.driver_instance != 0 ||
test_motor.timestamp == 0 ||
hrt_elapsed_time(&test_motor.timestamp) > 100_ms) {
continue;
@@ -634,123 +466,6 @@ unsigned MixingOutput::motorTest()
}
bool MixingOutput::update()
{
if (_use_dynamic_mixing) {
return updateDynamicMixer();
} else {
return updateStaticMixer();
}
}
bool MixingOutput::updateStaticMixer()
{
if (!_mixers) {
// do nothing until we have a valid mixer
return false;
}
// check arming state
if (_armed_sub.update(&_armed)) {
_armed.in_esc_calibration_mode &= _support_esc_calibration;
if (_ignore_lockdown) {
_armed.lockdown = false;
}
/* Update the armed status and check that we're not locked down.
* We also need to arm throttle for the ESC calibration. */
_throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode;
if (_armed.armed) {
_motor_test.in_test_mode = false;
}
}
if (_param_mot_slew_max.get() > FLT_EPSILON) {
updateOutputSlewrateMultirotorMixer();
}
updateOutputSlewrateSimplemixer(); // update dt for output slew rate in simple mixer
unsigned n_updates = 0;
/* get controls for required topics */
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_groups_subscribed & (1 << i)) {
if (_control_subs[i].copy(&_controls[i])) {
n_updates++;
}
/* During ESC calibration, we overwrite the throttle value. */
if (i == 0 && _armed.in_esc_calibration_mode) {
/* Set all controls to 0 */
memset(&_controls[i], 0, sizeof(_controls[i]));
/* except thrust to maximum. */
_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
/* Switch off the output limit ramp for the calibration. */
_output_state = OutputLimitState::ON;
}
}
}
// check for motor test (after topic updates)
if (!_armed.armed && !_armed.manual_lockdown) {
unsigned num_motor_test = motorTest();
if (num_motor_test > 0) {
if (_interface.updateOutputs(false, _current_output_value, num_motor_test, 1)) {
actuator_outputs_s actuator_outputs{};
setAndPublishActuatorOutputs(num_motor_test, actuator_outputs);
}
return true;
}
}
/* do mixing */
float outputs[MAX_ACTUATORS] {};
const unsigned mixed_num_outputs = _mixers->mix(outputs, _max_num_outputs);
/* the output limit call takes care of out of band errors, NaN and constrains */
output_limit_calc(_throttle_armed, mixed_num_outputs, outputs);
/* overwrite outputs in case of force_failsafe with _failsafe_value values */
if (_armed.force_failsafe) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
_current_output_value[i] = _failsafe_value[i];
}
}
bool stop_motors = mixed_num_outputs == 0 || !_throttle_armed;
/* overwrite outputs in case of lockdown with disarmed values */
if (_armed.lockdown || _armed.manual_lockdown) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
_current_output_value[i] = _disarmed_value[i];
}
stop_motors = true;
}
/* apply _param_mot_ordering */
reorderOutputs(_current_output_value);
/* now return the outputs to the driver */
if (_interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates)) {
actuator_outputs_s actuator_outputs{};
setAndPublishActuatorOutputs(mixed_num_outputs, actuator_outputs);
publishMixerStatus(actuator_outputs);
updateLatencyPerfCounter(actuator_outputs);
}
return true;
}
bool MixingOutput::updateDynamicMixer()
{
// check arming state
if (_armed_sub.update(&_armed)) {
@@ -1019,84 +734,15 @@ MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_output
_outputs_pub.publish(actuator_outputs);
}
void
MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
{
MultirotorMixer::saturation_status saturation_status;
saturation_status.value = _mixers->get_saturation_status();
if (saturation_status.flags.valid) {
control_allocator_status_s sat{};
sat.timestamp = hrt_absolute_time();
sat.torque_setpoint_achieved = true;
sat.thrust_setpoint_achieved = true;
// Note: the values '-1', '1' and '0' are just to indicate a negative,
// positive or no saturation to the rate controller. The actual magnitude
// is not used.
if (saturation_status.flags.roll_pos) {
sat.unallocated_torque[0] = 1.f;
sat.torque_setpoint_achieved = false;
} else if (saturation_status.flags.roll_neg) {
sat.unallocated_torque[0] = -1.f;
sat.torque_setpoint_achieved = false;
}
if (saturation_status.flags.pitch_pos) {
sat.unallocated_torque[1] = 1.f;
sat.torque_setpoint_achieved = false;
} else if (saturation_status.flags.pitch_neg) {
sat.unallocated_torque[1] = -1.f;
sat.torque_setpoint_achieved = false;
}
if (saturation_status.flags.yaw_pos) {
sat.unallocated_torque[2] = 1.f;
sat.torque_setpoint_achieved = false;
} else if (saturation_status.flags.yaw_neg) {
sat.unallocated_torque[2] = -1.f;
sat.torque_setpoint_achieved = false;
}
if (saturation_status.flags.thrust_pos) {
sat.unallocated_thrust[2] = 1.f;
sat.thrust_setpoint_achieved = false;
} else if (saturation_status.flags.thrust_neg) {
sat.unallocated_thrust[2] = -1.f;
sat.thrust_setpoint_achieved = false;
}
_control_allocator_status_pub.publish(sat);
}
}
void
MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs)
{
if (_use_dynamic_mixing) {
// Just check the first function. It means we only get the latency if motors are assigned first, which is the default
if (_function_allocated[0]) {
hrt_abstime timestamp_sample;
// Just check the first function. It means we only get the latency if motors are assigned first, which is the default
if (_function_allocated[0]) {
hrt_abstime timestamp_sample;
if (_function_allocated[0]->getLatestSampleTimestamp(timestamp_sample)) {
perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample);
}
}
} else {
// use first valid timestamp_sample for latency tracking
for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
const bool required = _groups_required & (1 << i);
const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
if (required && (timestamp_sample > 0)) {
perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample);
break;
}
if (_function_allocated[0]->getLatestSampleTimestamp(timestamp_sample)) {
perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample);
}
}
}
@@ -1104,10 +750,6 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output
uint16_t
MixingOutput::actualFailsafeValue(int index) const
{
if (!_use_dynamic_mixing) {
return _failsafe_value[index];
}
uint16_t value = 0;
if (_failsafe_value[index] == UINT16_MAX) { // if set to default, use the one provided by the function
@@ -1126,34 +768,6 @@ MixingOutput::actualFailsafeValue(int index) const
return value;
}
void
MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
{
if (MAX_ACTUATORS < 4) {
return;
}
if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
/*
* Betaflight default motor ordering:
* 4 2
* ^
* 3 1
*/
const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3] };
values[0] = value_tmp[3];
values[1] = value_tmp[0];
values[2] = value_tmp[1];
values[3] = value_tmp[2];
}
/* else: PX4, no need to reorder
* 3 1
* ^
* 2 4
*/
}
int MixingOutput::reorderedMotorIndex(int index) const
{
if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
@@ -1170,37 +784,3 @@ int MixingOutput::reorderedMotorIndex(int index) const
return index;
}
int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
{
const MixingOutput *output = (const MixingOutput *)handle;
input = output->_controls[control_group].control[control_index];
/* limit control input */
input = math::constrain(input, -1.f, 1.f);
/* motor spinup phase - lock throttle to zero */
if (output->_output_state == OutputLimitState::RAMP) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* limit the throttle output to zero during motor spinup,
* as the motors cannot follow any demand yet
*/
input = 0.0f;
}
}
/* throttle not arming - mark throttle input as invalid */
if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* set the throttle to an invalid value */
input = NAN;
}
}
return 0;
}
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};