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

Merge pull request #132 from PX4/#111-px4io-integrated-mixing 

#111 px4io integrated mixing
This commit is contained in:
px4dev 2013-01-06 00:40:45 -08:00
parent b3e16b4861 9df2aaf312
commit 94fa60fa04
25 changed files with 1037 additions and 674 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
apps/drivers/boards/px4io/px4io_internal.h
View File

@ -61,28 +61,6 @@
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
/* R/C in/out channels **************************************************************/
/* XXX just GPIOs for now - eventually timer pins */
#define GPIO_CH1_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0)
#define GPIO_CH2_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1)
#define GPIO_CH3_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8)
#define GPIO_CH4_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9)
#define GPIO_CH5_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6)
#define GPIO_CH6_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7)
#define GPIO_CH7_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0)
#define GPIO_CH8_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1)
#define GPIO_CH1_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
#define GPIO_CH2_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
#define GPIO_CH3_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
#define GPIO_CH4_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
#define GPIO_CH5_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
#define GPIO_CH6_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
#define GPIO_CH7_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
#define GPIO_CH8_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
/* Safety switch button *************************************************************/
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)

23
apps/drivers/drv_mixer.h
View File

@ -100,28 +100,13 @@ struct mixer_simple_s {
*/
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
/** multirotor output definition */
struct mixer_rotor_output_s {
float angle; /**< rotor angle clockwise from forward in radians */
float distance; /**< motor distance from centre in arbitrary units */
};
/** multirotor mixer */
struct mixer_multirotor_s {
uint8_t rotor_count;
struct mixer_control_s controls[4]; /**< controls are roll, pitch, yaw, thrust */
struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */
};
/* _MIXERIOC(3) was deprecated */
/* _MIXERIOC(4) was deprecated */
/**
* Add a multirotor mixer in (struct mixer_multirotor_s *)arg
* Add mixer(s) from the buffer in (const char *)arg
*/
#define MIXERIOCADDMULTIROTOR _MIXERIOC(3)
/**
* Add mixers(s) from a the file in (const char *)arg
*/
#define MIXERIOCLOADFILE _MIXERIOC(4)
#define MIXERIOCLOADBUF _MIXERIOC(5)
/*
* XXX Thoughts for additional operations:

23
apps/drivers/hil/hil.cpp
View File

@ -577,26 +577,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
} else {
debug("loading mixers from %s", path);
ret = _mixers->load_from_file(path);
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
@ -605,10 +598,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
default:
ret = -ENOTTY;
break;

22
apps/drivers/px4fmu/fmu.cpp
View File

@ -544,28 +544,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
} else {
debug("loading mixers from %s", path);
ret = _mixers->load_from_file(path);
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
@ -574,7 +565,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}

187
apps/drivers/px4io/px4io.cpp
View File

@ -92,7 +92,7 @@ public:
bool dump_one;
private:
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS;
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
@ -113,7 +113,8 @@ private:
orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
MixerGroup *_mixers; ///< loaded mixers
const char *volatile _mix_buf; ///< mixer text buffer
volatile unsigned _mix_buf_len; ///< size of the mixer text buffer
bool _primary_pwm_device; ///< true if we are the default PWM output
@ -160,6 +161,11 @@ private:
*/
void config_send();
/**
* Send a buffer containing mixer text to PX4IO
*/
int mixer_send(const char *buf, unsigned buflen);
/**
* Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing.
@ -189,7 +195,8 @@ PX4IO::PX4IO() :
_t_actuators(-1),
_t_armed(-1),
_t_outputs(-1),
_mixers(nullptr),
_mix_buf(nullptr),
_mix_buf_len(0),
_primary_pwm_device(false),
_relays(0),
_switch_armed(false),
@ -212,6 +219,7 @@ PX4IO::~PX4IO()
/* give it another 100ms */
usleep(100000);
}
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
task_delete(_task);
@ -242,6 +250,7 @@ PX4IO::init()
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@ -253,13 +262,16 @@ PX4IO::init()
debug("PX4IO connected");
break;
}
usleep(100000);
}
if (!_connected) {
/* error here will result in everything being torn down */
log("PX4IO not responding");
return -EIO;
}
return OK;
}
@ -272,7 +284,7 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
void
PX4IO::task_main()
{
log("starting");
debug("starting");
/* open the serial port */
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
@ -294,10 +306,12 @@ PX4IO::task_main()
/* protocol stream */
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
if (_io_stream == nullptr) {
log("failed to allocate HX protocol stream");
goto out;
}
hx_stream_set_counters(_io_stream,
perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
perf_alloc(PC_COUNT, "PX4IO frames received"),
@ -334,13 +348,18 @@ PX4IO::task_main()
fds[2].fd = _t_armed;
fds[2].events = POLLIN;
log("ready");
debug("ready");
/* lock against the ioctl handler */
lock();
/* loop handling received serial bytes */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
unlock();
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
lock();
/* this would be bad... */
if (ret < 0) {
@ -357,26 +376,21 @@ PX4IO::task_main()
if (fds[0].revents & POLLIN)
io_recv();
/* if we have new data from the ORB, go handle it */
/* if we have new control data from the ORB, handle it */
if (fds[1].revents & POLLIN) {
/* get controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* mix */
if (_mixers != nullptr) {
/* XXX is this the right count? */
_mixers->mix(&_outputs.output[0], _max_actuators);
/* scale controls to PWM (temporary measure) */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _controls.control[i]);
/* convert to PWM values */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
/* and flag for update */
_send_needed = true;
}
/* and flag for update */
_send_needed = true;
}
/* if we have an arming state update, handle it */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
@ -395,14 +409,26 @@ PX4IO::task_main()
_config_needed = false;
config_send();
}
/* send a mixer update if needed */
if (_mix_buf != nullptr) {
mixer_send(_mix_buf, _mix_buf_len);
/* clear the buffer record so the ioctl handler knows we're done */
_mix_buf = nullptr;
_mix_buf_len = 0;
}
}
unlock();
out:
debug("exiting");
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
::close(_serial_fd);
/* clean up the alternate device node */
@ -455,25 +481,27 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{
const px4io_report *rep = (const px4io_report *)buffer;
lock();
// lock();
/* sanity-check the received frame size */
if (bytes_received != sizeof(px4io_report)) {
debug("got %u expected %u", bytes_received, sizeof(px4io_report));
goto out;
}
if (rep->i2f_magic != I2F_MAGIC) {
debug("bad magic");
goto out;
}
_connected = true;
/* publish raw rc channel values from IO if valid channels are present */
if (rep->channel_count > 0) {
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
for (int i = 0; i < rep->channel_count; i++)
{
for (int i = 0; i < rep->channel_count; i++) {
_input_rc.values[i] = rep->rc_channel[i];
}
@ -490,13 +518,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
dump_one = false;
printf("IO: %s armed ", rep->armed ? "" : "not");
for (unsigned i = 0; i < rep->channel_count; i++)
printf("%d: %d ", i, rep->rc_channel[i]);
printf("\n");
}
out:
unlock();
// unlock();
return;
}
void
@ -509,9 +540,10 @@ PX4IO::io_send()
/* set outputs */
for (unsigned i = 0; i < _max_actuators; i++)
cmd.servo_command[i] = _outputs.output[i];
cmd.output_control[i] = _outputs.output[i];
/* publish as we send */
/* XXX needs to be based off post-mix values from the IO side */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
/* update relays */
@ -522,6 +554,7 @@ PX4IO::io_send()
cmd.arm_ok = (_armed.armed && !_armed.lockdown);
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret)
debug("send error %d", ret);
}
@ -535,10 +568,46 @@ PX4IO::config_send()
cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
if (ret)
debug("config error %d", ret);
}
int
PX4IO::mixer_send(const char *buf, unsigned buflen)
{
uint8_t frame[HX_STREAM_MAX_FRAME];
px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
msg->action = F2I_MIXER_ACTION_RESET;
do {
unsigned count = buflen;
if (count > F2I_MIXER_MAX_TEXT)
count = F2I_MIXER_MAX_TEXT;
if (count > 0) {
memcpy(&msg->text[0], buf, count);
buf += count;
buflen -= count;
}
int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count);
if (ret) {
log("mixer send error %d", ret);
return ret;
}
msg->action = F2I_MIXER_ACTION_APPEND;
} while (buflen > 0);
return 0;
}
int
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
@ -562,7 +631,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
/* fake an update to the selected servo channel */
/* fake an update to the selected 'servo' channel */
if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true;
@ -603,67 +672,28 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break;
case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _max_actuators;
*(unsigned *)arg = PX4IO_CONTROL_CHANNELS;
break;
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
ret = 0; /* load always resets */
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
case MIXERIOCLOADBUF:
/* build the new mixer from the supplied argument */
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
/* set the buffer up for transfer */
_mix_buf = (const char *)arg;
_mix_buf_len = strnlen(_mix_buf, 1024);
/* validate the new mixer */
if (mixer->check()) {
delete mixer;
ret = -EINVAL;
/* drop the lock and wait for the thread to clear the transmit */
unlock();
} else {
/* if we don't have a group yet, allocate one */
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
while (_mix_buf != nullptr)
usleep(1000);
/* add the new mixer to the group */
_mixers->add_mixer(mixer);
}
lock();
}
break;
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADFILE: {
MixerGroup *newmixers;
const char *path = (const char *)arg;
/* allocate a new mixer group and load it from the file */
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (newmixers->load_from_file(path) != 0) {
delete newmixers;
ret = -EINVAL;
}
/* swap the new mixers in for the old */
if (_mixers != nullptr) {
delete _mixers;
}
_mixers = newmixers;
}
ret = 0;
break;
default:
@ -705,6 +735,13 @@ test(void)
close(fd);
actuator_armed_s aa;
aa.armed = true;
aa.lockdown = false;
orb_advertise(ORB_ID(actuator_armed), &aa);
exit(0);
}
@ -724,6 +761,7 @@ monitor(void)
if (fds[0].revents == POLLIN) {
int c;
read(0, &c, 1);
if (cancels-- == 0)
exit(0);
}
@ -810,6 +848,7 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "test"))
test();
if (!strcmp(argv[1], "monitor"))
monitor();

1
apps/drivers/px4io/uploader.cpp
View File

@ -190,6 +190,7 @@ PX4IO_Uploader::drain()
do {
ret = recv(c, 250);
if (ret == OK) {
//log("discard 0x%02x", c);
}

4
apps/px4io/Makefile
View File

@ -41,7 +41,9 @@
#
CSRCS = $(wildcard *.c) \
../systemlib/hx_stream.c \
../systemlib/perf_counter.c
../systemlib/perf_counter.c \
../systemlib/up_cxxinitialize.c
CXXSRCS = $(wildcard *.cpp)
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common

29
apps/px4io/comms.c
View File

@ -70,6 +70,8 @@ static struct px4io_report report;
static void comms_handle_frame(void *arg, const void *buffer, size_t length);
perf_counter_t comms_rx_errors;
static void
comms_init(void)
{
@ -77,6 +79,9 @@ comms_init(void)
fmu_fd = open("/dev/ttyS1", O_RDWR);
stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
comms_rx_errors = perf_alloc(PC_COUNT, "rx_err");
hx_stream_set_counters(stream, 0, 0, comms_rx_errors);
/* default state in the report to FMU */
report.i2f_magic = I2F_MAGIC;
@ -110,6 +115,7 @@ comms_main(void)
if (fds.revents & POLLIN) {
char buf[32];
ssize_t count = read(fmu_fd, buf, sizeof(buf));
for (int i = 0; i < count; i++)
hx_stream_rx(stream, buf[i]);
}
@ -123,7 +129,8 @@ comms_main(void)
/* should we send a report to the FMU? */
now = hrt_absolute_time();
delta = now - last_report_time;
if ((delta > FMU_MIN_REPORT_INTERVAL) &&
if ((delta > FMU_MIN_REPORT_INTERVAL) &&
(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
system_state.fmu_report_due = false;
@ -132,6 +139,7 @@ comms_main(void)
/* populate the report */
for (unsigned i = 0; i < system_state.rc_channels; i++)
report.rc_channel[i] = system_state.rc_channel_data[i];
report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
@ -168,19 +176,17 @@ comms_handle_command(const void *buffer, size_t length)
irqstate_t flags = irqsave();
/* fetch new PWM output values */
for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
system_state.fmu_channel_data[i] = cmd->servo_command[i];
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
system_state.fmu_channel_data[i] = cmd->output_control[i];
/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
if (system_state.arm_ok && !cmd->arm_ok) {
if (system_state.arm_ok && !cmd->arm_ok)
system_state.armed = false;
}
system_state.arm_ok = cmd->arm_ok;
system_state.mixer_use_fmu = true;
system_state.fmu_data_received = true;
/* handle changes signalled by FMU */
// if (!system_state.arm_ok && system_state.armed)
// system_state.armed = false;
@ -203,13 +209,12 @@ comms_handle_command(const void *buffer, size_t length)
break;
}
}
system_state.relays[i] != cmd->relay_state[i]
system_state.relays[i] = cmd->relay_state[i];
}
irqrestore(flags);
}
static void
comms_handle_frame(void *arg, const void *buffer, size_t length)
{
@ -222,11 +227,17 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
case F2I_MAGIC:
comms_handle_command(buffer, length);
break;
case F2I_CONFIG_MAGIC:
comms_handle_config(buffer, length);
break;
case F2I_MIXER_MAGIC:
mixer_handle_text(buffer, length);
break;
default:
frame_bad++;
frame_bad++;
break;
}
}

4
apps/px4io/controls.c
View File

@ -90,6 +90,7 @@ controls_main(void)
if (fds[0].revents & POLLIN)
locked |= dsm_input();
if (fds[1].revents & POLLIN)
locked |= sbus_input();
@ -139,6 +140,7 @@ ppm_input(void)
/* PPM data exists, copy it */
system_state.rc_channels = ppm_decoded_channels;
for (unsigned i = 0; i < ppm_decoded_channels; i++)
system_state.rc_channel_data[i] = ppm_buffer[i];
@ -150,5 +152,5 @@ ppm_input(void)
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;
}
}
}

40
apps/px4io/dsm.c
View File

@ -47,7 +47,7 @@
#include <termios.h>
#include <systemlib/ppm_decode.h>
#include <drivers/drv_hrt.h>
#define DEBUG
@ -97,6 +97,7 @@ dsm_init(const char *device)
dsm_guess_format(true);
debug("DSM: ready");
} else {
debug("DSM: open failed");
}
@ -118,7 +119,7 @@ dsm_input(void)
* frame transmission time is ~1.4ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 5ms passes between calls,
* and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
@ -126,6 +127,7 @@ dsm_input(void)
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
if ((now - last_rx_time) > 5000) {
if (partial_frame_count > 0) {
dsm_frame_drops++;
@ -142,6 +144,7 @@ dsm_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
goto out;
last_rx_time = now;
/*
@ -153,7 +156,7 @@ dsm_input(void)
* If we don't have a full frame, return
*/
if (partial_frame_count < DSM_FRAME_SIZE)
goto out;
goto out;
/*
* Great, it looks like we might have a frame. Go ahead and
@ -164,7 +167,7 @@ dsm_input(void)
out:
/*
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
*/
return (now - last_frame_time) < 200000;
}
@ -212,6 +215,7 @@ dsm_guess_format(bool reset)
/* if the channel decodes, remember the assigned number */
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
cs10 |= (1 << channel);
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
cs11 |= (1 << channel);
@ -222,7 +226,7 @@ dsm_guess_format(bool reset)
if (samples++ < 5)
return;
/*
/*
* Iterate the set of sensible sniffed channel sets and see whether
* decoding in 10 or 11-bit mode has yielded anything we recognise.
*
@ -233,7 +237,7 @@ dsm_guess_format(bool reset)
* See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
* of this issue.
*/
static uint32_t masks[] = {
static uint32_t masks[] = {
0x3f, /* 6 channels (DX6) */
0x7f, /* 7 channels (DX7) */
0xff, /* 8 channels (DX8) */
@ -247,14 +251,17 @@ dsm_guess_format(bool reset)
if (cs10 == masks[i])
votes10++;
if (cs11 == masks[i])
votes11++;
}
if ((votes11 == 1) && (votes10 == 0)) {
channel_shift = 11;
debug("DSM: detected 11-bit format");
return;
}
if ((votes10 == 1) && (votes11 == 0)) {
channel_shift = 10;
debug("DSM: detected 10-bit format");
@ -270,13 +277,13 @@ static void
dsm_decode(hrt_abstime frame_time)
{
/*
debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
*/
/*
* If we have lost signal for at least a second, reset the
debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
*/
/*
* If we have lost signal for at least a second, reset the
* format guessing heuristic.
*/
if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
@ -292,7 +299,7 @@ dsm_decode(hrt_abstime frame_time)
}
/*
* The encoding of the first two bytes is uncertain, so we're
* The encoding of the first two bytes is uncertain, so we're
* going to ignore them for now.
*
* Each channel is a 16-bit unsigned value containing either a 10-
@ -322,9 +329,10 @@ dsm_decode(hrt_abstime frame_time)
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
if (channel_shift == 11)
value /= 2;
value += 998;
/*
/*
* Store the decoded channel into the R/C input buffer, taking into
* account the different ideas about channel assignement that we have.
*
@ -335,14 +343,18 @@ dsm_decode(hrt_abstime frame_time)
case 0:
channel = 2;
break;
case 1:
channel = 0;
break;
case 2:
channel = 1;
default:
break;
}
system_state.rc_channel_data[channel] = value;
}

133
apps/px4io/mixer.c → apps/px4io/mixer.cpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file mixer.c
* @file mixer.cpp
*
* Control channel input/output mixer and failsafe.
*/
@ -49,8 +49,12 @@
#include <fcntl.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
extern "C" {
//#define DEBUG
#include "px4io.h"
}
/*
* Count of periodic calls in which we have no FMU input.
@ -58,28 +62,23 @@
static unsigned fmu_input_drops;
#define FMU_INPUT_DROP_LIMIT 20
/*
* Update a mixer based on the current control signals.
*/
static void mixer_update(int mixer, uint16_t *inputs, int input_count);
/* current servo arm/disarm state */
bool mixer_servos_armed = false;
/*
* Each mixer consumes a set of inputs and produces a single output.
*/
struct mixer {
uint16_t current_value;
/* XXX more config here */
} mixers[IO_SERVO_COUNT];
/* selected control values and count for mixing */
static uint16_t *control_values;
static int control_count;
static int mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &control);
static MixerGroup mixer_group(mixer_callback, 0);
void
mixer_tick(void)
{
uint16_t *control_values;
int control_count;
int i;
bool should_arm;
/*
@ -87,7 +86,7 @@ mixer_tick(void)
*/
if (system_state.mixer_use_fmu) {
/* we have recent control data from the FMU */
control_count = PX4IO_OUTPUT_CHANNELS;
control_count = PX4IO_CONTROL_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
/* check that we are receiving fresh data from the FMU */
@ -98,6 +97,7 @@ mixer_tick(void)
if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
system_state.mixer_use_fmu = false;
}
} else {
fmu_input_drops = 0;
system_state.fmu_data_received = false;
@ -115,17 +115,31 @@ mixer_tick(void)
}
/*
* Tickle each mixer, if we have control data.
* Run the mixers if we have any control data at all.
*/
if (control_count > 0) {
for (i = 0; i < IO_SERVO_COUNT; i++) {
mixer_update(i, control_values, control_count);
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
if (i < mixed) {
/* scale to servo output */
system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
} else {
/* set to zero to inhibit PWM pulse output */
system_state.servos[i] = 0;
}
/*
* If we are armed, update the servo output.
*/
if (system_state.armed && system_state.arm_ok)
up_pwm_servo_set(i, mixers[i].current_value);
up_pwm_servo_set(i, system_state.servos[i]);
}
}
@ -133,6 +147,7 @@ mixer_tick(void)
* Decide whether the servos should be armed right now.
*/
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
@ -145,13 +160,73 @@ mixer_tick(void)
}
}
static void
mixer_update(int mixer, uint16_t *inputs, int input_count)
static int
mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &control)
{
/* simple passthrough for now */
if (mixer < input_count) {
mixers[mixer].current_value = inputs[mixer];
} else {
mixers[mixer].current_value = 0;
}
/* if the control index refers to an input that's not valid, we can't return it */
if (control_index >= control_count)
return -1;
/* scale from current PWM units (1000-2000) to mixer input values */
control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
return 0;
}
static char mixer_text[256];
static unsigned mixer_text_length = 0;
void
mixer_handle_text(const void *buffer, size_t length)
{
px4io_mixdata *msg = (px4io_mixdata *)buffer;
debug("mixer text %u", length);
if (length < sizeof(px4io_mixdata))
return;
unsigned text_length = length - sizeof(px4io_mixdata);
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
debug("reset");
mixer_group.reset();
mixer_text_length = 0;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
debug("append %d", length);
/* check for overflow - this is really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
return;
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
debug("buflen %u", mixer_text_length);
/* 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);
/* if anything was parsed */
if (resid != mixer_text_length) {
debug("used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0)
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
mixer_text_length = resid;
}
break;
}
}

52
apps/px4io/protocol.h
View File

@ -41,31 +41,27 @@
#pragma once
#define PX4IO_OUTPUT_CHANNELS 8
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 4
#pragma pack(push, 1)
/* command from FMU to IO */
/**
* Periodic command from FMU to IO.
*/
struct px4io_command {
uint16_t f2i_magic;
#define F2I_MAGIC 0x636d
#define F2I_MAGIC 0x636d
uint16_t servo_command[PX4IO_OUTPUT_CHANNELS];
uint16_t output_control[PX4IO_CONTROL_CHANNELS];
bool relay_state[PX4IO_RELAY_CHANNELS];
bool arm_ok;
};
/* config message from FMU to IO */
struct px4io_config {
uint16_t f2i_config_magic;
#define F2I_CONFIG_MAGIC 0x6366
/* XXX currently nothing here */
};
/* report from IO to FMU */
/**
* Periodic report from IO to FMU
*/
struct px4io_report {
uint16_t i2f_magic;
#define I2F_MAGIC 0x7570
@ -75,4 +71,34 @@ struct px4io_report {
uint8_t channel_count;
};
/**
* As-needed config message from FMU to IO
*/
struct px4io_config {
uint16_t f2i_config_magic;
#define F2I_CONFIG_MAGIC 0x6366
/* XXX currently nothing here */
};
/**
* As-needed mixer data upload.
*
* This message adds text to the mixer text buffer; the text
* buffer is drained as the definitions are consumed.
*/
struct px4io_mixdata {
uint16_t f2i_mixer_magic;
#define F2I_MIXER_MAGIC 0x6d74
uint8_t action;
#define F2I_MIXER_ACTION_RESET 0
#define F2I_MIXER_ACTION_APPEND 1
char text[0]; /* actual text size may vary */
};
/* maximum size is limited by the HX frame size */
#define F2I_MIXER_MAX_TEXT (HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata))
#pragma pack(pop)

7
apps/px4io/px4io.c
View File

@ -55,10 +55,15 @@
__EXPORT int user_start(int argc, char *argv[]);
extern void up_cxxinitialize(void);
struct sys_state_s system_state;
int user_start(int argc, char *argv[])
{
/* run C++ ctors before we go any further */
up_cxxinitialize();
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
@ -83,7 +88,7 @@ int user_start(int argc, char *argv[])
up_pwm_servo_init(0xff);
/* start the flight control signal handler */
task_create("FCon",
task_create("FCon",
SCHED_PRIORITY_DEFAULT,
1024,
(main_t)controls_main,

25
apps/px4io/px4io.h
View File

@ -31,11 +31,11 @@
*
****************************************************************************/
/**
* @file px4io.h
*
* General defines and structures for the PX4IO module firmware.
*/
/**
* @file px4io.h
*
* General defines and structures for the PX4IO module firmware.
*/
#include <nuttx/config.h>
@ -66,8 +66,7 @@
/*
* System state structure.
*/
struct sys_state_s
{
struct sys_state_s {
bool armed; /* IO armed */
bool arm_ok; /* FMU says OK to arm */
@ -82,7 +81,12 @@ struct sys_state_s
/*
* Control signals from FMU.
*/
uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS];
/*
* Mixed servo outputs
*/
uint16_t servos[IO_SERVO_COUNT];
/*
* Relay controls
@ -136,8 +140,8 @@ extern volatile int timers[TIMER_NUM_TIMERS];
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_SERVO_ACC1_EN, (_s))
#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_SERVO_ACC2_EN, (_s))
#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
#define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
#define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
@ -149,6 +153,7 @@ extern volatile int timers[TIMER_NUM_TIMERS];
* Mixer
*/
extern void mixer_tick(void);
extern void mixer_handle_text(const void *buffer, size_t length);
/*
* Safety switch/LED.

22
apps/px4io/safety.c
View File

@ -31,9 +31,9 @@
*
****************************************************************************/
/**
* @file Safety button logic.
*/
/**
* @file Safety button logic.
*/
#include <nuttx/config.h>
#include <stdio.h>
@ -95,13 +95,13 @@ safety_init(void)
static void
safety_check_button(void *arg)
{
/*
/*
* Debounce the safety button, change state if it has been held for long enough.
*
*/
safety_button_pressed = BUTTON_SAFETY;
if(safety_button_pressed) {
if (safety_button_pressed) {
//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
}
@ -109,34 +109,42 @@ safety_check_button(void *arg)
if (safety_button_pressed && !system_state.armed) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to armed state and notify the FMU */
system_state.armed = true;
counter++;
system_state.fmu_report_due = true;
}
/* Disarm quickly */
/* Disarm quickly */
} else if (safety_button_pressed && system_state.armed) {
if (counter < DISARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == DISARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
system_state.armed = false;
counter++;
system_state.fmu_report_due = true;
}
} else {
counter = 0;
}
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_SAFE;
if (system_state.armed) {
if (system_state.arm_ok) {
pattern = LED_PATTERN_IO_FMU_ARMED;
} else {
pattern = LED_PATTERN_IO_ARMED;
}
} else if (system_state.arm_ok) {
pattern = LED_PATTERN_FMU_ARMED;
}
@ -167,8 +175,10 @@ failsafe_blink(void *arg)
/* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_use_fmu) {
failsafe = !failsafe;
} else {
failsafe = false;
}
LED_AMBER(failsafe);
}

50
apps/px4io/sbus.c
View File

@ -88,6 +88,7 @@ sbus_init(const char *device)
last_rx_time = hrt_absolute_time();
debug("Sbus: ready");
} else {
debug("Sbus: open failed");
}
@ -109,7 +110,7 @@ sbus_input(void)
* frame transmission time is ~2ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 3ms passes between calls,
* and if an interval of more than 3ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
@ -117,6 +118,7 @@ sbus_input(void)
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
if ((now - last_rx_time) > 3000) {
if (partial_frame_count > 0) {
sbus_frame_drops++;
@ -133,6 +135,7 @@ sbus_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
goto out;
last_rx_time = now;
/*
@ -144,7 +147,7 @@ sbus_input(void)
* If we don't have a full frame, return
*/
if (partial_frame_count < SBUS_FRAME_SIZE)
goto out;
goto out;
/*
* Great, it looks like we might have a frame. Go ahead and
@ -155,7 +158,7 @@ sbus_input(void)
out:
/*
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
*/
return (now - last_frame_time) < 200000;
}
@ -177,23 +180,23 @@ struct sbus_bit_pick {
uint8_t mask;
uint8_t lshift;
};
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
/* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} },
/* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} },
/* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} },
/* 3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00, 0} },
/* 4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00, 0} },
/* 5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03, 9} },
/* 6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00, 0} },
/* 7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00, 0} },
/* 8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00, 0} },
/* 9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00, 0} },
/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} },
/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00, 0} },
/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00, 0} },
/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03, 9} },
/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00, 0} },
/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} }
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
/* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
/* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
/* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
/* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
/* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
/* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} },
/* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
/* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} },
/* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
/* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} },
/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
};
static void
@ -213,8 +216,8 @@ sbus_decode(hrt_abstime frame_time)
/* we have received something we think is a frame */
last_frame_time = frame_time;
unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
/* use the decoder matrix to extract channel data */
for (unsigned channel = 0; channel < chancount; channel++) {
@ -232,6 +235,7 @@ sbus_decode(hrt_abstime frame_time)
value |= piece;
}
}
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
system_state.rc_channel_data[channel] = (value / 2) + 998;
}
@ -245,7 +249,7 @@ sbus_decode(hrt_abstime frame_time)
system_state.rc_channels = chancount;
/* and note that we have received data from the R/C controller */
system_state.rc_channels_timestamp = frame_time;
system_state.rc_channels_timestamp = frame_time;
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;

74
apps/systemcmds/mixer/mixer.c
View File

@ -43,14 +43,17 @@
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <ctype.h>
#include <nuttx/compiler.h>
#include <systemlib/err.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
__EXPORT int mixer_main(int argc, char *argv[]);
static void usage(const char *reason);
static void load(const char *devname, const char *fname);
static void usage(const char *reason) noreturn_function;
static void load(const char *devname, const char *fname) noreturn_function;
int
mixer_main(int argc, char *argv[])
@ -63,12 +66,9 @@ mixer_main(int argc, char *argv[])
usage("missing device or filename");
load(argv[2], argv[3]);
} else {
usage("unrecognised command");
}
return 0;
usage("unrecognised command");
}
static void
@ -79,34 +79,58 @@ usage(const char *reason)
fprintf(stderr, "usage:\n");
fprintf(stderr, " mixer load <device> <filename>\n");
/* XXX automatic setups for quad, etc. */
/* XXX other useful commands? */
exit(1);
}
static void
load(const char *devname, const char *fname)
{
int dev = -1;
int ret, result = 1;
int dev;
FILE *fp;
char line[80];
char buf[512];
/* open the device */
if ((dev = open(devname, 0)) < 0) {
fprintf(stderr, "can't open %s\n", devname);
goto out;
if ((dev = open(devname, 0)) < 0)
err(1, "can't open %s\n", devname);
/* reset mixers on the device */
if (ioctl(dev, MIXERIOCRESET, 0))
err(1, "can't reset mixers on %s", devname);
/* open the mixer definition file */
fp = fopen(fname, "r");
if (fp == NULL)
err(1, "can't open %s", fname);
/* 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) == NULL)
break;
/* if the line doesn't look like a mixer definition line, skip it */
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
continue;
/* XXX an optimisation here would be to strip extra whitespace */
/* if the line is too long to fit in the buffer, bail */
if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
break;
/* add the line to the buffer */
strcat(buf, line);
}
/* tell it to load the file */
ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname);
/* XXX pass the buffer to the device */
int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
if (ret != 0) {
fprintf(stderr, "failed loading %s\n", fname);
}
result = 0;
out:
if (dev != -1)
close(dev);
exit(result);
if (ret < 0)
err(1, "error loading mixers from %s", fname);
exit(0);
}

88
apps/systemlib/mixer/mixer.cpp
View File

@ -137,89 +137,15 @@ NullMixer::groups_required(uint32_t &groups)
}
/****************************************************************************/
SimpleMixer::SimpleMixer(ControlCallback control_cb,
uintptr_t cb_handle,
mixer_simple_s *mixinfo) :
Mixer(control_cb, cb_handle),
_info(mixinfo)
NullMixer *
NullMixer::from_text(const char *buf, unsigned &buflen)
{
}
NullMixer *nm = nullptr;
SimpleMixer::~SimpleMixer()
{
if (_info != nullptr)
free(_info);
}
unsigned
SimpleMixer::mix(float *outputs, unsigned space)
{
float sum = 0.0f;
if (_info == nullptr)
return 0;
if (space < 1)
return 0;
for (unsigned i = 0; i < _info->control_count; i++) {
float input;
_control_cb(_cb_handle,
_info->controls[i].control_group,
_info->controls[i].control_index,
input);
sum += scale(_info->controls[i].scaler, input);
if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
nm = new NullMixer;
buflen -= 2;
}
*outputs = scale(_info->output_scaler, sum);
return 1;
}
void
SimpleMixer::groups_required(uint32_t &groups)
{
for (unsigned i = 0; i < _info->control_count; i++)
groups |= 1 << _info->controls[i].control_group;
}
int
SimpleMixer::check()
{
int ret;
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 (_info->control_count > 32)
return -2;
/* validate the output scaler */
ret = scale_check(_info->output_scaler);
if (ret != 0)
return ret;
/* validate input scalers */
for (unsigned i = 0; i < _info->control_count; i++) {
/* verify that we can fetch the control */
if (_control_cb(_cb_handle,
_info->controls[i].control_group,
_info->controls[i].control_index,
junk) != 0) {
return -3;
}
/* validate the scaler */
ret = scale_check(_info->controls[i].scaler);
if (ret != 0)
return (10 * i + ret);
}
return 0;
return nm;
}

146
apps/systemlib/mixer/mixer.h
View File

@ -234,50 +234,51 @@ public:
void add_mixer(Mixer *mixer);
/**
* Reads a mixer definition from a file and configures a corresponding
* group.
* Remove all the mixers from the group.
*/
void reset();
/**
* Adds mixers to the group based on a text description in a buffer.
*
* The mixer group must be empty when this function is called.
* 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.
*
* A mixer definition is a text representation of the configuration of a
* mixer. Definition lines begin with a capital letter followed by a colon.
* Null Mixer
* ..........
*
* Null Mixer:
* The null mixer definition has the form:
*
* Z:
* Z:
*
* This mixer generates a constant zero output, and is normally used to
* skip over outputs that are not in use.
* Simple Mixer
* ............
*
* Simple Mixer:
* A simple mixer definition begins with:
*
* M: <scaler count>
* S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*>
* S: ...
* M: <control count>
* O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
*
* The definition consists of a single-line header indicating the
* number of scalers and then one line defining each scaler. The first
* scaler in the file is always the output scaler, followed by the input
* scalers.
* The definition continues with <control count> entries describing the control
* inputs and their scaling, in the form:
*
* The <control ...> values for the output scaler are ignored by the mixer.
* 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:
*
* Values marked * are integers representing floating point values; values are
* scaled by 10000 on load/save.
* R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
*
* Multiple mixer definitions may be stored in a single file; it is assumed that
* the reader will know how many to expect and read accordingly.
*
* A mixer entry with a scaler count of zero indicates a disabled mixer. This
* will return NULL for the mixer when processed by this function, and will be
* generated by passing NULL as the mixer to mixer_save.
*
* @param path The mixer configuration file to read.
* @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_file(const char *path);
int load_from_buf(const char *buf, unsigned &buflen);
private:
Mixer *_first; /**< linked list of mixers */
@ -294,6 +295,21 @@ public:
NullMixer();
~NullMixer() {};
/**
* 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);
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);
};
@ -318,6 +334,47 @@ public:
mixer_simple_s *mixinfo);
~SimpleMixer();
/**
* 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);
/**
* Factory method for PWM/PPM input to internal float representation.
*
* @param control_cb The callback to invoke when fetching a
* control value.
* @param cb_handle Handle passed to the control callback.
* @param input The control index used when fetching the input.
* @param min The PWM/PPM value considered to be "minimum" (gives -1.0 out)
* @param mid The PWM/PPM value considered to be the midpoint (gives 0.0 out)
* @param max The PWM/PPM value considered to be "maximum" (gives 1.0 out)
* @return A new SimpleMixer instance, or nullptr if one could not be
* allocated.
*/
static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb,
uintptr_t cb_handle,
unsigned input,
uint16_t min,
uint16_t mid,
uint16_t max);
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);
@ -330,10 +387,18 @@ public:
* @return Zero if the mixer makes sense, nonzero otherwise.
*/
int check();
protected:
private:
mixer_simple_s *_info;
mixer_simple_s *_info;
static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler);
static int parse_control_scaler(const char *buf,
unsigned &buflen,
mixer_scaler_s &scaler,
uint8_t &control_group,
uint8_t &control_index);
};
/**
@ -395,6 +460,27 @@ public:
float deadband);
~MultirotorMixer();
/**
* 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);
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);

341
apps/systemlib/mixer/mixer_group.cpp
View File

@ -56,250 +56,6 @@
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
namespace
{
/**
* Effectively fdgets() with some extra smarts.
*/
static int
mixer_getline(int fd, char *line, unsigned maxlen)
{
/* reduce line budget by 1 to account for terminal NUL */
maxlen--;
/* loop looking for a non-comment line */
for (;;) {
int ret;
char c;
char *p = line;
/* loop reading characters for this line */
for (;;) {
ret = read(fd, &c, 1);
/* on error or EOF, return same */
if (ret <= 0) {
debug("read: EOF");
return ret;
}
/* ignore carriage returns */
if (c == '\r')
continue;
/* line termination */
if (c == '\n') {
/* ignore malformed lines */
if ((p - line) < 4)
break;
if (line[1] != ':')
break;
/* terminate line as string and return */
*p = '\0';
debug("read: '%s'", line);
return 1;
}
/* if we have space, accumulate the byte and go on */
if ((p - line) < maxlen)
*p++ = c;
}
}
}
/**
* Parse an output scaler from the buffer.
*/
static int
mixer_parse_output_scaler(const char *buf, mixer_scaler_s &scaler)
{
int s[5];
if (sscanf(buf, "O: %d %d %d %d %d",
&s[0], &s[1], &s[2], &s[3], &s[4]) != 5) {
debug("scaler parse failed on '%s'", buf);
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;
return 0;
}
/**
* Parse a control scaler from the buffer.
*/
static int
mixer_parse_control_scaler(const char *buf, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
{
unsigned u[2];
int s[5];
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("scaler parse failed on '%s'", buf);
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 *
mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, unsigned inputs)
{
mixer_simple_s *mixinfo = nullptr;
char buf[60];
int ret;
/* let's assume we're going to read a simple mixer */
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
mixinfo->control_count = inputs;
/* first, get the output scaler */
ret = mixer_getline(fd, buf, sizeof(buf));
if (ret < 1) {
debug("failed reading for output scaler");
goto fail;
}
if (mixer_parse_output_scaler(buf, mixinfo->output_scaler)) {
debug("failed parsing output scaler");
goto fail;
}
/* now get any inputs */
for (unsigned i = 0; i < inputs; i++) {
ret = mixer_getline(fd, buf, sizeof(buf));
if (ret < 1) {
debug("failed reading for control scaler");
goto fail;
}
if (mixer_parse_control_scaler(buf,
mixinfo->controls[i].scaler,
mixinfo->controls[i].control_group,
mixinfo->controls[i].control_index)) {
debug("failed parsing control scaler");
goto fail;
}
debug("got control %d", i);
}
/* XXX should be a factory that validates the mixinfo ... */
return new SimpleMixer(control_cb, cb_handle, mixinfo);
fail:
free(mixinfo);
return nullptr;
}
MultirotorMixer *
mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf)
{
MultirotorMixer::Geometry geometry;
char geomname[8];
int s[4];
if (sscanf(buf, "R: %s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) {
debug("multirotor parse failed on '%s'", buf);
return nullptr;
}
if (!strcmp(geomname, "4+")) {
geometry = MultirotorMixer::QUAD_PLUS;
} else if (!strcmp(geomname, "4x")) {
geometry = MultirotorMixer::QUAD_X;
} else if (!strcmp(geomname, "6+")) {
geometry = MultirotorMixer::HEX_PLUS;
} else if (!strcmp(geomname, "6x")) {
geometry = MultirotorMixer::HEX_X;
} else if (!strcmp(geomname, "8+")) {
geometry = MultirotorMixer::OCTA_PLUS;
} else if (!strcmp(geomname, "8x")) {
geometry = MultirotorMixer::OCTA_X;
} else {
debug("unrecognised geometry '%s'", geomname);
return nullptr;
}
return new MultirotorMixer(
control_cb,
cb_handle,
geometry,
s[0] / 10000.0f,
s[1] / 10000.0f,
s[2] / 10000.0f,
s[3] / 10000.0f);
}
int
mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer *&mixer)
{
int ret;
char buf[60];
unsigned inputs;
ret = mixer_getline(fd, buf, sizeof(buf));
/* end of file or error ?*/
if (ret < 1) {
debug("getline %d", ret);
return ret;
}
/* slot is empty - allocate a null mixer */
if (buf[0] == 'Z') {
debug("got null mixer");
mixer = new NullMixer();
return 1;
}
/* is it a simple mixer? */
if (sscanf(buf, "M: %u", &inputs) == 1) {
debug("got simple mixer with %d inputs", inputs);
mixer = mixer_load_simple(control_cb, cb_handle, fd, inputs);
return (mixer == nullptr) ? -1 : 1;
}
/* is it a multirotor mixer? */
if (buf[0] == 'R') {
debug("got a multirotor mixer");
mixer = mixer_load_multirotor(control_cb, cb_handle, buf);
return (mixer == nullptr) ? -1 : 1;
}
/* we don't recognise the mixer type */
debug("unrecognized mixer type '%c'", buf[0]);
return -1;
}
} // namespace
MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
Mixer(control_cb, cb_handle),
_first(nullptr)
@ -308,14 +64,7 @@ MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
MixerGroup::~MixerGroup()
{
Mixer *mixer;
/* discard sub-mixers */
while (_first != nullptr) {
mixer = _first;
_first = mixer->_next;
delete mixer;
}
reset();
}
void
@ -332,6 +81,19 @@ MixerGroup::add_mixer(Mixer *mixer)
mixer->_next = nullptr;
}
void
MixerGroup::reset()
{
Mixer *mixer;
/* discard sub-mixers */
while (_first != nullptr) {
mixer = _first;
_first = mixer->_next;
delete mixer;
}
}
unsigned
MixerGroup::mix(float *outputs, unsigned space)
{
@ -358,44 +120,63 @@ MixerGroup::groups_required(uint32_t &groups)
}
int
MixerGroup::load_from_file(const char *path)
MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
{
if (_first != nullptr)
return -1;
int ret = -1;
const char *end = buf + buflen;
int fd = open(path, O_RDONLY);
/*
* 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;
if (fd < 0) {
debug("failed to open %s", path);
return -1;
}
/*
* 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;
for (unsigned count = 0;; count++) {
int result;
Mixer *mixer;
case 'M':
m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid);
break;
result = mixer_load(_control_cb,
_cb_handle,
fd,
mixer);
case 'R':
m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid);
break;
/* error? */
if (result < 0) {
debug("error");
return -1;
default:
/* it's probably junk or whitespace, skip a byte and retry */
buflen--;
continue;
}
/* EOF or error */
if (result < 1) {
printf("[mixer] loaded %u mixers\n", count);
debug("EOF");
/*
* 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;
} else {
/*
* There is data in the buffer that we expected to parse, but it didn't,
* so give up for now.
*/
break;
}
debug("loaded mixer %p", mixer);
add_mixer(mixer);
}
close(fd);
return 0;
/* nothing more in the buffer for us now */
return ret;
}

66
apps/systemlib/mixer/mixer_multirotor.cpp
View File

@ -54,6 +54,9 @@
#include "mixer.h"
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
#define CW (-1.0f)
#define CCW (1.0f)
@ -151,6 +154,61 @@ MultirotorMixer::~MultirotorMixer()
{
}
MultirotorMixer *
MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
{
MultirotorMixer::Geometry geometry;
char geomname[8];
int s[4];
int used;
if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
debug("multirotor parse failed on '%s'", buf);
return nullptr;
}
if (used > (int)buflen) {
debug("multirotor spec used %d of %u", used, buflen);
return nullptr;
}
buflen -= used;
if (!strcmp(geomname, "4+")) {
geometry = MultirotorMixer::QUAD_PLUS;
} else if (!strcmp(geomname, "4x")) {
geometry = MultirotorMixer::QUAD_X;
} else if (!strcmp(geomname, "6+")) {
geometry = MultirotorMixer::HEX_PLUS;
} else if (!strcmp(geomname, "6x")) {
geometry = MultirotorMixer::HEX_X;
} else if (!strcmp(geomname, "8+")) {
geometry = MultirotorMixer::OCTA_PLUS;
} else if (!strcmp(geomname, "8x")) {
geometry = MultirotorMixer::OCTA_X;
} else {
debug("unrecognised geometry '%s'", geomname);
return nullptr;
}
debug("adding multirotor mixer '%s'", geomname);
return new MultirotorMixer(
control_cb,
cb_handle,
geometry,
s[0] / 10000.0f,
s[1] / 10000.0f,
s[2] / 10000.0f,
s[3] / 10000.0f);
}
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
@ -170,10 +228,12 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* keep roll, pitch and yaw control to 0 below min thrust */
if (thrust <= min_thrust) {
output_factor = 0.0f;
/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
} else if (thrust < startpoint_full_control && thrust > min_thrust) {
output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust);
/* and then stay at full control */
output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
/* and then stay at full control */
} else {
output_factor = max_thrust;
}

334
apps/systemlib/mixer/mixer_simple.cpp Normal file
View File

@ -0,0 +1,334 @@
/****************************************************************************
*
* 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_simple.cpp
*
* Simple summing mixer.
*/
#include <nuttx/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 <ctype.h>
#include "mixer.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),
_info(mixinfo)
{
}
SimpleMixer::~SimpleMixer()
{
if (_info != nullptr)
free(_info);
}
static const char *
findtag(const char *buf, unsigned &buflen, char tag)
{
while (buflen >= 2) {
if ((buf[0] == tag) && (buf[1] == ':'))
return buf;
buf++;
buflen--;
}
return nullptr;
}
static void
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)))
buflen -= (p - buf);
}
int
SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
{
int ret;
int s[5];
buf = findtag(buf, buflen, 'O');
if ((buf == nullptr) || (buflen < 12))
return -1;
if ((ret = sscanf(buf, "O: %d %d %d %d %d",
&s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
debug("scaler parse failed on '%s' (got %d)", buf, ret);
return -1;
}
skipline(buf, buflen);
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;
}
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))
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;
}
skipline(buf, buflen);
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;
/* 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;
}
buflen -= used;
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;
if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
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))
goto out;
}
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
if (sm != nullptr) {
mixinfo = nullptr;
debug("loaded mixer with %d inputs", inputs);
} else {
debug("could not allocate memory for mixer");
}
out:
if (mixinfo != nullptr)
free(mixinfo);
return sm;
}
SimpleMixer *
SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max)
{
SimpleMixer *sm = nullptr;
mixer_simple_s *mixinfo = nullptr;
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1));
if (mixinfo == nullptr) {
debug("could not allocate memory for mixer info");
goto out;
}
mixinfo->control_count = 1;
/*
* Always pull from group 0, with the input value giving the channel.
*/
mixinfo->controls[0].control_group = 0;
mixinfo->controls[0].control_index = input;
/*
* Conversion uses both the input and output side of the mixer.
*
* The input side is used to slide the control value such that the min argument
* results in a value of zero.
*
* The output side is used to apply the scaling for the min/max values so that
* the resulting output is a -1.0 ... 1.0 value for the min...max range.
*/
mixinfo->controls[0].scaler.negative_scale = 1.0f;
mixinfo->controls[0].scaler.positive_scale = 1.0f;
mixinfo->controls[0].scaler.offset = -mid;
mixinfo->controls[0].scaler.min_output = -(mid - min);
mixinfo->controls[0].scaler.max_output = (max - mid);
mixinfo->output_scaler.negative_scale = 500.0f / (mid - min);
mixinfo->output_scaler.positive_scale = 500.0f / (max - mid);
mixinfo->output_scaler.offset = 0.0f;
mixinfo->output_scaler.min_output = -1.0f;
mixinfo->output_scaler.max_output = 1.0f;
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
if (sm != nullptr) {
mixinfo = nullptr;
debug("PWM input mixer for %d", input);
} else {
debug("could not allocate memory for PWM input mixer");
}
out:
if (mixinfo != nullptr)
free(mixinfo);
return sm;
}
unsigned
SimpleMixer::mix(float *outputs, unsigned space)
{
float sum = 0.0f;
if (_info == nullptr)
return 0;
if (space < 1)
return 0;
for (unsigned i = 0; i < _info->control_count; i++) {
float input;
_control_cb(_cb_handle,
_info->controls[i].control_group,
_info->controls[i].control_index,
input);
sum += scale(_info->controls[i].scaler, input);
}
*outputs = scale(_info->output_scaler, sum);
return 1;
}
void
SimpleMixer::groups_required(uint32_t &groups)
{
for (unsigned i = 0; i < _info->control_count; i++)
groups |= 1 << _info->controls[i].control_group;
}
int
SimpleMixer::check()
{
int ret;
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 (_info->control_count > 32)
return -2;
/* validate the output scaler */
ret = scale_check(_info->output_scaler);
if (ret != 0)
return ret;
/* validate input scalers */
for (unsigned i = 0; i < _info->control_count; i++) {
/* verify that we can fetch the control */
if (_control_cb(_cb_handle,
_info->controls[i].control_group,
_info->controls[i].control_index,
junk) != 0) {
return -3;
}
/* validate the scaler */
ret = scale_check(_info->controls[i].scaler);
if (ret != 0)
return (10 * i + ret);
}
return 0;
}

9
nuttx/configs/px4io/common/ld.script
View File

@ -74,6 +74,15 @@ SECTIONS
_etext = ABSOLUTE(.);
} > flash
/*
* Init functions (static constructors and the like)
*/
.init_section : {
_sinit = ABSOLUTE(.);
KEEP(*(.init_array .init_array.*))
_einit = ABSOLUTE(.);
} > flash
.ARM.extab : {
*(.ARM.extab*)
} > flash

3
nuttx/configs/px4io/io/appconfig
View File

@ -35,3 +35,6 @@ CONFIGURED_APPS += drivers/boards/px4io
CONFIGURED_APPS += drivers/stm32
CONFIGURED_APPS += px4io
# Mixer library from systemlib
CONFIGURED_APPS += systemlib/mixer

6
nuttx/configs/px4io/io/defconfig
View File

@ -87,6 +87,8 @@ CONFIG_ARCH_LEDS=n
CONFIG_ARCH_BUTTONS=n
CONFIG_ARCH_CALIBRATION=n
CONFIG_ARCH_DMA=n
CONFIG_ARCH_MATH_H=y
CONFIG_ARMV7M_CMNVECTOR=y
#
@ -342,8 +344,8 @@ CONFIG_DEBUG_I2C=n
CONFIG_DEBUG_INPUT=n
CONFIG_MSEC_PER_TICK=1
CONFIG_HAVE_CXX=n
CONFIG_HAVE_CXXINITIALIZE=n
CONFIG_HAVE_CXX=y
CONFIG_HAVE_CXXINITIALIZE=y
CONFIG_MM_REGIONS=1
CONFIG_MM_SMALL=y
CONFIG_ARCH_LOWPUTC=y