diff options
Diffstat (limited to 'apps')
| -rw-r--r-- | apps/drivers/device/cdev.cpp | 4 | ||||
| -rw-r--r-- | apps/drivers/device/device.h | 6 | ||||
| -rw-r--r-- | apps/px4/fmu/fmu.cpp | 51 | ||||
| -rw-r--r-- | apps/px4/px4io/driver/px4io.cpp | 549 | ||||
| -rw-r--r-- | apps/px4/px4io/driver/uploader.cpp | 2 | ||||
| -rw-r--r-- | apps/px4/tests/test_sensors.c | 330 | ||||
| -rw-r--r-- | apps/px4io/protocol.h | 2 |
7 files changed, 343 insertions, 601 deletions
diff --git a/apps/drivers/device/cdev.cpp b/apps/drivers/device/cdev.cpp index 6b8bf0c2d..d07d26e82 100644 --- a/apps/drivers/device/cdev.cpp +++ b/apps/drivers/device/cdev.cpp @@ -74,7 +74,7 @@ static int cdev_poll(struct file *filp, struct pollfd *fds, bool setup); * Note that we use the GNU extension syntax here because we don't get designated * initialisers in gcc 4.6. */ -static const struct file_operations cdev_fops = { +const struct file_operations CDev::fops = { open : cdev_open, close : cdev_close, read : cdev_read, @@ -118,7 +118,7 @@ CDev::init() goto out; // now register the driver - ret = register_driver(_devname, &cdev_fops, 0666, (void *)this); + ret = register_driver(_devname, &fops, 0666, (void *)this); if (ret != OK) goto out; diff --git a/apps/drivers/device/device.h b/apps/drivers/device/device.h index 98dbf8bfb..9af678465 100644 --- a/apps/drivers/device/device.h +++ b/apps/drivers/device/device.h @@ -287,6 +287,12 @@ public: protected: /** + * Pointer to the default cdev file operations table; useful for + * registering clone devices etc. + */ + static const struct file_operations fops; + + /** * Check the current state of the device for poll events from the * perspective of the file. * diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 18f27d49e..6d990c46b 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -79,7 +79,7 @@ public: FMUServo(Mode mode, int update_rate); ~FMUServo(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual int init(); @@ -93,6 +93,7 @@ private: int _t_armed; orb_advert_t _t_outputs; unsigned _num_outputs; + bool _primary_pwm_device; volatile bool _task_should_exit; bool _armed; @@ -118,7 +119,7 @@ FMUServo *g_servo; } // namespace FMUServo::FMUServo(Mode mode, int update_rate) : - CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH), + CDev("fmuservo", "/dev/px4fmu"), _mode(mode), _update_rate(update_rate), _task(-1), @@ -126,6 +127,7 @@ FMUServo::FMUServo(Mode mode, int update_rate) : _t_armed(-1), _t_outputs(0), _num_outputs(0), + _primary_pwm_device(false), _task_should_exit(false), _armed(false), _mixers(nullptr) @@ -135,18 +137,16 @@ FMUServo::FMUServo(Mode mode, int update_rate) : FMUServo::~FMUServo() { if (_task != -1) { - - /* task should wake up every 100ms or so at least */ + /* tell the task we want it to go away */ _task_should_exit = true; - unsigned i = 0; - + unsigned i = 10; do { - /* wait 20ms */ - usleep(20000); + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); /* if we have given up, kill it */ - if (++i > 10) { + if (--i == 0) { task_delete(_task); break; } @@ -154,6 +154,10 @@ FMUServo::~FMUServo() } while (_task != -1); } + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + g_servo = nullptr; } @@ -170,6 +174,13 @@ FMUServo::init() if (ret != OK) return ret; + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + /* start the IO interface task */ _task = task_spawn("fmuservo", SCHED_DEFAULT, @@ -216,8 +227,12 @@ FMUServo::task_main() break; } - /* subscribe to objects that we are interested in watching */ - _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); /* 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, update_rate_in_ms); @@ -226,11 +241,13 @@ FMUServo::task_main() orb_set_interval(_t_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ - struct actuator_outputs_s outputs; + actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); - _t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); + /* advertise the mixed control outputs */ + _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &outputs); - struct pollfd fds[2]; + pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_armed; @@ -282,7 +299,7 @@ FMUServo::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - struct actuator_armed_s aa; + actuator_armed_s aa; /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); @@ -320,7 +337,7 @@ FMUServo::control_callback(uintptr_t handle, } int -FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) +FMUServo::ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; int channel; @@ -569,7 +586,7 @@ fake(int argc, char *argv[]) exit(1); } - struct actuator_controls_s ac; + actuator_controls_s ac; ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp index c3371c138..66db1c360 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,185 +61,122 @@ #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 +class PX4IO : public device::CDev { public: - PX4IO_RC(); - ~PX4IO_RC(); + PX4IO(); + ~PX4IO(); 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); + virtual int ioctl(file *filp, int cmd, unsigned long arg); private: - orb_advert_t _publication; - struct rc_input_values _input; -}; + static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; -/* 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; -} + int _serial_fd; ///< serial interface to PX4IO + hx_stream_t _io_stream; ///< HX protocol stream -PX4IO_RC::~PX4IO_RC() -{ - if (_publication != -1) - ::close(_publication); -} + int _task; ///< worker task + volatile bool _task_should_exit; -int -PX4IO_RC::init() -{ - int ret; + int _t_actuators; ///< actuator output topic + actuator_controls_s _controls; ///< actuator outputs - ret = CDev::init(); + int _t_armed; ///< system armed control topic + actuator_armed_s _armed; ///< system armed state - /* advertise ourselves as the RC input controller */ - if (ret == OK) { - _publication = orb_advertise(ORB_ID(input_rc), &_input); - if (_publication < 0) - ret = -errno; - } + orb_advert_t _t_outputs; ///< mixed outputs topic + actuator_outputs_s _outputs; ///< mixed outputs - return ret; -} + MixerGroup *_mixers; ///< loaded mixers -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; + bool _primary_pwm_device; ///< true if we are the default PWM output - if (channels > PX4IO_INPUT_CHANNELS) - return -EIO; + volatile bool _switch_armed; ///< PX4IO switch armed state + // XXX how should this work? - lock(); - for (i = 0; i < channels; i++) - pdata[i] = _input.values[i]; - unlock(); + bool _send_needed; ///< If true, we need to send a packet to IO - return i * sizeof(servo_position_t); -} + /** + * Trampoline to the worker task + */ + static void task_main_trampoline(int argc, char *argv[]); -void -PX4IO_RC::set_channels(unsigned count, const servo_position_t *data) -{ + /** + * worker task + */ + void task_main(); - ASSERT(count <= PX4IO_INPUT_CHANNELS); + /** + * Handle receiving bytes from PX4IO + */ + void io_recv(); - /* convert incoming servo position values into 0-100 range */ - lock(); - for (unsigned i = 0; i < count; i++) { - rc_input_t chn; + /** + * HX protocol callback trampoline. + */ + static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); - if (data[i] < 1000) { - chn = 0; - } else if (data[i] > 2000) { - chn = 100; - } else { - chn = (data[i] - 1000) / 10; - } + /** + * Callback invoked when we receive a whole packet from PX4IO + */ + void rx_callback(const uint8_t *buffer, size_t bytes_received); - _input.values[i] = chn; - } - _input.channel_count = count; - unlock(); + /** + * Send an update packet to PX4IO + */ + void io_send(); - /* publish to anyone that might be listening */ - if (_publication != -1) - orb_publish(ORB_ID(input_rc), _publication, &_input); + /** + * Mixer control callback; invoked to fetch a control from a specific + * group/index during mixing. + */ + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); +}; -} -class PX4IO : public device::CDev +namespace { -public: - PX4IO(); - ~PX4IO(); - - 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; - - /** command to be sent to IO */ - struct px4io_command _next_command; - /** RC channel input from IO */ - servo_position_t _rc_channel[PX4IO_INPUT_CHANNELS]; - int _rc_channel_count; - - volatile bool _armed; - volatile bool _task_should_exit; - - bool _send_needed; - - hx_stream_t _io_stream; - - static void task_main_trampoline(int argc, char *argv[]); - void task_main(); - void io_recv(); - - 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); +PX4IO *g_dev; - void io_send(); -}; +} 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), + _primary_pwm_device(false), + _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,19 +184,18 @@ PX4IO::PX4IO() : PX4IO::~PX4IO() { - if (_rc != nullptr) - delete _rc; if (_task != -1) { - /* task should wake up every 100ms or so at least */ + /* tell the task we want it to go away */ _task_should_exit = true; - unsigned i = 0; + /* spin waiting for the thread to stop */ + unsigned i = 10; do { - /* wait 20ms */ - usleep(20000); + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); /* if we have given up, kill it */ - if (++i > 10) { + if (--i == 0) { task_delete(_task); break; } @@ -267,6 +203,14 @@ PX4IO::~PX4IO() } while (_task != -1); } + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + /* kill the HX stream */ + if (_io_stream != nullptr) + hx_stream_free(_io_stream); + g_dev = nullptr; } @@ -277,17 +221,20 @@ 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) return ret; + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (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) { debug("task start failed: %d", errno); return -errno; @@ -305,36 +252,61 @@ 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); + 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 */ - /* poll descriptor(s) */ - struct pollfd fds[1]; - fds[0].fd = _fd; + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); + /* 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(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &_outputs); + + /* poll descriptor */ + 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; + + log("ready"); /* loop handling received serial bytes */ while (!_task_should_exit) { /* sleep waiting for data, but no more than 100ms */ - int ret = ::poll(&fds[0], 1, 100); + int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 1000); /* this would be bad... */ if (ret < 0) { @@ -347,9 +319,36 @@ 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,23 +356,40 @@ 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) - hx_stream_rx(_io_stream, c); + uint8_t buf[32]; + int count; + + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_serial_fd, buf, sizeof(buf)); + + /* pass received bytes to the packet decoder */ + for (int i = 0; i < count; i++) + hx_stream_rx(_io_stream, buf[i]); } void @@ -385,98 +401,139 @@ PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_receiv void PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) { - const struct px4io_report *rep = (const struct px4io_report *)buffer; + const px4io_report *rep = (const px4io_report *)buffer; /* sanity-check the received frame size */ - if (bytes_received != sizeof(struct px4io_report)) + if (bytes_received != sizeof(px4io_report)) return; - lock(); + /* XXX handle R/C inputs here ... needs code sharing/library */ - /* pass RC input data to the driver */ - if (_rc != nullptr) - _rc->set_channels(rep->channel_count, &rep->rc_channel[0]); - _armed = rep->armed; - - /* send an update frame */ - _send_needed = true; + /* remember the latched arming switch state */ + _switch_armed = rep->armed; unlock(); - } 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) +PX4IO::ioctl(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): + /* fake an update to the selected servo channel */ + if ((arg >= 900) && (arg <= 2100)) { + _outputs.output[cmd - PWM_SERVO_SET(0)] = arg; + _send_needed = true; + } else { + ret = -EINVAL; } + break; - /* channel get? */ - if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PX4IO_INPUT_CHANNELS))) { - int channel = cmd - PWM_SERVO_GET(0); + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): + /* copy the current output value from the channel */ + *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; + break; - /* currently no data for this channel */ - if (channel >= _rc_channel_count) { - ret = -ERANGE; - break; + case MIXERIOCGETOUTPUTCOUNT: + *(unsigned *)arg = _max_actuators; + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + /* build the new mixer from the supplied argument */ + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + /* validate the new mixer */ + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + /* if we don't have a group yet, allocate one */ + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + /* add the new mixer to the group */ + _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: { + 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; + default: /* not a recognised value */ ret = -ENOTTY; } @@ -492,28 +549,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 +590,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'"); } diff --git a/apps/px4/px4io/driver/uploader.cpp b/apps/px4/px4io/driver/uploader.cpp index 7d6a9e171..0fbbac839 100644 --- a/apps/px4/px4io/driver/uploader.cpp +++ b/apps/px4/px4io/driver/uploader.cpp @@ -318,7 +318,7 @@ PX4IO_Uploader::verify() ret = recv(c); if (ret != OK) { - log("%d: got %d waiting for bytes", ret); + log("%d: got %d waiting for bytes", base + i, ret); return ret; } diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c index 87ea7f058..c801869ab 100644 --- a/apps/px4/tests/test_sensors.c +++ b/apps/px4/tests/test_sensors.c @@ -56,10 +56,6 @@ #include "tests.h" -#include <arch/board/drv_lis331.h> -#include <arch/board/drv_bma180.h> -#include <arch/board/drv_l3gd20.h> -#include <arch/board/drv_hmc5883l.h> #include <drivers/drv_accel.h> /**************************************************************************** @@ -75,10 +71,6 @@ ****************************************************************************/ //static int lis331(int argc, char *argv[]); -static int l3gd20(int argc, char *argv[]); -static int bma180(int argc, char *argv[]); -static int hmc5883l(int argc, char *argv[]); -static int ms5611(int argc, char *argv[]); static int mpu6000(int argc, char *argv[]); /**************************************************************************** @@ -90,12 +82,7 @@ struct { const char *path; int (* test)(int argc, char *argv[]); } sensors[] = { - {"bma180", "/dev/bma180", bma180}, {"mpu6000", "/dev/accel", mpu6000}, - {"l3gd20", "/dev/l3gd20", l3gd20}, - {"hmc5883l", "/dev/hmc5883l", hmc5883l}, - {"ms5611", "/dev/ms5611", ms5611}, -// {"lis331", "/dev/lis331", lis331}, {NULL, NULL, NULL} }; @@ -107,226 +94,6 @@ struct { * Private Functions ****************************************************************************/ -//static int -//lis331(int argc, char *argv[]) -//{ -// int fd; -// int16_t buf[3]; -// int ret; -// -// fd = open("/dev/lis331", O_RDONLY); -// if (fd < 0) { -// printf("\tlis331: not present on PX4FMU v1.5 and later\n"); -// return ERROR; -// } -// -// if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) || -// ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) { -// -// printf("LIS331: ioctl fail\n"); -// return ERROR; -// } -// -// /* wait at least 100ms, sensor should have data after no more than 20ms */ -// usleep(100000); -// -// /* read data - expect samples */ -// ret = read(fd, buf, sizeof(buf)); -// if (ret != sizeof(buf)) { -// printf("LIS331: read1 fail (%d)\n", ret); -// return ERROR; -// } -// -// /* read data - expect no samples (should not be ready again yet) */ -// ret = read(fd, buf, sizeof(buf)); -// if (ret != 0) { -// printf("LIS331: read2 fail (%d)\n", ret); -// return ERROR; -// } -// -// /* XXX more tests here */ -// -// return 0; -//} - -static int -l3gd20(int argc, char *argv[]) -{ - printf("\tL3GD20: test start\n"); - fflush(stdout); - - int fd; - int16_t buf[3] = {0, 0, 0}; - int ret; - - fd = open("/dev/l3gd20", O_RDONLY | O_NONBLOCK); - - if (fd < 0) { - printf("L3GD20: open fail\n"); - return ERROR; - } - -// if (ioctl(fd, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_50HZ) || -// ioctl(fd, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { -// -// printf("L3GD20: ioctl fail\n"); -// return ERROR; -// } else { -// printf("\tconfigured..\n"); -// } -// -// /* wait at least 100ms, sensor should have data after no more than 2ms */ -// usleep(100000); - - - - /* read data - expect samples */ - ret = read(fd, buf, sizeof(buf)); - - if (ret != sizeof(buf)) { - printf("\tL3GD20: read1 fail (%d should have been %d)\n", ret, sizeof(buf)); - //return ERROR; - - } else { - printf("\tL3GD20 values #1: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); - } - - /* wait at least 2 ms, sensor should have data after no more than 1.5ms */ - usleep(2000); - - /* read data - expect no samples (should not be ready again yet) */ - ret = read(fd, buf, sizeof(buf)); - - if (ret != sizeof(buf)) { - printf("\tL3GD20: read2 fail (%d)\n", ret); - close(fd); - return ERROR; - - } else { - printf("\tL3GD20 values #2: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); - } - - /* empty sensor buffer */ - ret = 0; - - while (ret != sizeof(buf)) { - // Keep reading until successful - ret = read(fd, buf, sizeof(buf)); - } - - /* test if FIFO is operational */ - usleep(14800); // Expecting 10 measurements - - ret = 0; - int count = 0; - bool dataready = true; - - while (dataready) { - // Keep reading until successful - ret = read(fd, buf, sizeof(buf)); - - if (ret != sizeof(buf)) { - dataready = false; - - } else { - count++; - } - } - - printf("\tL3GD20: Drained FIFO with %d values (expected 8-12)\n", count); - - /* read data - expect no samples (should not be ready again yet) */ - ret = read(fd, buf, sizeof(buf)); - - if (ret != 0) { - printf("\tL3GD20: Note: read3 got data - there should not have been data ready\n", ret); -// return ERROR; - } - - close(fd); - - /* Let user know everything is ok */ - printf("\tOK: L3GD20 passed all tests successfully\n"); - return OK; -} - -static int -bma180(int argc, char *argv[]) -{ - // XXX THIS SENSOR IS OBSOLETE - // TEST REMAINS, BUT ALWAYS RETURNS OK - - printf("\tBMA180: test start\n"); - fflush(stdout); - - int fd; - int16_t buf[3] = {0, 0, 0}; - int ret; - - fd = open("/dev/bma180", O_RDONLY); - - if (fd < 0) { - printf("\tBMA180: open fail\n"); - return OK; - } - -// if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) || -// ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) { -// -// printf("BMA180: ioctl fail\n"); -// return ERROR; -// } -// - /* wait at least 100ms, sensor should have data after no more than 20ms */ - usleep(100000); - - /* read data - expect samples */ - ret = read(fd, buf, sizeof(buf)); - - if (ret != sizeof(buf)) { - printf("\tBMA180: read1 fail (%d)\n", ret); - close(fd); - return OK; - - } else { - printf("\tBMA180 values: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); - } - - /* wait at least 10ms, sensor should have data after no more than 2ms */ - usleep(100000); - - ret = read(fd, buf, sizeof(buf)); - - if (ret != sizeof(buf)) { - printf("\tBMA180: read2 fail (%d)\n", ret); - close(fd); - return OK; - - } else { - printf("\tBMA180: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); - } - - /* empty sensor buffer */ - ret = 0; - - while (ret != sizeof(buf)) { - // Keep reading until successful - ret = read(fd, buf, sizeof(buf)); - } - - ret = read(fd, buf, sizeof(buf)); - - if (ret != 0) { - printf("\tBMA180: Note: read3 got data - there should not have been data ready\n", ret); - } - - /* Let user know everything is ok */ - printf("\tOK: BMA180 passed all tests successfully\n"); - close(fd); - - return OK; -} - static int mpu6000(int argc, char *argv[]) { @@ -379,103 +146,6 @@ mpu6000(int argc, char *argv[]) return OK; } -static int -ms5611(int argc, char *argv[]) -{ - printf("\tMS5611: test start\n"); - fflush(stdout); - - int fd; - float buf[3] = {0.0f, 0.0f, 0.0f}; - int ret; - - fd = open("/dev/ms5611", O_RDONLY); - - if (fd < 0) { - printf("\tMS5611: open fail\n"); - return ERROR; - } - - for (int i = 0; i < 5; i++) { - /* read data - expect samples */ - ret = read(fd, buf, sizeof(buf)); - - if (ret != sizeof(buf)) { - - if ((int8_t)ret == -EAGAIN || (int8_t)ret == -EINPROGRESS) { - /* waiting for device to become ready, this is not an error */ - } else { - printf("\tMS5611: read fail (%d)\n", ret); - close(fd); - return ERROR; - } - - } else { - - /* hack for float printing */ - int32_t pressure_int = buf[0]; - int32_t altitude_int = buf[1]; - int32_t temperature_int = buf[2]; - - printf("\tMS5611: pressure:%d.%03d mbar - altitude: %d.%02d meters - temp:%d.%02d deg celcius\n", pressure_int, (int)(buf[0] * 1000 - pressure_int * 1000), altitude_int, (int)(buf[1] * 100 - altitude_int * 100), temperature_int, (int)(buf[2] * 100 - temperature_int * 100)); - } - - /* wait at least 10ms, sensor should have data after no more than 6.5ms */ - usleep(10000); - } - - close(fd); - - /* Let user know everything is ok */ - printf("\tOK: MS5611 passed all tests successfully\n"); - - return OK; -} - -static int -hmc5883l(int argc, char *argv[]) -{ - printf("\tHMC5883L: test start\n"); - fflush(stdout); - - int fd; - int16_t buf[7] = {0, 0, 0}; - int ret; - - fd = open("/dev/hmc5883l", O_RDONLY); - - if (fd < 0) { - printf("\tHMC5883L: open fail\n"); - return ERROR; - } - - int i; - - for (i = 0; i < 5; i++) { - /* wait at least 7ms, sensor should have data after no more than 6.5ms */ - usleep(7000); - - /* read data - expect samples */ - ret = read(fd, buf, sizeof(buf)); - - if (ret != sizeof(buf)) { - printf("\tHMC5883L: read1 fail (%d) values: x:%d\ty:%d\tz:%d\n", ret, buf[0], buf[1], buf[2]); - close(fd); - return ERROR; - - } else { - printf("\tHMC5883L: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); - } - } - - close(fd); - - /* Let user know everything is ok */ - printf("\tOK: HMC5883L passed all tests successfully\n"); - - return OK; -} - /**************************************************************************** * Public Functions ****************************************************************************/ diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 96e0ca350..edc2c4bd2 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -39,6 +39,8 @@ * messages and the corresponding complexity involved. */ +#pragma once + /* * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL * TREES ARE MERGED. |