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

Merged latest master

This commit is contained in:
Lorenz Meier 2013-01-06 11:32:08 +01:00
parent 8eb8909a3c 58309fd6a8
commit 34d078b556
37 changed files with 1244 additions and 681 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -11,6 +11,7 @@ Make.dep
*.o
*.a
*~
*.dSYM
Images/*.bin
Images/*.px4
nuttx/Make.defs
@ -40,3 +41,4 @@ nsh_romfsimg.h
cscope.out
.configX-e
nuttx-export.zip
dot.gdbinit

3
apps/Makefile
View File

@ -112,6 +112,9 @@ endef
$(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN))))
# EXTERNAL_APPS is used to add out of tree apps to the build
INSTALLED_APPS += $(EXTERNAL_APPS)
# The external/ directory may also be added to the INSTALLED_APPS. But there
# is no external/ directory in the repository. Rather, this directory may be
# provided by the user (possibly as a symbolic link) to add libraries and

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:

3
apps/drivers/drv_pwm_output.h
View File

@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
/** set update rate in Hz */
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

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

@ -503,6 +503,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
// up_pwm_servo_arm(false);
break;
case PWM_SERVO_SET_UPDATE_RATE:
g_hil->set_pwm_rate(arg);
break;
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@ -577,26 +581,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
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);
@ -605,10 +602,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
default:
ret = -ENOTTY;
break;

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

@ -470,6 +470,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(false);
break;
case PWM_SERVO_SET_UPDATE_RATE:
set_pwm_rate(arg);
break;
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@ -544,19 +548,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
@ -564,8 +560,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
} 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 +569,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}

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

@ -61,6 +61,7 @@
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
@ -92,7 +93,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
@ -116,10 +117,13 @@ 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
uint32_t _relays; ///< state of the PX4IO relays, one bit per relay
volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work?
@ -161,6 +165,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.
@ -190,8 +199,10 @@ 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),
_send_needed(false),
_config_needed(false)
@ -212,6 +223,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 +254,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 +266,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 +288,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 +310,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"),
@ -338,13 +356,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) {
@ -361,26 +384,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);
/* convert to PWM values */
/* scale controls to PWM (temporary measure) */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
_outputs.output[i] = 1500 + (600 * _controls.control[i]);
/* 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);
@ -399,7 +417,18 @@ 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");
@ -407,6 +436,7 @@ out:
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
::close(_serial_fd);
/* clean up the alternate device node */
@ -459,25 +489,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];
}
@ -511,13 +543,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
@ -530,17 +565,21 @@ 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);
// XXX relays
/* update relays */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
/* armed and not locked down */
cmd.arm_ok = (_armed.armed && !_armed.lockdown);
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret)
debug("send error %d", ret);
}
@ -554,10 +593,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)
{
@ -579,9 +654,14 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
_send_needed = true;
break;
case PWM_SERVO_SET_UPDATE_RATE:
// not supported yet
ret = -EINVAL;
break;
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;
@ -597,68 +677,53 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break;
case GPIO_RESET:
_relays = 0;
_send_needed = true;
break;
case GPIO_SET:
case GPIO_CLEAR:
/* make sure only valid bits are being set */
if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) {
ret = EINVAL;
break;
}
if (cmd == GPIO_SET) {
_relays |= arg;
} else {
_relays &= ~arg;
}
_send_needed = true;
break;
case GPIO_GET:
*(uint32_t *)arg = _relays;
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:
@ -700,6 +765,13 @@ test(void)
close(fd);
actuator_armed_s aa;
aa.armed = true;
aa.lockdown = false;
orb_advertise(ORB_ID(actuator_armed), &aa);
exit(0);
}
@ -719,6 +791,7 @@ monitor(void)
if (fds[0].revents == POLLIN) {
int c;
read(0, &c, 1);
if (cancels-- == 0)
exit(0);
}
@ -805,6 +878,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);
}

3
apps/mavlink/mavlink_log.c
View File

@ -40,6 +40,7 @@
*/
#include <string.h>
#include <stdlib.h>
#include "mavlink_log.h"
@ -51,7 +52,7 @@ void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) {
}
int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) {
return lb->count == lb->size;
return lb->count == (int)lb->size;
}
int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) {

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

44
apps/px4io/comms.c
View File

@ -69,6 +69,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)
{
@ -76,6 +78,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;
@ -112,6 +117,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]);
}
@ -125,6 +131,7 @@ 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) &&
(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
@ -134,6 +141,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;
@ -211,31 +219,45 @@ 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;
/* XXX do relay changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
/* handle relay state changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
if (system_state.relays[i] != cmd->relay_state[i]) {
switch (i) {
case 0:
POWER_ACC1(cmd->relay_state[i]);
break;
case 1:
POWER_ACC2(cmd->relay_state[i]);
break;
case 2:
POWER_RELAY1(cmd->relay_state[i]);
break;
case 3:
POWER_RELAY2(cmd->relay_state[i]);
break;
}
}
system_state.relays[i] = cmd->relay_state[i];
}
irqrestore(flags);
}
static void
comms_handle_frame(void *arg, const void *buffer, size_t length)
{
@ -248,9 +270,15 @@ 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:
break;
}

2
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];

12
apps/px4io/dsm.c
View File

@ -97,6 +97,7 @@ dsm_init(const char *device)
dsm_guess_format(true);
debug("DSM: ready");
} else {
debug("DSM: open failed");
}
@ -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;
/*
@ -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);
@ -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");
@ -322,6 +329,7 @@ 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;
/*
@ -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;
}

131
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;
}
}

50
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
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
@ -79,4 +75,34 @@ struct px4io_report {
uint8_t overcurrent;
};
/**
* 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)

5
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));

15
apps/px4io/px4io.h
View File

@ -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
@ -145,8 +149,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))
@ -161,6 +165,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.

10
apps/px4io/safety.c
View File

@ -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 */
} 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);
}

28
apps/px4io/sbus.c
View File

@ -53,7 +53,7 @@
#include "debug.h"
#define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16
#define SBUS_INPUT_CHANNELS 18
static int sbus_fd = -1;
@ -87,9 +87,9 @@ sbus_init(const char *device)
partial_frame_count = 0;
last_rx_time = hrt_absolute_time();
debug("Sbus: ready");
debug("S.Bus: ready");
} else {
debug("Sbus: open failed");
debug("S.Bus: open failed");
}
return sbus_fd;
@ -117,6 +117,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 +134,7 @@ sbus_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
goto out;
last_rx_time = now;
/*
@ -205,9 +207,13 @@ sbus_decode(hrt_abstime frame_time)
return;
}
/* if the failsafe bit is set, we consider the frame invalid */
if (frame[23] & (1 << 4)) {
return;
/* if the failsafe or connection lost bit is set, we consider the frame invalid */
if ((frame[23] & (1 << 2)) && /* signal lost */
(frame[23] & (1 << 3))) { /* failsafe */
/* actively announce signal loss */
system_state.rc_channels = 0;
return 1;
}
/* we have received something we think is a frame */
@ -232,13 +238,17 @@ 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;
}
if (PX4IO_INPUT_CHANNELS >= 18) {
chancount = 18;
/* XXX decode the two switch channels */
/* decode switch channels if data fields are wide enough */
if (chancount > 17) {
/* channel 17 (index 16) */
system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998;
/* channel 18 (index 17) */
system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998;
}
/* note the number of channels decoded */

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);
}

90
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;
if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
nm = new NullMixer;
buflen -= 2;
}
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);
}
*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;
}

142
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:
*
* 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;
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;
case 'M':
m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid);
break;
case 'R':
m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid);
break;
default:
/* it's probably junk or whitespace, skip a byte and retry */
buflen--;
continue;
}
for (unsigned count = 0;; count++) {
int result;
Mixer *mixer;
/*
* If we constructed something, add it to the group.
*/
if (m != nullptr) {
add_mixer(m);
result = mixer_load(_control_cb,
_cb_handle,
fd,
mixer);
/* we constructed something */
ret = 0;
/* error? */
if (result < 0) {
debug("error");
return -1;
}
/* only adjust buflen if parsing was successful */
buflen = resid;
} else {
/* EOF or error */
if (result < 1) {
printf("[mixer] loaded %u mixers\n", count);
debug("EOF");
/*
* 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;
}

60
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)
{
@ -171,9 +229,11 @@ MultirotorMixer::mix(float *outputs, unsigned space)
if (thrust <= min_thrust) {
output_factor = 0.0f;
/* 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 */
} 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;
}

5
apps/uORB/topics/vehicle_gps_position.h
View File

@ -65,8 +65,8 @@ struct vehicle_gps_position_s
uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */
uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */
uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
float s_variance; // XXX testing
float p_variance; // XXX testing
float s_variance; /**< speed accuracy estimate cm/s */
float p_variance; /**< position accuracy estimate cm */
uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */
float vel_n; /**< GPS ground speed in m/s */
float vel_e; /**< GPS ground speed in m/s */
@ -84,7 +84,6 @@ struct vehicle_gps_position_s
/* flags */
float vel_ned_valid; /**< Flag to indicate if NED speed is valid */
};
/**

18
nuttx/configs/px4fmu/nsh/defconfig
View File

@ -246,18 +246,18 @@ CONFIG_USART6_SERIAL_CONSOLE=n
#Mavlink messages can be bigger than 128
CONFIG_USART1_TXBUFSIZE=512
CONFIG_USART2_TXBUFSIZE=128
CONFIG_USART3_TXBUFSIZE=128
CONFIG_UART4_TXBUFSIZE=128
CONFIG_UART5_TXBUFSIZE=64
CONFIG_USART2_TXBUFSIZE=256
CONFIG_USART3_TXBUFSIZE=256
CONFIG_UART4_TXBUFSIZE=256
CONFIG_UART5_TXBUFSIZE=256
CONFIG_USART6_TXBUFSIZE=128
CONFIG_USART1_RXBUFSIZE=512
CONFIG_USART2_RXBUFSIZE=128
CONFIG_USART3_RXBUFSIZE=128
CONFIG_UART4_RXBUFSIZE=128
CONFIG_UART5_RXBUFSIZE=128
CONFIG_USART6_RXBUFSIZE=128
CONFIG_USART2_RXBUFSIZE=256
CONFIG_USART3_RXBUFSIZE=256
CONFIG_UART4_RXBUFSIZE=256
CONFIG_UART5_RXBUFSIZE=256
CONFIG_USART6_RXBUFSIZE=256
CONFIG_USART1_BAUD=57600
CONFIG_USART2_BAUD=115200

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
#
@ -343,8 +345,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

6
nuttx/drivers/serial/serial.c
View File

@ -660,8 +660,10 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
int ret = dev->ops->ioctl(filep, cmd, arg);
/* If the low-level handler didn't handle the call, see if we can handle it here */
/*
* The device ioctl() handler returns -ENOTTY when it doesn't know
* how to handle the command. Check if we can handle it here.
*/
if (ret == -ENOTTY)
{
switch (cmd)

1
nuttx/include/stdio.h
View File

@ -133,6 +133,7 @@ EXTERN void perror(FAR const char *s);
EXTERN int ungetc(int c, FAR FILE *stream);
EXTERN int vprintf(FAR const char *format, va_list ap);
EXTERN int vfprintf(FAR FILE *stream, const char *format, va_list ap);
EXTERN int vdprintf(FAR int fd, const char *format, va_list ap);
EXTERN int vsprintf(FAR char *buf, const char *format, va_list ap);
EXTERN int avsprintf(FAR char **ptr, const char *fmt, va_list ap);
EXTERN int vsnprintf(FAR char *buf, size_t size, const char *format, va_list ap);

1
nuttx/lib/lib_internal.h
View File

@ -140,6 +140,7 @@ extern int lib_vsprintf(FAR struct lib_outstream_s *obj,
/* Defined lib_rawprintf.c */
extern int lib_rawvprintf(const char *src, va_list ap);
extern int lib_rawvdprintf(int fd, const char *fmt, va_list ap);
/* Defined lib_lowprintf.c */

2
nuttx/lib/stdio/Make.defs
View File

@ -50,7 +50,7 @@ CSRCS += lib_fopen.c lib_fclose.c lib_fread.c lib_libfread.c lib_fseek.c \
lib_gets.c lib_fwrite.c lib_libfwrite.c lib_fflush.c \
lib_libflushall.c lib_libfflush.c lib_rdflush.c lib_wrflush.c \
lib_fputc.c lib_puts.c lib_fputs.c lib_ungetc.c lib_vprintf.c \
lib_fprintf.c lib_vfprintf.c lib_stdinstream.c lib_stdoutstream.c \
lib_fprintf.c lib_vfprintf.c lib_vdprintf.c lib_stdinstream.c lib_stdoutstream.c \
lib_perror.c
endif
endif

15
nuttx/lib/stdio/lib_rawprintf.c
View File

@ -149,3 +149,18 @@ int lib_rawprintf(const char *fmt, ...)
return ret;
}
/****************************************************************************
* Name: lib_rawvdprintf
****************************************************************************/
int lib_rawvdprintf(int fd, const char *fmt, va_list ap)
{
/* Wrap the stdout in a stream object and let lib_vsprintf
* do the work.
*/
struct lib_rawoutstream_s rawoutstream;
lib_rawoutstream(&rawoutstream, fd);
return lib_vsprintf(&rawoutstream.public, fmt, ap);
}

81
nuttx/lib/stdio/lib_vdprintf.c Normal file
View File

@ -0,0 +1,81 @@
/****************************************************************************
* lib/stdio/lib_vdprintf.c
*
* Copyright (C) 2007-2009, 2011 Andrew Tridgell. All rights reserved.
* Author: Andrew Tridgell <andrew@tridgell.net>
*
* 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 NuttX 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.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdio.h>
#include "lib_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Type Declarations
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Global Constant Data
****************************************************************************/
/****************************************************************************
* Global Variables
****************************************************************************/
/****************************************************************************
* Private Constant Data
****************************************************************************/
/****************************************************************************
* Private Variables
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
int vdprintf(int fd, FAR const char *fmt, va_list ap)
{
return lib_rawvdprintf(fd, fmt, ap);
}

4
nuttx/tools/mkdeps.sh
View File

@ -86,10 +86,14 @@ dodep ()
fi
done
if [ -z "$fullpath" ]; then
if [ -r $1 ]; then
fullpath=$1
else
echo "# ERROR: No readable file for $1 found at any location"
show_usage
fi
fi
fi
$cc -M $cflags $fullpath || \
( echo "# ERROR: $cc -M $cflags $fullpath FAILED"; exit 4; )