diff options
-rw-r--r-- | apps/px4/fmu/fmu.cpp | 52 | ||||
-rw-r--r-- | apps/uORB/objects_common.cpp | 6 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_outputs.h | 68 |
3 files changed, 101 insertions, 25 deletions
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 9d8847246..83bc62509 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -64,6 +64,7 @@ #include <arch/board/up_pwm_servo.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_outputs.h> class FMUServo : public device::CDev @@ -88,6 +89,7 @@ private: int _task; int _t_actuators; int _t_armed; + int _t_outputs; unsigned _num_outputs; volatile bool _task_should_exit; @@ -98,15 +100,12 @@ private: actuator_controls_s _controls; static void task_main_trampoline(int argc, char *argv[]); - void task_main(); + void task_main() __attribute__((noreturn)); - static int control_callback_trampoline(uintptr_t handle, + static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - int control_callback(uint8_t control_group, - uint8_t control_index, - float &input); }; namespace @@ -212,6 +211,11 @@ FMUServo::task_main() _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 100); /* 10Hz update rate */ + /* advertise the mixed control outputs */ + struct actuator_output_s outputs; + memset(&outputs, 0, sizeof(outputs)); + _t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); + struct pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -237,7 +241,6 @@ FMUServo::task_main() /* do we have a control update? */ if (fds[0].revents & POLLIN) { - float outputs[num_outputs]; /* get controls - must always do this to avoid spinning */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); @@ -246,14 +249,20 @@ FMUServo::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs[0], num_outputs); + _mixers->mix(&outputs.output[0], num_outputs); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { /* scale for PWM output 900 - 2100us */ - up_pwm_servo_set(i, 1500 + (600 * outputs[i])); + outputs.output[i] = 1500 + (600 * outputs.output[i]); + + /* output to the servo */ + up_pwm_servo_set(i, outputs.output[i]); } + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); } } @@ -264,13 +273,14 @@ FMUServo::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - /* update PMW servo armed status */ + /* update PWM servo armed status */ up_pwm_servo_arm(aa.armed); } } ::close(_t_actuators); ::close(_t_armed); + ::close(_t_outputs); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -285,24 +295,14 @@ FMUServo::task_main() } int -FMUServo::control_callback_trampoline(uintptr_t handle, +FMUServo::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) { - return ((FMUServo *)handle)->control_callback(control_group, control_index, input); -} - -int -FMUServo::control_callback(uint8_t control_group, - uint8_t control_index, - float &input) -{ - /* XXX currently only supporting group zero */ - if ((control_group != 0) || (control_index > NUM_ACTUATOR_CONTROLS)) - return -1; + const actuator_controls_s *controls = (actuator_controls_s *)handle; - input = _controls.control[control_index]; + input = controls->control[control_index]; return 0; } @@ -377,7 +377,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) case MIXERIOCADDSIMPLE: { mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo); + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); if (mixer->check()) { delete mixer; @@ -385,7 +386,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) } else { if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); _mixers->add_mixer(mixer); } @@ -406,7 +408,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) _mixers = nullptr; } - _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); if (_mixers->load_from_file(path) != 0) { delete _mixers; diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index d6b566b6a..6ff66fceb 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -114,3 +114,9 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s); + +#include "topics/actuator_outputs.h" +ORB_DEFINE(actuator_outputs_0, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_1, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_2, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_3, struct actuator_output_s); diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h new file mode 100644 index 000000000..63441a059 --- /dev/null +++ b/apps/uORB/topics/actuator_outputs.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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 actuator_outputs.h + * + * Actuator output values. + * + * Values published to these topics are the outputs of the control mixing + * system as sent to the actuators (servos, motors, etc.) that operate + * the vehicle. + * + * Each topic can be published by a single output driver. + */ + +#ifndef TOPIC_ACTUATOR_OUTPUTS_H +#define TOPIC_ACTUATOR_OUTPUTS_H + +#include <stdint.h> +#include "../uORB.h" + +#define NUM_ACTUATOR_OUTPUTS 16 +#define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ + +struct actuator_output_s { + float output[NUM_ACTUATOR_OUTPUTS]; +}; + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(actuator_outputs_0); +ORB_DECLARE(actuator_outputs_1); +ORB_DECLARE(actuator_outputs_2); +ORB_DECLARE(actuator_outputs_3); + +/* output sets with pre-defined applications */ +#define ORB_ID_VEHICLE_CONTROLS ORB_ID(actuator_outputs_0) + +#endif
\ No newline at end of file |