diff options
-rw-r--r-- | apps/px4/px4io/driver/px4io.cpp | 472 |
1 files changed, 235 insertions, 237 deletions
diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp index c3371c138..954b1e891 100644 --- a/apps/px4/px4io/driver/px4io.cpp +++ b/apps/px4/px4io/driver/px4io.cpp @@ -37,8 +37,7 @@ * * PX4IO is connected via serial (or possibly some other interface at a later * point). - * - * XXX current design is racy as all hell; need a locking strategy. + */ #include <nuttx/config.h> @@ -53,6 +52,7 @@ #include <errno.h> #include <string.h> #include <stdio.h> +#include <stdlib.h> #include <unistd.h> #include <fcntl.h> @@ -61,129 +61,21 @@ #include <drivers/device/device.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> +#include <systemlib/mixer/mixer.h> +#include <drivers/drv_mixer.h> #include <systemlib/perf_counter.h> #include <systemlib/hx_stream.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/rc_channels.h> #include "px4io/protocol.h" #include "uploader.h" -class PX4IO; - - -namespace -{ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; - -PX4IO *g_dev; - -} - - -class PX4IO_RC : public device::CDev -{ -public: - PX4IO_RC(); - ~PX4IO_RC(); - - virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - - friend class PX4IO; -protected: - void set_channels(unsigned count, const servo_position_t *data); - -private: - orb_advert_t _publication; - struct rc_input_values _input; -}; - -/* XXX this may conflict with the onboard PPM input */ -PX4IO_RC::PX4IO_RC() : - CDev("px4io_rc", RC_INPUT_DEVICE_PATH), - _publication(-1) -{ - for (unsigned i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { - _input.values[i] = 0; - } - _input.channel_count = 0; -} - -PX4IO_RC::~PX4IO_RC() -{ - if (_publication != -1) - ::close(_publication); -} - -int -PX4IO_RC::init() -{ - int ret; - - ret = CDev::init(); - - /* advertise ourselves as the RC input controller */ - if (ret == OK) { - _publication = orb_advertise(ORB_ID(input_rc), &_input); - if (_publication < 0) - ret = -errno; - } - - return ret; -} - -ssize_t -PX4IO_RC::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned channels = buflen / sizeof(rc_input_t); - rc_input_t *pdata = (rc_input_t *)buffer; - unsigned i; - - if (channels > PX4IO_INPUT_CHANNELS) - return -EIO; - - lock(); - for (i = 0; i < channels; i++) - pdata[i] = _input.values[i]; - unlock(); - - return i * sizeof(servo_position_t); -} - -void -PX4IO_RC::set_channels(unsigned count, const servo_position_t *data) -{ - - ASSERT(count <= PX4IO_INPUT_CHANNELS); - - /* convert incoming servo position values into 0-100 range */ - lock(); - for (unsigned i = 0; i < count; i++) { - rc_input_t chn; - - if (data[i] < 1000) { - chn = 0; - } else if (data[i] > 2000) { - chn = 100; - } else { - chn = (data[i] - 1000) / 10; - } - - _input.values[i] = chn; - } - _input.channel_count = count; - unlock(); - - /* publish to anyone that might be listening */ - if (_publication != -1) - orb_publish(ORB_ID(input_rc), _publication, &_input); - -} class PX4IO : public device::CDev { @@ -193,53 +85,82 @@ public: virtual int init(); - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: - int _fd; - int _task; - PX4IO_RC *_rc; + static const unsigned _max_actuators = 8; - /** command to be sent to IO */ - struct px4io_command _next_command; + int _serial_fd; ///< serial interface to PX4IO + hx_stream_t _io_stream; ///< HX protocol stream - /** RC channel input from IO */ - servo_position_t _rc_channel[PX4IO_INPUT_CHANNELS]; - int _rc_channel_count; - - volatile bool _armed; + int _task; ///< worker task volatile bool _task_should_exit; - bool _send_needed; + int _t_actuators; ///< ORB subscription for actuator outputs + int _t_armed; + orb_advert_t _t_outputs; - hx_stream_t _io_stream; + MixerGroup *_mixers; + actuator_controls_s _controls; + actuator_armed_s _armed; + actuator_outputs_s _outputs; + orb_advert_t _t_input; + rc_input_values _input; + + volatile bool _switch_armed; ///< PX4IO switch armed state + // XXX how should this work? + + bool _send_needed; + + /** + * Trampoline to the worker task + */ static void task_main_trampoline(int argc, char *argv[]); + + /** + * worker task + */ void task_main(); + void io_recv(); + /** + * HX protocol callback. + */ static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); void rx_callback(const uint8_t *buffer, size_t bytes_received); void io_send(); + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); }; + +namespace +{ + +PX4IO *g_dev; + +} + PX4IO::PX4IO() : CDev("px4io", "/dev/px4io"), - _fd(-1), + _serial_fd(-1), + _io_stream(nullptr), _task(-1), - _rc(new PX4IO_RC), - _rc_channel_count(0), - _armed(false), _task_should_exit(false), - _send_needed(false), - _io_stream(nullptr) + _t_actuators(-1), + _t_armed(-1), + _t_outputs(-1), + _mixers(nullptr), + _t_input(-1), + _switch_armed(false), + _send_needed(false) { - /* set up the command we will use */ - _next_command.f2i_magic = F2I_MAGIC; - - /* we need this potentially before it could be set in px4io_main */ + /* we need this potentially before it could be set in task_main */ g_dev = this; _debug_enabled = true; @@ -247,8 +168,6 @@ PX4IO::PX4IO() : PX4IO::~PX4IO() { - if (_rc != nullptr) - delete _rc; if (_task != -1) { /* task should wake up every 100ms or so at least */ _task_should_exit = true; @@ -266,6 +185,8 @@ PX4IO::~PX4IO() } while (_task != -1); } + if (_io_stream != nullptr) + hx_stream_free(_io_stream); g_dev = nullptr; } @@ -277,10 +198,6 @@ PX4IO::init() ASSERT(_task == -1); - /* XXX send a who-are-you request */ - - /* XXX verify firmware/protocol version */ - /* do regular cdev init */ ret = CDev::init(); if (ret != OK) @@ -305,30 +222,51 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - ASSERT(_fd == -1); - - log("ready"); + log("starting"); /* open the serial port */ - _fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK); - if (_fd < 0) { + _serial_fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK); + if (_serial_fd < 0) { debug("failed to open serial port for IO: %d", errno); _task = -1; _exit(errno); } /* protocol stream */ - _io_stream = hx_stream_init(_fd, &PX4IO::rx_callback_trampoline, this); + _io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this); - perf_counter_t pc_tx_bytes = perf_alloc(PC_COUNT, "PX4IO frames transmitted"); - perf_counter_t pc_rx_bytes = perf_alloc(PC_COUNT, "PX4IO frames received"); + perf_counter_t pc_tx_frames = perf_alloc(PC_COUNT, "PX4IO frames transmitted"); + perf_counter_t pc_rx_frames = perf_alloc(PC_COUNT, "PX4IO frames received"); perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors"); - hx_stream_set_counters(_io_stream, pc_tx_bytes, pc_rx_bytes, pc_rx_errors); + hx_stream_set_counters(_io_stream, pc_tx_frames, pc_rx_frames, pc_rx_errors); + + /* XXX send a who-are-you request */ + + /* XXX verify firmware/protocol version */ + + /* subscribe to objects that we are interested in watching */ + _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + /* convert the update rate in hz to milliseconds, rounding down if necessary */ + //int update_rate_in_ms = int(1000 / _update_rate); + orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */ + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + _t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &_outputs); + + /* advertise the PX4IO R/C input */ + _t_input = orb_advertise(ORB_ID(input_rc), &_input); /* poll descriptor(s) */ - struct pollfd fds[1]; - fds[0].fd = _fd; + struct pollfd fds[3]; + fds[0].fd = _serial_fd; fds[0].events = POLLIN; + fds[1].fd = _t_actuators; + fds[1].events = POLLIN; + fds[2].fd = _t_armed; + fds[2].events = POLLIN; /* loop handling received serial bytes */ while (!_task_should_exit) { @@ -347,9 +285,35 @@ PX4IO::task_main() if (ret == 0) _send_needed = true; - /* if we have new data from IO, go handle it */ - if ((ret > 0) && (fds[0].revents & POLLIN)) - io_recv(); + if (ret > 0) { + /* if we have new data from IO, go handle it */ + if (fds[0].revents & POLLIN) + io_recv(); + + /* if we have new data from the ORB, go 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 */ + for (unsigned i = 0; i < _max_actuators; i++) + _outputs.output[i] = 1500 + (600 * _outputs.output[i]); + + /* and flag for update */ + _send_needed = true; + } + } + if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls); + _send_needed = true; + } + } /* send an update to IO if required */ if (_send_needed) { @@ -357,22 +321,31 @@ PX4IO::task_main() io_send(); } } - if (_io_stream != nullptr) - hx_stream_free(_io_stream); - ::close(_fd); /* tell the dtor that we are exiting */ _task = -1; _exit(0); } +int +PX4IO::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls->control[control_index]; + return 0; +} + void PX4IO::io_recv() { uint8_t c; /* handle bytes from IO */ - while (::read(_fd, &c, 1) == 1) + while (::read(_serial_fd, &c, 1) == 1) hx_stream_rx(_io_stream, c); } @@ -391,15 +364,10 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) if (bytes_received != sizeof(struct px4io_report)) return; - lock(); - - /* pass RC input data to the driver */ - if (_rc != nullptr) - _rc->set_channels(rep->channel_count, &rep->rc_channel[0]); - _armed = rep->armed; + /* XXX handle R/C inputs here ... needs code sharing/library */ - /* send an update frame */ - _send_needed = true; + /* remember the latched arming switch state */ + _switch_armed = rep->armed; unlock(); @@ -408,75 +376,115 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) void PX4IO::io_send() { - lock(); + px4io_command cmd; - /* send packet to IO while we're guaranteed it won't change */ - hx_stream_send(_io_stream, &_next_command, sizeof(_next_command)); + cmd.f2i_magic = F2I_MAGIC; - unlock(); -} + /* set outputs */ + for (unsigned i = 0; i < _max_actuators; i++) + cmd.servo_command[i] = _outputs.output[i]; -ssize_t -PX4IO::write(struct file *filp, const char *buffer, size_t len) -{ - unsigned channels = len / sizeof(servo_position_t); - servo_position_t *pdata = (servo_position_t *)buffer; - unsigned i; + /* publish as we send */ + orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); - if (channels > PX4IO_OUTPUT_CHANNELS) - return -EIO; + // XXX relays - lock(); - for (i = 0; i < channels; i++) - _next_command.servo_command[i] = pdata[i]; - unlock(); + cmd.arm_ok = _armed.armed; - return i * sizeof(servo_position_t); + hx_stream_send(_io_stream, &cmd, sizeof(cmd)); } int PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) { - int ret = -ENOTTY; + int ret = OK; lock(); /* regular ioctl? */ switch (cmd) { case PWM_SERVO_ARM: - _next_command.arm_ok = true; - ret = 0; + /* fake an armed transition */ + _armed.armed = true; + _send_needed = true; break; case PWM_SERVO_DISARM: - _next_command.arm_ok = false; - ret = 0; + /* fake a disarmed transition */ + _armed.armed = true; + _send_needed = true; break; - default: - /* channel set? */ - if ((cmd >= PWM_SERVO_SET(0)) && (cmd < PWM_SERVO_SET(PX4IO_OUTPUT_CHANNELS))) { - /* XXX sanity-check value? */ - _next_command.servo_command[cmd - PWM_SERVO_SET(0)] = arg; - ret = 0; - break; + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): + if ((arg >= 900) && (arg <= 2100)) { + _outputs.output[cmd - PWM_SERVO_SET(0)] = arg; + _send_needed = true; + } else { + ret = -EINVAL; } + break; + + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): + *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; + break; - /* channel get? */ - if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PX4IO_INPUT_CHANNELS))) { - int channel = cmd - PWM_SERVO_GET(0); + case MIXERIOCGETOUTPUTCOUNT: + *(unsigned *)arg = _max_actuators; + break; - /* currently no data for this channel */ - if (channel >= _rc_channel_count) { - ret = -ERANGE; - break; + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); } - *(servo_position_t *)arg = _rc_channel[channel]; - ret = 0; - break; } + 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; + } + + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + + if (_mixers->load_from_file(path) != 0) { + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + + } + break; + + default: /* not a recognised value */ ret = -ENOTTY; } @@ -492,28 +500,25 @@ px4io_main(int argc, char *argv[]) { if (!strcmp(argv[1], "start")) { - if (g_dev != nullptr) { - fprintf(stderr, "PX4IO: already loaded\n"); - return -EBUSY; - } + if (g_dev != nullptr) + errx(1, "already loaded"); /* create the driver - it will set g_dev */ (void)new PX4IO; - if (g_dev == nullptr) { - fprintf(stderr, "PX4IO: driver alloc failed\n"); - return -ENOMEM; - } + if (g_dev == nullptr) + errx(1, "driver alloc failed"); if (OK != g_dev->init()) { - fprintf(stderr, "PX4IO: driver init failed\n"); delete g_dev; - return -EIO; + errx(1, "driver init failed"); } - return OK; + exit(0); } + /* note, stop not currently implemented */ + if (!strcmp(argv[1], "update")) { PX4IO_Uploader *up; const char *fn[3]; @@ -536,26 +541,19 @@ px4io_main(int argc, char *argv[]) case OK: break; case -ENOENT: - fprintf(stderr, "PX4IO firmware file not found\n"); - break; + errx(1, "PX4IO firmware file not found"); case -EEXIST: case -EIO: - fprintf(stderr, "error updating PX4IO - check that bootloader mode is enabled\n"); - break; + errx(1, "error updating PX4IO - check that bootloader mode is enabled"); case -EINVAL: - fprintf(stderr, "verify failed - retry the update\n"); - break; + errx(1, "verify failed - retry the update"); case -ETIMEDOUT: - fprintf(stderr, "timed out waiting for bootloader - power-cycle and try again\n"); + errx(1, "timed out waiting for bootloader - power-cycle and try again"); default: - fprintf(stderr, "unexpected error %d\n", ret); - break; + errx(1, "unexpected error %d", ret); } return ret; } - - - printf("need a verb, only support 'start' and 'update'\n"); - return ERROR; + errx(1, "need a verb, only support 'start' and 'update'"); } |