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

Merged master

This commit is contained in:
Lorenz Meier 2013-01-06 10:50:41 +01:00
parent 0cc723dbc3 94fa60fa04
commit 329ac2f4ba
28 changed files with 1129 additions and 679 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 *.o
*.a *.a
*~ *~
*.dSYM
Images/*.bin Images/*.bin
Images/*.px4 Images/*.px4
nuttx/Make.defs nuttx/Make.defs
@ -40,3 +41,4 @@ nsh_romfsimg.h
cscope.out cscope.out
.configX-e .configX-e
nuttx-export.zip nuttx-export.zip
dot.gdbinit

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|\ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) 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 *************************************************************/ /* Safety switch button *************************************************************/
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) #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) #define MIXERIOCADDSIMPLE _MIXERIOC(2)
/** multirotor output definition */ /* _MIXERIOC(3) was deprecated */
struct mixer_rotor_output_s { /* _MIXERIOC(4) was deprecated */
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 */
};
/** /**
* Add a multirotor mixer in (struct mixer_multirotor_s *)arg * Add mixer(s) from the buffer in (const char *)arg
*/ */
#define MIXERIOCADDMULTIROTOR _MIXERIOC(3) #define MIXERIOCLOADBUF _MIXERIOC(5)
/**
* Add mixers(s) from a the file in (const char *)arg
*/
#define MIXERIOCLOADFILE _MIXERIOC(4)
/* /*
* XXX Thoughts for additional operations: * 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; break;
} }
case MIXERIOCADDMULTIROTOR: case MIXERIOCLOADBUF: {
/* XXX not yet supported */ const char *buf = (const char *)arg;
ret = -ENOTTY; unsigned buflen = strnlen(buf, 1024);
break;
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) { if (_mixers == nullptr) {
ret = -ENOMEM; ret = -ENOMEM;
} else { } else {
debug("loading mixers from %s", path); ret = _mixers->load_from_buf(buf, buflen);
ret = _mixers->load_from_file(path);
if (ret != 0) { if (ret != 0) {
debug("mixer load failed with %d", ret); debug("mixer load failed with %d", ret);
@ -605,10 +598,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL; ret = -EINVAL;
} }
} }
break; break;
} }
default: default:
ret = -ENOTTY; ret = -ENOTTY;
break; break;

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

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

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

@ -61,6 +61,7 @@
#include <drivers/device/device.h> #include <drivers/device/device.h>
#include <drivers/drv_rc_input.h> #include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h> #include <drivers/drv_mixer.h>
@ -91,7 +92,7 @@ public:
bool dump_one; bool dump_one;
private: private:
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS;
int _serial_fd; ///< serial interface to PX4IO int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream hx_stream_t _io_stream; ///< HX protocol stream
@ -112,10 +113,13 @@ private:
orb_advert_t _t_outputs; ///< mixed outputs topic orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs 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 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 volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work? // XXX how should this work?
@ -157,6 +161,11 @@ private:
*/ */
void config_send(); 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 * Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing. * group/index during mixing.
@ -186,8 +195,10 @@ PX4IO::PX4IO() :
_t_actuators(-1), _t_actuators(-1),
_t_armed(-1), _t_armed(-1),
_t_outputs(-1), _t_outputs(-1),
_mixers(nullptr), _mix_buf(nullptr),
_mix_buf_len(0),
_primary_pwm_device(false), _primary_pwm_device(false),
_relays(0),
_switch_armed(false), _switch_armed(false),
_send_needed(false), _send_needed(false),
_config_needed(false) _config_needed(false)
@ -208,6 +219,7 @@ PX4IO::~PX4IO()
/* give it another 100ms */ /* give it another 100ms */
usleep(100000); usleep(100000);
} }
/* well, kill it anyway, though this will probably crash */ /* well, kill it anyway, though this will probably crash */
if (_task != -1) if (_task != -1)
task_delete(_task); task_delete(_task);
@ -238,6 +250,7 @@ PX4IO::init()
/* start the IO interface task */ /* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) { if (_task < 0) {
debug("task start failed: %d", errno); debug("task start failed: %d", errno);
return -errno; return -errno;
@ -249,13 +262,16 @@ PX4IO::init()
debug("PX4IO connected"); debug("PX4IO connected");
break; break;
} }
usleep(100000); usleep(100000);
} }
if (!_connected) { if (!_connected) {
/* error here will result in everything being torn down */ /* error here will result in everything being torn down */
log("PX4IO not responding"); log("PX4IO not responding");
return -EIO; return -EIO;
} }
return OK; return OK;
} }
@ -268,7 +284,7 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
void void
PX4IO::task_main() PX4IO::task_main()
{ {
log("starting"); debug("starting");
/* open the serial port */ /* open the serial port */
_serial_fd = ::open("/dev/ttyS2", O_RDWR); _serial_fd = ::open("/dev/ttyS2", O_RDWR);
@ -290,10 +306,12 @@ PX4IO::task_main()
/* protocol stream */ /* protocol stream */
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this); _io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
if (_io_stream == nullptr) { if (_io_stream == nullptr) {
log("failed to allocate HX protocol stream"); log("failed to allocate HX protocol stream");
goto out; goto out;
} }
hx_stream_set_counters(_io_stream, hx_stream_set_counters(_io_stream,
perf_alloc(PC_COUNT, "PX4IO frames transmitted"), perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
perf_alloc(PC_COUNT, "PX4IO frames received"), perf_alloc(PC_COUNT, "PX4IO frames received"),
@ -330,13 +348,18 @@ PX4IO::task_main()
fds[2].fd = _t_armed; fds[2].fd = _t_armed;
fds[2].events = POLLIN; fds[2].events = POLLIN;
log("ready"); debug("ready");
/* lock against the ioctl handler */
lock();
/* loop handling received serial bytes */ /* loop handling received serial bytes */
while (!_task_should_exit) { while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */ /* sleep waiting for data, but no more than 100ms */
unlock();
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
lock();
/* this would be bad... */ /* this would be bad... */
if (ret < 0) { if (ret < 0) {
@ -353,26 +376,21 @@ PX4IO::task_main()
if (fds[0].revents & POLLIN) if (fds[0].revents & POLLIN)
io_recv(); 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) { if (fds[1].revents & POLLIN) {
/* get controls */ /* get controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* mix */ /* scale controls to PWM (temporary measure) */
if (_mixers != nullptr) {
/* XXX is this the right count? */
_mixers->mix(&_outputs.output[0], _max_actuators);
/* convert to PWM values */
for (unsigned i = 0; i < _max_actuators; i++) 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 */ /* and flag for update */
_send_needed = true; _send_needed = true;
} }
}
/* if we have an arming state update, handle it */
if (fds[2].revents & POLLIN) { if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
@ -391,7 +409,18 @@ PX4IO::task_main()
_config_needed = false; _config_needed = false;
config_send(); 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: out:
debug("exiting"); debug("exiting");
@ -399,6 +428,7 @@ out:
/* kill the HX stream */ /* kill the HX stream */
if (_io_stream != nullptr) if (_io_stream != nullptr)
hx_stream_free(_io_stream); hx_stream_free(_io_stream);
::close(_serial_fd); ::close(_serial_fd);
/* clean up the alternate device node */ /* clean up the alternate device node */
@ -451,25 +481,27 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{ {
const px4io_report *rep = (const px4io_report *)buffer; const px4io_report *rep = (const px4io_report *)buffer;
lock(); // lock();
/* sanity-check the received frame size */ /* sanity-check the received frame size */
if (bytes_received != sizeof(px4io_report)) { if (bytes_received != sizeof(px4io_report)) {
debug("got %u expected %u", bytes_received, sizeof(px4io_report)); debug("got %u expected %u", bytes_received, sizeof(px4io_report));
goto out; goto out;
} }
if (rep->i2f_magic != I2F_MAGIC) { if (rep->i2f_magic != I2F_MAGIC) {
debug("bad magic"); debug("bad magic");
goto out; goto out;
} }
_connected = true; _connected = true;
/* publish raw rc channel values from IO if valid channels are present */ /* publish raw rc channel values from IO if valid channels are present */
if (rep->channel_count > 0) { if (rep->channel_count > 0) {
_input_rc.timestamp = hrt_absolute_time(); _input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count; _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]; _input_rc.values[i] = rep->rc_channel[i];
} }
@ -486,13 +518,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
dump_one = false; dump_one = false;
printf("IO: %s armed ", rep->armed ? "" : "not"); printf("IO: %s armed ", rep->armed ? "" : "not");
for (unsigned i = 0; i < rep->channel_count; i++) for (unsigned i = 0; i < rep->channel_count; i++)
printf("%d: %d ", i, rep->rc_channel[i]); printf("%d: %d ", i, rep->rc_channel[i]);
printf("\n"); printf("\n");
} }
out: out:
unlock(); // unlock();
return;
} }
void void
@ -505,17 +540,21 @@ PX4IO::io_send()
/* set outputs */ /* set outputs */
for (unsigned i = 0; i < _max_actuators; i++) 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 */ /* 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); 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 */ /* armed and not locked down */
cmd.arm_ok = (_armed.armed && !_armed.lockdown); cmd.arm_ok = (_armed.armed && !_armed.lockdown);
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret) if (ret)
debug("send error %d", ret); debug("send error %d", ret);
} }
@ -529,10 +568,46 @@ PX4IO::config_send()
cfg.f2i_config_magic = F2I_CONFIG_MAGIC; cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
if (ret) if (ret)
debug("config error %d", 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 int
PX4IO::ioctl(file *filep, int cmd, unsigned long arg) PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{ {
@ -556,7 +631,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): 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)) { if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg; _outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true; _send_needed = true;
@ -572,68 +647,53 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break; 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: case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _max_actuators; *(unsigned *)arg = PX4IO_CONTROL_CHANNELS;
break; break;
case MIXERIOCRESET: case MIXERIOCRESET:
if (_mixers != nullptr) { ret = 0; /* load always resets */
delete _mixers;
_mixers = nullptr;
}
break; break;
case MIXERIOCADDSIMPLE: { case MIXERIOCLOADBUF:
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
/* build the new mixer from the supplied argument */ /* set the buffer up for transfer */
SimpleMixer *mixer = new SimpleMixer(control_callback, _mix_buf = (const char *)arg;
(uintptr_t)&_controls, mixinfo); _mix_buf_len = strnlen(_mix_buf, 1024);
/* validate the new mixer */ /* drop the lock and wait for the thread to clear the transmit */
if (mixer->check()) { unlock();
delete mixer;
ret = -EINVAL;
} else { while (_mix_buf != nullptr)
/* if we don't have a group yet, allocate one */ usleep(1000);
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
/* add the new mixer to the group */ lock();
_mixers->add_mixer(mixer);
}
} ret = 0;
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;
}
break; break;
default: default:
@ -675,6 +735,13 @@ test(void)
close(fd); close(fd);
actuator_armed_s aa;
aa.armed = true;
aa.lockdown = false;
orb_advertise(ORB_ID(actuator_armed), &aa);
exit(0); exit(0);
} }
@ -694,6 +761,7 @@ monitor(void)
if (fds[0].revents == POLLIN) { if (fds[0].revents == POLLIN) {
int c; int c;
read(0, &c, 1); read(0, &c, 1);
if (cancels-- == 0) if (cancels-- == 0)
exit(0); exit(0);
} }
@ -780,6 +848,7 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "test")) if (!strcmp(argv[1], "test"))
test(); test();
if (!strcmp(argv[1], "monitor")) if (!strcmp(argv[1], "monitor"))
monitor(); monitor();

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

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

4
apps/px4io/Makefile
View File

@ -41,7 +41,9 @@
# #
CSRCS = $(wildcard *.c) \ CSRCS = $(wildcard *.c) \
../systemlib/hx_stream.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 INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common

44
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); static void comms_handle_frame(void *arg, const void *buffer, size_t length);
perf_counter_t comms_rx_errors;
static void static void
comms_init(void) comms_init(void)
{ {
@ -77,6 +79,9 @@ comms_init(void)
fmu_fd = open("/dev/ttyS1", O_RDWR); fmu_fd = open("/dev/ttyS1", O_RDWR);
stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); 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 */ /* default state in the report to FMU */
report.i2f_magic = I2F_MAGIC; report.i2f_magic = I2F_MAGIC;
@ -110,6 +115,7 @@ comms_main(void)
if (fds.revents & POLLIN) { if (fds.revents & POLLIN) {
char buf[32]; char buf[32];
ssize_t count = read(fmu_fd, buf, sizeof(buf)); ssize_t count = read(fmu_fd, buf, sizeof(buf));
for (int i = 0; i < count; i++) for (int i = 0; i < count; i++)
hx_stream_rx(stream, buf[i]); hx_stream_rx(stream, buf[i]);
} }
@ -123,6 +129,7 @@ comms_main(void)
/* should we send a report to the FMU? */ /* should we send a report to the FMU? */
now = hrt_absolute_time(); now = hrt_absolute_time();
delta = now - last_report_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 || (delta > FMU_MAX_REPORT_INTERVAL))) {
@ -132,6 +139,7 @@ comms_main(void)
/* populate the report */ /* populate the report */
for (unsigned i = 0; i < system_state.rc_channels; i++) for (unsigned i = 0; i < system_state.rc_channels; i++)
report.rc_channel[i] = system_state.rc_channel_data[i]; report.rc_channel[i] = system_state.rc_channel_data[i];
report.channel_count = system_state.rc_channels; report.channel_count = system_state.rc_channels;
report.armed = system_state.armed; report.armed = system_state.armed;
@ -168,31 +176,45 @@ comms_handle_command(const void *buffer, size_t length)
irqstate_t flags = irqsave(); irqstate_t flags = irqsave();
/* fetch new PWM output values */ /* fetch new PWM output values */
for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
system_state.fmu_channel_data[i] = cmd->servo_command[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 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.armed = false;
}
system_state.arm_ok = cmd->arm_ok; system_state.arm_ok = cmd->arm_ok;
system_state.mixer_use_fmu = true; system_state.mixer_use_fmu = true;
system_state.fmu_data_received = true; system_state.fmu_data_received = true;
/* handle changes signalled by FMU */ /* handle changes signalled by FMU */
// if (!system_state.arm_ok && system_state.armed) // if (!system_state.arm_ok && system_state.armed)
// system_state.armed = false; // system_state.armed = false;
/* XXX do relay changes here */ /* handle relay state changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) 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]; system_state.relays[i] = cmd->relay_state[i];
}
irqrestore(flags); irqrestore(flags);
} }
static void static void
comms_handle_frame(void *arg, const void *buffer, size_t length) comms_handle_frame(void *arg, const void *buffer, size_t length)
{ {
@ -205,9 +227,15 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
case F2I_MAGIC: case F2I_MAGIC:
comms_handle_command(buffer, length); comms_handle_command(buffer, length);
break; break;
case F2I_CONFIG_MAGIC: case F2I_CONFIG_MAGIC:
comms_handle_config(buffer, length); comms_handle_config(buffer, length);
break; break;
case F2I_MIXER_MAGIC:
mixer_handle_text(buffer, length);
break;
default: default:
frame_bad++; frame_bad++;
break; break;

2
apps/px4io/controls.c
View File

@ -90,6 +90,7 @@ controls_main(void)
if (fds[0].revents & POLLIN) if (fds[0].revents & POLLIN)
locked |= dsm_input(); locked |= dsm_input();
if (fds[1].revents & POLLIN) if (fds[1].revents & POLLIN)
locked |= sbus_input(); locked |= sbus_input();
@ -139,6 +140,7 @@ ppm_input(void)
/* PPM data exists, copy it */ /* PPM data exists, copy it */
system_state.rc_channels = ppm_decoded_channels; system_state.rc_channels = ppm_decoded_channels;
for (unsigned i = 0; i < ppm_decoded_channels; i++) for (unsigned i = 0; i < ppm_decoded_channels; i++)
system_state.rc_channel_data[i] = ppm_buffer[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); dsm_guess_format(true);
debug("DSM: ready"); debug("DSM: ready");
} else { } else {
debug("DSM: open failed"); debug("DSM: open failed");
} }
@ -126,6 +127,7 @@ dsm_input(void)
* if we didn't drop bytes... * if we didn't drop bytes...
*/ */
now = hrt_absolute_time(); now = hrt_absolute_time();
if ((now - last_rx_time) > 5000) { if ((now - last_rx_time) > 5000) {
if (partial_frame_count > 0) { if (partial_frame_count > 0) {
dsm_frame_drops++; dsm_frame_drops++;
@ -142,6 +144,7 @@ dsm_input(void)
/* if the read failed for any reason, just give up here */ /* if the read failed for any reason, just give up here */
if (ret < 1) if (ret < 1)
goto out; goto out;
last_rx_time = now; last_rx_time = now;
/* /*
@ -212,6 +215,7 @@ dsm_guess_format(bool reset)
/* if the channel decodes, remember the assigned number */ /* if the channel decodes, remember the assigned number */
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
cs10 |= (1 << channel); cs10 |= (1 << channel);
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
cs11 |= (1 << channel); cs11 |= (1 << channel);
@ -247,14 +251,17 @@ dsm_guess_format(bool reset)
if (cs10 == masks[i]) if (cs10 == masks[i])
votes10++; votes10++;
if (cs11 == masks[i]) if (cs11 == masks[i])
votes11++; votes11++;
} }
if ((votes11 == 1) && (votes10 == 0)) { if ((votes11 == 1) && (votes10 == 0)) {
channel_shift = 11; channel_shift = 11;
debug("DSM: detected 11-bit format"); debug("DSM: detected 11-bit format");
return; return;
} }
if ((votes10 == 1) && (votes11 == 0)) { if ((votes10 == 1) && (votes11 == 0)) {
channel_shift = 10; channel_shift = 10;
debug("DSM: detected 10-bit format"); 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 */ /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
if (channel_shift == 11) if (channel_shift == 11)
value /= 2; value /= 2;
value += 998; value += 998;
/* /*
@ -335,14 +343,18 @@ dsm_decode(hrt_abstime frame_time)
case 0: case 0:
channel = 2; channel = 2;
break; break;
case 1: case 1:
channel = 0; channel = 0;
break; break;
case 2: case 2:
channel = 1; channel = 1;
default: default:
break; break;
} }
system_state.rc_channel_data[channel] = value; 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. * Control channel input/output mixer and failsafe.
*/ */
@ -49,8 +49,12 @@
#include <fcntl.h> #include <fcntl.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
extern "C" {
//#define DEBUG
#include "px4io.h" #include "px4io.h"
}
/* /*
* Count of periodic calls in which we have no FMU input. * Count of periodic calls in which we have no FMU input.
@ -58,28 +62,23 @@
static unsigned fmu_input_drops; static unsigned fmu_input_drops;
#define FMU_INPUT_DROP_LIMIT 20 #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 */ /* current servo arm/disarm state */
bool mixer_servos_armed = false; bool mixer_servos_armed = false;
/* /* selected control values and count for mixing */
* Each mixer consumes a set of inputs and produces a single output. static uint16_t *control_values;
*/ static int control_count;
struct mixer {
uint16_t current_value; static int mixer_callback(uintptr_t handle,
/* XXX more config here */ uint8_t control_group,
} mixers[IO_SERVO_COUNT]; uint8_t control_index,
float &control);
static MixerGroup mixer_group(mixer_callback, 0);
void void
mixer_tick(void) mixer_tick(void)
{ {
uint16_t *control_values;
int control_count;
int i;
bool should_arm; bool should_arm;
/* /*
@ -87,7 +86,7 @@ mixer_tick(void)
*/ */
if (system_state.mixer_use_fmu) { if (system_state.mixer_use_fmu) {
/* we have recent control data from the 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]; control_values = &system_state.fmu_channel_data[0];
/* check that we are receiving fresh data from the FMU */ /* 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) { if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
system_state.mixer_use_fmu = false; system_state.mixer_use_fmu = false;
} }
} else { } else {
fmu_input_drops = 0; fmu_input_drops = 0;
system_state.fmu_data_received = false; 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) { if (control_count > 0) {
for (i = 0; i < IO_SERVO_COUNT; i++) { float outputs[IO_SERVO_COUNT];
mixer_update(i, control_values, control_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 we are armed, update the servo output.
*/ */
if (system_state.armed && system_state.arm_ok) 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. * Decide whether the servos should be armed right now.
*/ */
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
if (should_arm && !mixer_servos_armed) { if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */ /* need to arm, but not armed */
up_pwm_servo_arm(true); up_pwm_servo_arm(true);
@ -145,13 +160,73 @@ mixer_tick(void)
} }
} }
static void static int
mixer_update(int mixer, uint16_t *inputs, int input_count) mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &control)
{ {
/* simple passthrough for now */ /* if the control index refers to an input that's not valid, we can't return it */
if (mixer < input_count) { if (control_index >= control_count)
mixers[mixer].current_value = inputs[mixer]; return -1;
} else {
mixers[mixer].current_value = 0; /* 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 #pragma once
#define PX4IO_OUTPUT_CHANNELS 8 #define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12 #define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 4 #define PX4IO_RELAY_CHANNELS 4
#pragma pack(push, 1) #pragma pack(push, 1)
/* command from FMU to IO */ /**
* Periodic command from FMU to IO.
*/
struct px4io_command { struct px4io_command {
uint16_t f2i_magic; 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 relay_state[PX4IO_RELAY_CHANNELS];
bool arm_ok; bool arm_ok;
}; };
/* config message from FMU to IO */ /**
struct px4io_config { * Periodic report from IO to FMU
uint16_t f2i_config_magic; */
#define F2I_CONFIG_MAGIC 0x6366
/* XXX currently nothing here */
};
/* report from IO to FMU */
struct px4io_report { struct px4io_report {
uint16_t i2f_magic; uint16_t i2f_magic;
#define I2F_MAGIC 0x7570 #define I2F_MAGIC 0x7570
@ -75,4 +71,34 @@ struct px4io_report {
uint8_t channel_count; 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) #pragma pack(pop)

5
apps/px4io/px4io.c
View File

@ -55,10 +55,15 @@
__EXPORT int user_start(int argc, char *argv[]); __EXPORT int user_start(int argc, char *argv[]);
extern void up_cxxinitialize(void);
struct sys_state_s system_state; struct sys_state_s system_state;
int user_start(int argc, char *argv[]) int user_start(int argc, char *argv[])
{ {
/* run C++ ctors before we go any further */
up_cxxinitialize();
/* reset all to zero */ /* reset all to zero */
memset(&system_state, 0, sizeof(system_state)); memset(&system_state, 0, sizeof(system_state));

15
apps/px4io/px4io.h
View File

@ -66,8 +66,7 @@
/* /*
* System state structure. * System state structure.
*/ */
struct sys_state_s struct sys_state_s {
{
bool armed; /* IO armed */ bool armed; /* IO armed */
bool arm_ok; /* FMU says OK to arm */ bool arm_ok; /* FMU says OK to arm */
@ -82,7 +81,12 @@ struct sys_state_s
/* /*
* Control signals from FMU. * 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 * Relay controls
@ -136,8 +140,8 @@ extern volatile int timers[TIMER_NUM_TIMERS];
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_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_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_SERVO_ACC2_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_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
#define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
@ -149,6 +153,7 @@ extern volatile int timers[TIMER_NUM_TIMERS];
* Mixer * Mixer
*/ */
extern void mixer_tick(void); extern void mixer_tick(void);
extern void mixer_handle_text(const void *buffer, size_t length);
/* /*
* Safety switch/LED. * 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 (safety_button_pressed && !system_state.armed) {
if (counter < ARM_COUNTER_THRESHOLD) { if (counter < ARM_COUNTER_THRESHOLD) {
counter++; counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) { } else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to armed state and notify the FMU */ /* change to armed state and notify the FMU */
system_state.armed = true; system_state.armed = true;
counter++; counter++;
system_state.fmu_report_due = true; system_state.fmu_report_due = true;
} }
/* Disarm quickly */ /* Disarm quickly */
} else if (safety_button_pressed && system_state.armed) { } else if (safety_button_pressed && system_state.armed) {
if (counter < DISARM_COUNTER_THRESHOLD) { if (counter < DISARM_COUNTER_THRESHOLD) {
counter++; counter++;
} else if (counter == DISARM_COUNTER_THRESHOLD) { } else if (counter == DISARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */ /* change to disarmed state and notify the FMU */
system_state.armed = false; system_state.armed = false;
counter++; counter++;
system_state.fmu_report_due = true; system_state.fmu_report_due = true;
} }
} else { } else {
counter = 0; counter = 0;
} }
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_SAFE; uint16_t pattern = LED_PATTERN_SAFE;
if (system_state.armed) { if (system_state.armed) {
if (system_state.arm_ok) { if (system_state.arm_ok) {
pattern = LED_PATTERN_IO_FMU_ARMED; pattern = LED_PATTERN_IO_FMU_ARMED;
} else { } else {
pattern = LED_PATTERN_IO_ARMED; pattern = LED_PATTERN_IO_ARMED;
} }
} else if (system_state.arm_ok) { } else if (system_state.arm_ok) {
pattern = LED_PATTERN_FMU_ARMED; pattern = LED_PATTERN_FMU_ARMED;
} }
@ -167,8 +175,10 @@ failsafe_blink(void *arg)
/* blink the failsafe LED if we don't have FMU input */ /* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_use_fmu) { if (!system_state.mixer_use_fmu) {
failsafe = !failsafe; failsafe = !failsafe;
} else { } else {
failsafe = false; failsafe = false;
} }
LED_AMBER(failsafe); LED_AMBER(failsafe);
} }

3
apps/px4io/sbus.c
View File

@ -117,6 +117,7 @@ sbus_input(void)
* if we didn't drop bytes... * if we didn't drop bytes...
*/ */
now = hrt_absolute_time(); now = hrt_absolute_time();
if ((now - last_rx_time) > 3000) { if ((now - last_rx_time) > 3000) {
if (partial_frame_count > 0) { if (partial_frame_count > 0) {
sbus_frame_drops++; sbus_frame_drops++;
@ -133,6 +134,7 @@ sbus_input(void)
/* if the read failed for any reason, just give up here */ /* if the read failed for any reason, just give up here */
if (ret < 1) if (ret < 1)
goto out; goto out;
last_rx_time = now; last_rx_time = now;
/* /*
@ -236,6 +238,7 @@ sbus_decode(hrt_abstime frame_time)
value |= piece; value |= piece;
} }
} }
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
system_state.rc_channel_data[channel] = (value / 2) + 998; system_state.rc_channel_data[channel] = (value / 2) + 998;
} }

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

@ -43,14 +43,17 @@
#include <unistd.h> #include <unistd.h>
#include <fcntl.h> #include <fcntl.h>
#include <errno.h> #include <errno.h>
#include <ctype.h>
#include <nuttx/compiler.h>
#include <systemlib/err.h>
#include <drivers/drv_mixer.h> #include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
__EXPORT int mixer_main(int argc, char *argv[]); __EXPORT int mixer_main(int argc, char *argv[]);
static void usage(const char *reason); static void usage(const char *reason) noreturn_function;
static void load(const char *devname, const char *fname); static void load(const char *devname, const char *fname) noreturn_function;
int int
mixer_main(int argc, char *argv[]) mixer_main(int argc, char *argv[])
@ -63,12 +66,9 @@ mixer_main(int argc, char *argv[])
usage("missing device or filename"); usage("missing device or filename");
load(argv[2], argv[3]); load(argv[2], argv[3]);
} else {
usage("unrecognised command");
} }
return 0; usage("unrecognised command");
} }
static void static void
@ -79,34 +79,58 @@ usage(const char *reason)
fprintf(stderr, "usage:\n"); fprintf(stderr, "usage:\n");
fprintf(stderr, " mixer load <device> <filename>\n"); fprintf(stderr, " mixer load <device> <filename>\n");
/* XXX automatic setups for quad, etc. */ /* XXX other useful commands? */
exit(1); exit(1);
} }
static void static void
load(const char *devname, const char *fname) load(const char *devname, const char *fname)
{ {
int dev = -1; int dev;
int ret, result = 1; FILE *fp;
char line[80];
char buf[512];
/* open the device */ /* open the device */
if ((dev = open(devname, 0)) < 0) { if ((dev = open(devname, 0)) < 0)
fprintf(stderr, "can't open %s\n", devname); err(1, "can't open %s\n", devname);
goto out;
/* 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 */ /* XXX pass the buffer to the device */
ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname); int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
if (ret != 0) { if (ret < 0)
fprintf(stderr, "failed loading %s\n", fname); err(1, "error loading mixers from %s", fname);
} exit(0);
result = 0;
out:
if (dev != -1)
close(dev);
exit(result);
} }

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

@ -137,89 +137,15 @@ NullMixer::groups_required(uint32_t &groups)
} }
/****************************************************************************/ NullMixer *
NullMixer::from_text(const char *buf, unsigned &buflen)
SimpleMixer::SimpleMixer(ControlCallback control_cb,
uintptr_t cb_handle,
mixer_simple_s *mixinfo) :
Mixer(control_cb, cb_handle),
_info(mixinfo)
{ {
NullMixer *nm = nullptr;
if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
nm = new NullMixer;
buflen -= 2;
} }
SimpleMixer::~SimpleMixer() return nm;
{
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;
} }

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

@ -234,50 +234,51 @@ public:
void add_mixer(Mixer *mixer); void add_mixer(Mixer *mixer);
/** /**
* Reads a mixer definition from a file and configures a corresponding * Remove all the mixers from the group.
* 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 * Null Mixer
* mixer. Definition lines begin with a capital letter followed by a colon. * ..........
* *
* Null Mixer: * The null mixer definition has the form:
* *
* Z: * Z:
* *
* This mixer generates a constant zero output, and is normally used to * Simple Mixer
* skip over outputs that are not in use. * ............
* *
* Simple Mixer: * A simple mixer definition begins with:
* *
* M: <scaler count> * M: <control count>
* S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*> * O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
* S: ...
* *
* The definition consists of a single-line header indicating the * The definition continues with <control count> entries describing the control
* number of scalers and then one line defining each scaler. The first * inputs and their scaling, in the form:
* scaler in the file is always the output scaler, followed by the input
* scalers.
* *
* 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 * R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
* scaled by 10000 on load/save.
* *
* Multiple mixer definitions may be stored in a single file; it is assumed that * @param buf The mixer configuration buffer.
* the reader will know how many to expect and read accordingly. * @param buflen The length of the buffer, updated to reflect
* * bytes as they are consumed.
* 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.
* @return Zero on successful load, nonzero otherwise. * @return Zero on successful load, nonzero otherwise.
*/ */
int load_from_file(const char *path); int load_from_buf(const char *buf, unsigned &buflen);
private: private:
Mixer *_first; /**< linked list of mixers */ Mixer *_first; /**< linked list of mixers */
@ -294,6 +295,21 @@ public:
NullMixer(); NullMixer();
~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 unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups); virtual void groups_required(uint32_t &groups);
}; };
@ -318,6 +334,47 @@ public:
mixer_simple_s *mixinfo); mixer_simple_s *mixinfo);
~SimpleMixer(); ~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 unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups); virtual void groups_required(uint32_t &groups);
@ -330,10 +387,18 @@ public:
* @return Zero if the mixer makes sense, nonzero otherwise. * @return Zero if the mixer makes sense, nonzero otherwise.
*/ */
int check(); int check();
protected: protected:
private: 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); float deadband);
~MultirotorMixer(); ~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 unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups); 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 { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } 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) : MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
Mixer(control_cb, cb_handle), Mixer(control_cb, cb_handle),
_first(nullptr) _first(nullptr)
@ -308,14 +64,7 @@ MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
MixerGroup::~MixerGroup() MixerGroup::~MixerGroup()
{ {
Mixer *mixer; reset();
/* discard sub-mixers */
while (_first != nullptr) {
mixer = _first;
_first = mixer->_next;
delete mixer;
}
} }
void void
@ -332,6 +81,19 @@ MixerGroup::add_mixer(Mixer *mixer)
mixer->_next = nullptr; mixer->_next = nullptr;
} }
void
MixerGroup::reset()
{
Mixer *mixer;
/* discard sub-mixers */
while (_first != nullptr) {
mixer = _first;
_first = mixer->_next;
delete mixer;
}
}
unsigned unsigned
MixerGroup::mix(float *outputs, unsigned space) MixerGroup::mix(float *outputs, unsigned space)
{ {
@ -358,44 +120,63 @@ MixerGroup::groups_required(uint32_t &groups)
} }
int int
MixerGroup::load_from_file(const char *path) MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
{ {
if (_first != nullptr) int ret = -1;
return -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); * Use the next character as a hint to decide which mixer class to construct.
return -1; */
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; * If we constructed something, add it to the group.
Mixer *mixer; */
if (m != nullptr) {
add_mixer(m);
result = mixer_load(_control_cb, /* we constructed something */
_cb_handle, ret = 0;
fd,
mixer);
/* error? */ /* only adjust buflen if parsing was successful */
if (result < 0) { buflen = resid;
debug("error"); } else {
return -1;
}
/* EOF or error */ /*
if (result < 1) { * There is data in the buffer that we expected to parse, but it didn't,
printf("[mixer] loaded %u mixers\n", count); * so give up for now.
debug("EOF"); */
break; break;
} }
debug("loaded mixer %p", mixer);
add_mixer(mixer);
} }
close(fd); /* nothing more in the buffer for us now */
return 0; return ret;
} }

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

@ -54,6 +54,9 @@
#include "mixer.h" #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 CW (-1.0f)
#define CCW (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 unsigned
MultirotorMixer::mix(float *outputs, unsigned space) MultirotorMixer::mix(float *outputs, unsigned space)
{ {
@ -171,9 +229,11 @@ MultirotorMixer::mix(float *outputs, unsigned space)
if (thrust <= min_thrust) { if (thrust <= min_thrust) {
output_factor = 0.0f; 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) { } else if (thrust < startpoint_full_control && thrust > min_thrust) {
output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust); output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
/* and then stay at full control */ /* and then stay at full control */
} else { } else {
output_factor = max_thrust; 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(.); _etext = ABSOLUTE(.);
} > flash } > flash
/*
* Init functions (static constructors and the like)
*/
.init_section : {
_sinit = ABSOLUTE(.);
KEEP(*(.init_array .init_array.*))
_einit = ABSOLUTE(.);
} > flash
.ARM.extab : { .ARM.extab : {
*(.ARM.extab*) *(.ARM.extab*)
} > flash } > flash

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

@ -35,3 +35,6 @@ CONFIGURED_APPS += drivers/boards/px4io
CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += drivers/stm32
CONFIGURED_APPS += px4io 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_BUTTONS=n
CONFIG_ARCH_CALIBRATION=n CONFIG_ARCH_CALIBRATION=n
CONFIG_ARCH_DMA=n CONFIG_ARCH_DMA=n
CONFIG_ARCH_MATH_H=y
CONFIG_ARMV7M_CMNVECTOR=y CONFIG_ARMV7M_CMNVECTOR=y
# #
@ -342,8 +344,8 @@ CONFIG_DEBUG_I2C=n
CONFIG_DEBUG_INPUT=n CONFIG_DEBUG_INPUT=n
CONFIG_MSEC_PER_TICK=1 CONFIG_MSEC_PER_TICK=1
CONFIG_HAVE_CXX=n CONFIG_HAVE_CXX=y
CONFIG_HAVE_CXXINITIALIZE=n CONFIG_HAVE_CXXINITIALIZE=y
CONFIG_MM_REGIONS=1 CONFIG_MM_REGIONS=1
CONFIG_MM_SMALL=y CONFIG_MM_SMALL=y
CONFIG_ARCH_LOWPUTC=y CONFIG_ARCH_LOWPUTC=y

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

@ -660,9 +660,9 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
int ret = dev->ops->ioctl(filep, cmd, arg); int ret = dev->ops->ioctl(filep, cmd, arg);
/* Append any higher level TTY flags */ /* If the low-level handler didn't handle the call, see if we can handle it here */
if (ret == OK) if (ret == -ENOTTY)
{ {
switch (cmd) switch (cmd)
{ {
@ -686,8 +686,43 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
irqrestore(state); irqrestore(state);
*(int *)arg = count; *(int *)arg = count;
ret = 0;
break;
} }
case FIONWRITE:
{
int count;
irqstate_t state = irqsave();
/* determine the number of bytes free in the buffer */
if (dev->xmit.head < dev->xmit.tail)
{
count = dev->xmit.tail - dev->xmit.head - 1;
}
else
{
count = dev->xmit.size - (dev->xmit.head - dev->xmit.tail) - 1;
}
irqrestore(state);
*(int *)arg = count;
ret = 0;
break;
}
}
}
/* Append any higher level TTY flags */
else if (ret == OK)
{
switch (cmd)
{
#ifdef CONFIG_SERIAL_TERMIOS #ifdef CONFIG_SERIAL_TERMIOS
case TCGETS: case TCGETS:
{ {

4
nuttx/include/nuttx/fs/ioctl.h
View File

@ -110,6 +110,10 @@
* OUT: Bytes readable from this fd * OUT: Bytes readable from this fd
*/ */
#define FIONWRITE _FIOC(0x0005) /* IN: Location to return value (int *)
* OUT: Bytes writable to this fd
*/
/* NuttX file system ioctl definitions **************************************/ /* NuttX file system ioctl definitions **************************************/
#define _DIOCVALID(c) (_IOC_TYPE(c)==_DIOCBASE) #define _DIOCVALID(c) (_IOC_TYPE(c)==_DIOCBASE)