dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mixer: move MixerGroup out of Mixer hierarchy and delete mixer.h header

This commit is contained in:
Daniel Agar 2019-11-20 21:26:43 -05:00
parent 011186b8a5
commit 79dc676c8f
36 changed files with 233 additions and 352 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp
View File

@ -56,7 +56,7 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/esc_status.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <lib/mixer/mixer_load.h>
#include <battery/battery.h>
@ -307,7 +307,7 @@ int initialize_mixers(const char *mixers_filename)
}
if (_mixers == nullptr) {
_mixers = new MixerGroup(mixers_control_callback, (uintptr_t)_controls);
_mixers = new MixerGroup();
}
if (_mixers == nullptr) {
@ -315,7 +315,7 @@ int initialize_mixers(const char *mixers_filename)
return -1;
} else {
int ret = _mixers->load_from_buf(buf, buflen);
int ret = _mixers->load_from_buf(mixers_control_callback, (uintptr_t)_controls, buf, buflen);
if (ret != 0) {
PX4_ERR("Unable to parse mixers file");

6
src/drivers/linux_pwm_out/linux_pwm_out.cpp
View File

@ -51,7 +51,7 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <lib/mixer/mixer_load.h>
#include <parameters/param.h>
#include <output_limit/output_limit.h>
@ -155,12 +155,12 @@ int initialize_mixer(const char *mixer_filename)
unsigned buflen = sizeof(buf);
memset(buf, '\0', buflen);
_mixer_group = new MixerGroup(mixer_control_callback, (uintptr_t) &_controls);
_mixer_group = new MixerGroup();
// PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename);
if (load_mixer_file(mixer_filename, buf, buflen) == 0) {
if (_mixer_group->load_from_buf(buf, buflen) == 0) {
if (_mixer_group->load_from_buf(mixer_control_callback, (uintptr_t) &_controls, buf, buflen) == 0) {
PX4_INFO("Loaded mixer from file %s", mixer_filename);
return 0;

6
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -72,7 +72,7 @@
#include <drivers/drv_tone_alarm.h>
#include <systemlib/err.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
@ -1045,7 +1045,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
unsigned buflen = strlen(buf);
if (_mixers == nullptr) {
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
_mixers = new MixerGroup();
}
if (_mixers == nullptr) {
@ -1053,7 +1053,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
} else {
ret = _mixers->load_from_buf(buf, buflen);
ret = _mixers->load_from_buf(control_callback, (uintptr_t)&_controls, buf, buflen);
if (ret != 0) {
DEVICE_DEBUG("mixer load failed with %d", ret);

3
src/drivers/pwm_out_sim/PWMSim.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* 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
@ -37,7 +37,6 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_pwm_output.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>

3
src/drivers/px4io/px4io.cpp
View File

@ -70,7 +70,8 @@
#include <rc/dsm.h>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <lib/mixer/MultirotorMixer.hpp>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <parameters/param.h>

2
src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp
View File

@ -46,7 +46,7 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <lib/mixer/mixer_multirotor_normalized.generated.h>
#include <parameters/param.h>

60
src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp
View File

@ -48,12 +48,13 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <mixer/mixer.h>
#include <mixer/mixer_load.h>
#include <mixer/mixer_multirotor_normalized.generated.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <output_limit/output_limit.h>
#include <lib/mixer/MultirotorMixer.hpp>
#include <lib/mixer/MixerGroup.hpp>
#include <lib/mixer/mixer_load.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <lib/output_limit/output_limit.h>
#include <dev_fs_lib_pwm.h>
/*
@ -174,55 +175,30 @@ void update_params(Mixer::Airmode &airmode)
int initialize_mixer(const char *mixer_filename)
{
char buf[2048];
size_t buflen = sizeof(buf);
PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename);
int fd_load = ::open(mixer_filename, O_RDONLY);
if (fd_load != -1) {
int nRead = ::read(fd_load, buf, buflen);
close(fd_load);
int nRead = ::read(fd_load, buf, buflen);
close(fd_load);
if (nRead > 0) {
_mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
if (nRead > 0) {
_mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
if (_mixer != nullptr) {
PX4_INFO("Successfully initialized mixer from config file");
return 0;
} else {
PX4_ERR("Unable to parse from mixer config file");
return -1;
}
if (_mixer != nullptr) {
PX4_INFO("Successfully initialized mixer from config file");
return 0;
} else {
PX4_WARN("Unable to read from mixer config file");
return -2;
}
} else {
PX4_WARN("No mixer config file found, using default mixer.");
/* Mixer file loading failed, fall back to default mixer configuration for
* QUAD_X airframe. */
float roll_scale = 1;
float pitch_scale = 1;
float yaw_scale = 1;
float deadband = 0;
_mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
MultirotorGeometry::QUAD_X,
roll_scale, pitch_scale, yaw_scale, deadband);
if (_mixer == nullptr) {
PX4_ERR("Unable to parse from mixer config file");
return -1;
}
return 0;
} else {
PX4_WARN("Unable to read from mixer config file");
return -2;
}
}
void subscribe()

7
src/drivers/tap_esc/tap_esc.cpp
View File

@ -59,7 +59,7 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <mixer/mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include "tap_esc_common.h"
@ -677,7 +677,7 @@ int TAP_ESC::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
unsigned buflen = strlen(buf);
if (_mixers == nullptr) {
_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
_mixers = new MixerGroup();
}
if (_mixers == nullptr) {
@ -685,8 +685,7 @@ int TAP_ESC::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
ret = -ENOMEM;
} else {
ret = _mixers->load_from_buf(buf, buflen);
ret = _mixers->load_from_buf(control_callback_trampoline, (uintptr_t)this, buf, buflen);
if (ret != 0) {
PX4_DEBUG("mixer load failed with %d", ret);

1
src/drivers/uavcan/uavcan_main.cpp
View File

@ -50,7 +50,6 @@
#include <fcntl.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <lib/mixer/mixer.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>

1
src/drivers/uavcan/uavcan_servers.cpp
View File

@ -44,7 +44,6 @@
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <lib/mixer/mixer.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>

2
src/drivers/uavcanesc/uavcanesc_main.cpp
View File

@ -43,7 +43,7 @@
#include <fcntl.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>

2
src/drivers/uavcannode/uavcannode_main.cpp
View File

@ -46,7 +46,7 @@
#include <fcntl.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <version/version.h>
__BEGIN_DECLS
#include <nuttx/board.h>

11
src/lib/mixer/HelicopterMixer.cpp
View File

@ -37,8 +37,6 @@
* Helicopter mixers.
*/
#include "mixer.h"
#include "HelicopterMixer.hpp"
#include <mathlib/mathlib.h>
@ -52,9 +50,9 @@
using math::constrain;
HelicopterMixer::HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s *mixer_info) :
HelicopterMixer::HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info) :
Mixer(control_cb, cb_handle),
_mixer_info(*mixer_info)
_mixer_info(mixer_info)
{
}
@ -182,10 +180,7 @@ HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
HelicopterMixer *hm = new HelicopterMixer(
control_cb,
cb_handle,
&mixer_info);
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);

6
src/lib/mixer/HelicopterMixer.hpp
View File

@ -33,6 +33,8 @@
#pragma once
#include "Mixer.hpp"
/** helicopter swash servo mixer */
struct mixer_heli_servo_s {
float angle;
@ -50,7 +52,7 @@ 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];
struct mixer_heli_servo_s servos[4]; /**< up to four inputs */
mixer_heli_servo_s servos[4]; /**< up to four inputs */
};
/**
@ -69,7 +71,7 @@ public:
* @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);
HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info);
virtual ~HelicopterMixer() = default;
// no copy, assignment, move, move assignment

8
src/lib/mixer/Mixer.cpp
View File

@ -37,8 +37,6 @@
* Programmable multi-channel mixer library.
*/
#include "mixer.h"
#include "Mixer.hpp"
#include <math.h>
@ -48,12 +46,6 @@
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) :
_control_cb(control_cb),
_cb_handle(cb_handle)
{
}
float
Mixer::get_control(uint8_t group, uint8_t index)
{

105
src/lib/mixer/Mixer.hpp
View File

@ -31,16 +31,102 @@
*
****************************************************************************/
/**
* @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
/** simple channel scaler */
struct mixer_scaler_s {
float negative_scale;
float positive_scale;
float offset;
float min_output;
float max_output;
};
#include <mathlib/mathlib.h>
/**
* Abstract class defining a mixer mixing zero or more inputs to
@ -74,7 +160,7 @@ public:
*
* @param control_cb Callback invoked when reading controls.
*/
Mixer(ControlCallback control_cb, uintptr_t cb_handle);
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
@ -146,6 +232,7 @@ public:
virtual unsigned get_multirotor_count() { return 0; }
protected:
/** client-supplied callback used when fetching control values */
ControlCallback _control_cb;
uintptr_t _cb_handle;

40
src/lib/mixer/MixerGroup.cpp
View File

@ -37,39 +37,19 @@
* Mixer collection.
*/
#include <px4_platform_common/px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include "mixer.h"
#include "MixerGroup.hpp"
#include "MixerGroup.hpp"
#include "NullMixer.hpp"
#include "SimpleMixer.hpp"
#include "MultirotorMixer.hpp"
#include "HelicopterMixer.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)
MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
Mixer(control_cb, cb_handle)
{
}
MixerGroup::~MixerGroup()
{
reset();
}
void
MixerGroup::add_mixer(Mixer *mixer)
{
@ -187,7 +167,7 @@ MixerGroup::set_thrust_factor(float val)
}
void
MixerGroup::set_airmode(Airmode airmode)
MixerGroup::set_airmode(Mixer::Airmode airmode)
{
Mixer *mixer = _first;
@ -255,7 +235,7 @@ MixerGroup::groups_required(uint32_t &groups)
}
int
MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
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;
@ -278,15 +258,15 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
break;
case 'M':
m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid);
m = SimpleMixer::from_text(control_cb, cb_handle, p, resid);
break;
case 'R':
m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid);
m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid);
break;
case 'H':
m = HelicopterMixer::from_text(_control_cb, _cb_handle, p, resid);
m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid);
break;
default:

34
src/lib/mixer/MixerGroup.hpp
View File

@ -33,15 +33,21 @@
#pragma once
#include "Mixer.hpp"
/**
* Group of mixers, built up from single mixers and processed
* in order when mixing.
*/
class MixerGroup : public Mixer
class MixerGroup
{
public:
MixerGroup(ControlCallback control_cb, uintptr_t cb_handle);
virtual ~MixerGroup();
MixerGroup() = default;
~MixerGroup()
{
reset();
}
// no copy, assignment, move, move assignment
MixerGroup(const MixerGroup &) = delete;
@ -49,9 +55,11 @@ public:
MixerGroup(MixerGroup &&) = delete;
MixerGroup &operator=(MixerGroup &&) = delete;
unsigned mix(float *outputs, unsigned space) override;
uint16_t get_saturation_status() override;
void groups_required(uint32_t &groups) override;
unsigned mix(float *outputs, unsigned space);
uint16_t get_saturation_status();
void groups_required(uint32_t &groups);
/**
* Add a mixer to the group.
@ -125,7 +133,7 @@ public:
* bytes as they are consumed.
* @return Zero on successful load, nonzero otherwise.
*/
int load_from_buf(const char *buf, unsigned &buflen);
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
@ -137,25 +145,25 @@ public:
* @param[in] delta_out_max Maximum delta output.
*
*/
void set_max_delta_out_once(float delta_out_max) override;
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);
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) override;
void set_thrust_factor(float val);
void set_airmode(Airmode airmode) override;
void set_airmode(Mixer::Airmode airmode);
unsigned get_multirotor_count() override;
unsigned get_multirotor_count();
private:
Mixer *_first{nullptr}; /**< linked list of mixers */

35
src/lib/mixer/MultirotorMixer.cpp
View File

@ -37,7 +37,7 @@
* Multi-rotor mixers.
*/
#include "mixer.h"
#include "MultirotorMixer.hpp"
#include <float.h>
#include <cstring>
@ -45,8 +45,6 @@
#include <mathlib/mathlib.h>
#include "MultirotorMixer.hpp"
#ifdef MIXER_MULTIROTOR_USE_MOCK_GEOMETRY
enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {
QUAD_X,
@ -81,28 +79,18 @@ const char *_config_key[] = {"4x"};
//#include <debug.h>
//#define debug(fmt, args...) syslog(fmt "\n", ##args)
MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
uintptr_t cb_handle,
MultirotorGeometry geometry,
float roll_scale,
float pitch_scale,
float yaw_scale,
float idle_speed) :
Mixer(control_cb, cb_handle),
_roll_scale(roll_scale),
_pitch_scale(pitch_scale),
_yaw_scale(yaw_scale),
_idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */
_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]),
_outputs_prev(new float[_rotor_count]),
_tmp_array(new float[_rotor_count])
MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry,
float roll_scale, float pitch_scale, float yaw_scale, float idle_speed) :
MultirotorMixer(control_cb, cb_handle, _config_index[(int)geometry], _config_rotor_count[(int)geometry])
{
for (unsigned i = 0; i < _rotor_count; ++i) {
_outputs_prev[i] = _idle_speed;
}
_roll_scale = roll_scale;
_pitch_scale = pitch_scale;
_yaw_scale = yaw_scale;
_idle_speed = -1.0f + idle_speed * 2.0f; /* shift to output range here to avoid runtime calculation */
}
MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, Rotor *rotors, unsigned rotor_count) :
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),
@ -423,7 +411,6 @@ MultirotorMixer::mix(float *outputs, unsigned space)
outputs[i] = _outputs_prev[i] - _delta_out_max;
clipping_low_roll_pitch = true;
clipping_low_yaw = true;
}
}

13
src/lib/mixer/MultirotorMixer.hpp
View File

@ -33,6 +33,8 @@
#pragma once
#include "Mixer.hpp"
/**
* Supported multirotor geometries.
*
@ -77,13 +79,8 @@ public:
* 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,
float roll_scale,
float pitch_scale,
float yaw_scale,
float idle_speed);
MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry,
float roll_scale, float pitch_scale, float yaw_scale, float idle_speed);
/**
* Constructor (for testing).
@ -93,7 +90,7 @@ public:
* @param rotors control allocation matrix
* @param rotor_count length of rotors array (= number of motors)
*/
MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, Rotor *rotors, unsigned rotor_count);
MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count);
virtual ~MultirotorMixer();
// no copy, assignment, move, move assignment

7
src/lib/mixer/NullMixer.cpp
View File

@ -31,17 +31,12 @@
*
****************************************************************************/
#include "mixer.h"
#include "NullMixer.hpp"
#include <math.h>
#include <cstring>
#include <ctype.h>
NullMixer::NullMixer() :
Mixer(nullptr, 0)
{
}
unsigned
NullMixer::mix(float *outputs, unsigned space)
{

10
src/lib/mixer/NullMixer.hpp
View File

@ -33,6 +33,8 @@
#pragma once
#include "Mixer.hpp"
/**
* Null mixer; returns zero.
*
@ -41,7 +43,7 @@
class NullMixer : public Mixer
{
public:
NullMixer();
NullMixer() : Mixer(nullptr, 0) {}
virtual ~NullMixer() = default;
// no copy, assignment, move, move assignment
@ -65,9 +67,9 @@ public:
*/
static NullMixer *from_text(const char *buf, unsigned &buflen);
unsigned mix(float *outputs, unsigned space) override;
unsigned mix(float *outputs, unsigned space) override;
unsigned set_trim(float trim) override { return 1; }
unsigned get_trim(float *trim) override { return 1; }
unsigned set_trim(float trim) override { return 1; }
unsigned get_trim(float *trim) override { return 1; }
};

6
src/lib/mixer/SimpleMixer.cpp
View File

@ -37,7 +37,8 @@
* Simple summing mixer.
*/
#include "mixer.h"
#include "SimpleMixer.hpp"
#include <stdio.h>
#include <stdlib.h>
@ -190,7 +191,6 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
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
@ -202,7 +202,6 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
mixinfo->output_scaler.max_output = 1.0f;
} else {
if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) {
debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
goto out;
@ -217,7 +216,6 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
goto out;
}
}
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);

19
src/lib/mixer/SimpleMixer.hpp
View File

@ -33,11 +33,22 @@
#pragma once
#include "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 */
struct mixer_scaler_s scaler; /**< scaling applied to the input before use */
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))
@ -45,8 +56,8 @@ struct mixer_control_s {
/** simple mixer */
struct mixer_simple_s {
uint8_t control_count; /**< number of inputs */
struct mixer_scaler_s output_scaler; /**< scaling for the output */
struct mixer_control_s controls[0]; /**< actual size of the array is set by control_count */
mixer_scaler_s output_scaler; /**< scaling for the output */
mixer_control_s controls[]; /**< actual size of the array is set by control_count */
};
/**
@ -132,6 +143,6 @@ private:
static int parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
uint8_t &control_index);
mixer_simple_s *_pinfo{nullptr};
mixer_simple_s *_pinfo;
};

4
src/lib/mixer/load_mixer_file.cpp
View File

@ -45,8 +45,8 @@
int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
{
FILE *fp;
char line[120];
FILE *fp;
char line[120];
/* open the mixer definition file */
fp = fopen(fname, "r");

138
src/lib/mixer/mixer.h
View File

@ -1,138 +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.h
*
* 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 <stdint.h>
#include <lib/mathlib/mathlib.h>
#include "Mixer.hpp"
#include "MixerGroup.hpp"
#include "NullMixer.hpp"
#include "SimpleMixer.hpp"
#include "MultirotorMixer.hpp"
#include "HelicopterMixer.hpp"

3
src/lib/mixer/test_mixer_multirotor.cpp
View File

@ -36,7 +36,8 @@
* via file or stdin and compares the mixer output against expected values.
*/
#include "mixer.h"
#include "MultirotorMixer.hpp"
#include <cstdio>
#include <cmath>

6
src/lib/mixer_module/mixer_module.cpp
View File

@ -33,6 +33,8 @@
#include "mixer_module.hpp"
#include <lib/mixer/MultirotorMixer.hpp>
#include <uORB/PublicationQueued.hpp>
#include <px4_platform_common/log.h>
@ -543,7 +545,7 @@ void MixingOutput::resetMixer()
int MixingOutput::loadMixer(const char *buf, unsigned len)
{
if (_mixers == nullptr) {
_mixers = new MixerGroup(controlCallback, (uintptr_t)this);
_mixers = new MixerGroup();
}
if (_mixers == nullptr) {
@ -551,7 +553,7 @@ int MixingOutput::loadMixer(const char *buf, unsigned len)
return -ENOMEM;
}
int ret = _mixers->load_from_buf(buf, len);
int ret = _mixers->load_from_buf(controlCallback, (uintptr_t)this, buf, len);
if (ret != 0) {
PX4_ERR("mixer load failed with %d", ret);

3
src/lib/mixer_module/mixer_module.hpp
View File

@ -35,7 +35,7 @@
#include <board_config.h>
#include <drivers/drv_pwm_output.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/output_limit/output_limit.h>
#include <px4_platform_common/atomic.h>
@ -52,7 +52,6 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/test_motor.h>
/**
* @class OutputModuleInterface
* Base class for an output module.

2
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -33,7 +33,7 @@
#pragma once
#include <lib/mixer/mixer.h>
#include <lib/mixer/Mixer.hpp>
#include <matrix/matrix/math.hpp>
#include <perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>

1
src/modules/mc_rate_control/MulticopterRateControl.hpp
View File

@ -37,7 +37,6 @@
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mixer/mixer.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>

2
src/modules/mc_rate_control/RateControl/RateControl.hpp
View File

@ -42,7 +42,7 @@
#include <matrix/matrix/math.hpp>
#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/mixer/mixer.h>
#include <lib/mixer/MultirotorMixer.hpp>
#include <uORB/topics/rate_ctrl_status.h>
class RateControl

8
src/modules/px4iofirmware/mixer.cpp
View File

@ -51,7 +51,7 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <mixer/mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <output_limit/output_limit.h>
#include <rc/sbus.h>
@ -96,7 +96,7 @@ static volatile mixer_source source;
static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
static int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits);
static MixerGroup mixer_group(mixer_callback, 0);
static MixerGroup mixer_group;
int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits)
{
@ -114,7 +114,7 @@ int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits)
}
void
mixer_tick(void)
mixer_tick()
{
/* check if the mixer got modified */
mixer_handle_text_create_mixer();
@ -540,7 +540,7 @@ mixer_handle_text_create_mixer()
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
mixer_group.load_from_buf(mixer_callback, 0, &mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {

2
src/systemcmds/mixer/mixer.cpp
View File

@ -49,7 +49,7 @@
#include <ctype.h>
#include <drivers/drv_mixer.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer/Mixer.hpp>
#include <lib/mixer/mixer_load.h>
#include <uORB/topics/actuator_controls.h>

1
src/systemcmds/tests/test_autodeclination.cpp
View File

@ -43,7 +43,6 @@
#include <ecl/geo_lookup/geo_mag_declination.h>
#include <px4iofirmware/px4io.h>
#include <systemlib/err.h>
#include <lib/mixer/mixer.h>
#include <math.h>
#include <stdio.h>

22
src/systemcmds/tests/test_mixer.cpp
View File

@ -43,8 +43,8 @@
#include <math.h>
#include <px4_platform_common/px4_config.h>
#include <mixer/mixer.h>
#include <mixer/mixer_load.h>
#include <lib/mixer/MixerGroup.hpp>
#include <lib/mixer/mixer_load.h>
#include <output_limit/output_limit.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
@ -95,7 +95,7 @@ class MixerTest : public UnitTest
{
public:
virtual bool run_tests();
MixerTest();
MixerTest() = default;
private:
bool mixerTest();
@ -112,11 +112,6 @@ private:
MixerGroup mixer_group;
};
MixerTest::MixerTest() :
mixer_group(mixer_callback, 0)
{
}
bool MixerTest::run_tests()
{
ut_run_test(loadIOPass);
@ -251,11 +246,8 @@ bool MixerTest::load_mixer(const char *filename, unsigned expected_count, bool v
}
bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loaded, unsigned expected_count,
const unsigned chunk_size,
bool verbose)
const unsigned chunk_size, bool verbose)
{
/* load the mixer in chunks, like
* in the case of a remote load,
* e.g. on PX4IO.
@ -264,7 +256,7 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade
/* load at once test */
unsigned xx = loaded;
mixer_group.reset();
mixer_group.load_from_buf(&buf[0], xx);
mixer_group.load_from_buf(mixer_callback, 0, &buf[0], xx);
if (expected_count > 0) {
ut_compare("check number of mixers loaded", mixer_group.count(), expected_count);
@ -275,7 +267,7 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade
empty_buf[0] = ' ';
empty_buf[1] = '\0';
mixer_group.reset();
mixer_group.load_from_buf(&empty_buf[0], empty_load);
mixer_group.load_from_buf(mixer_callback, 0, &empty_buf[0], empty_load);
if (verbose) {
PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
@ -310,7 +302,7 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade
/* process the text buffer, adding new mixers as their descriptions can be parsed */
resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
mixer_group.load_from_buf(mixer_callback, 0, &mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {