From bfbd17a2fa5ef685d2d2f71ab0cedbffec6d62d7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Oct 2012 16:53:52 -0700 Subject: Make it possible to run fmu and px4io simultaneously with full control over both sets of possible PWM outputs. First started wins. --- apps/px4/fmu/fmu.cpp | 51 ++++++++++++------ apps/px4/px4io/driver/px4io.cpp | 115 +++++++++++++++++++++++++++------------- 2 files changed, 111 insertions(+), 55 deletions(-) (limited to 'apps/px4') 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 954b1e891..e38307eb8 100644 --- a/apps/px4/px4io/driver/px4io.cpp +++ b/apps/px4/px4io/driver/px4io.cpp @@ -85,10 +85,10 @@ public: virtual int init(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(file *filp, int cmd, unsigned long arg); private: - static const unsigned _max_actuators = 8; + static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; int _serial_fd; ///< serial interface to PX4IO hx_stream_t _io_stream; ///< HX protocol stream @@ -96,22 +96,23 @@ private: int _task; ///< worker task volatile bool _task_should_exit; - int _t_actuators; ///< ORB subscription for actuator outputs - int _t_armed; - orb_advert_t _t_outputs; + int _t_actuators; ///< actuator output topic + actuator_controls_s _controls; ///< actuator outputs - MixerGroup *_mixers; - actuator_controls_s _controls; - actuator_armed_s _armed; - actuator_outputs_s _outputs; + int _t_armed; ///< system armed control topic + actuator_armed_s _armed; ///< system armed state - orb_advert_t _t_input; - rc_input_values _input; + orb_advert_t _t_outputs; ///< mixed outputs topic + actuator_outputs_s _outputs; ///< mixed outputs + + MixerGroup *_mixers; ///< loaded mixers + + bool _primary_pwm_device; ///< true if we are the default PWM output volatile bool _switch_armed; ///< PX4IO switch armed state // XXX how should this work? - bool _send_needed; + bool _send_needed; ///< If true, we need to send a packet to IO /** * Trampoline to the worker task @@ -123,15 +124,30 @@ private: */ void task_main(); + /** + * Handle receiving bytes from PX4IO + */ void io_recv(); /** - * HX protocol callback. + * HX protocol callback trampoline. */ static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); + + /** + * Callback invoked when we receive a whole packet from PX4IO + */ void rx_callback(const uint8_t *buffer, size_t bytes_received); + /** + * Send an update packet to PX4IO + */ void io_send(); + + /** + * 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, @@ -156,7 +172,7 @@ PX4IO::PX4IO() : _t_armed(-1), _t_outputs(-1), _mixers(nullptr), - _t_input(-1), + _primary_pwm_device(false), _switch_armed(false), _send_needed(false) { @@ -169,22 +185,29 @@ PX4IO::PX4IO() : PX4IO::~PX4IO() { 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; } } 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); @@ -203,6 +226,13 @@ PX4IO::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); if (_task < 0) { @@ -244,8 +274,12 @@ PX4IO::task_main() /* XXX verify firmware/protocol version */ - /* 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, 20); /* XXX 50Hz hardcoded for now */ @@ -254,13 +288,11 @@ PX4IO::task_main() 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); + _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &_outputs); - /* poll descriptor(s) */ - struct pollfd fds[3]; + /* poll descriptor */ + pollfd fds[3]; fds[0].fd = _serial_fd; fds[0].events = POLLIN; fds[1].fd = _t_actuators; @@ -358,10 +390,10 @@ 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; /* XXX handle R/C inputs here ... needs code sharing/library */ @@ -395,7 +427,7 @@ PX4IO::io_send() } int -PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) +PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { int ret = OK; @@ -416,6 +448,7 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) 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; @@ -425,6 +458,7 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) break; 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; @@ -442,18 +476,22 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) 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); } @@ -466,20 +504,21 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) break; case MIXERIOCLOADFILE: { + MixerGroup *newmixers; const char *path = (const char *)arg; - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; + /* 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; } - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - - if (_mixers->load_from_file(path) != 0) { + /* swap the new mixers in for the old */ + if (_mixers != nullptr) { delete _mixers; - _mixers = nullptr; - ret = -EINVAL; } + _mixers = newmixers; } break; -- cgit v1.2.3