diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-27 11:53:50 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-27 11:53:50 +0100 |
commit | 90b94b50501cd388d6acfba66bb048393860af74 (patch) | |
tree | 3693a05f8d29e5a1045e7c053fc17050f92ac86f /apps/drivers/px4io/px4io.cpp | |
parent | 7777d4416d77a683621ef8c18769f4661356e65e (diff) | |
download | px4-firmware-90b94b50501cd388d6acfba66bb048393860af74.tar.gz px4-firmware-90b94b50501cd388d6acfba66bb048393860af74.tar.bz2 px4-firmware-90b94b50501cd388d6acfba66bb048393860af74.zip |
Ported all mixers to actuator_controls_effective topic, mixers do not output the limited result yet
Diffstat (limited to 'apps/drivers/px4io/px4io.cpp')
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 18 |
1 files changed, 17 insertions, 1 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index bf23c3937..2385385a2 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -70,6 +70,7 @@ #include <systemlib/systemlib.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/rc_channels.h> @@ -102,6 +103,9 @@ private: int _t_actuators; ///< actuator output topic actuator_controls_s _controls; ///< actuator outputs + orb_advert_t _t_actuators_effective; ///< effective actuator controls topic + actuator_controls_effective_s _controls_effective; ///< effective controls + int _t_armed; ///< system armed control topic actuator_armed_s _armed; ///< system armed state @@ -184,6 +188,7 @@ PX4IO::PX4IO() : _task_should_exit(false), _connected(false), _t_actuators(-1), + _t_actuators_effective(-1), _t_armed(-1), _t_outputs(-1), _mixers(nullptr), @@ -319,6 +324,11 @@ PX4IO::task_main() _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + /* advertise the limited control inputs */ + memset(&_controls_effective, 0, sizeof(_controls_effective)); + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), + &_controls_effective); + /* advertise the mixed control outputs */ memset(&_outputs, 0, sizeof(_outputs)); _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), @@ -372,6 +382,12 @@ PX4IO::task_main() /* XXX is this the right count? */ _mixers->mix(&_outputs.output[0], _max_actuators); + // XXX output actual limited values + memcpy(&_controls_effective, &_controls, sizeof(_controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective); + + /* convert to PWM values */ for (unsigned i = 0; i < _max_actuators; i++) _outputs.output[i] = 1500 + (600 * _outputs.output[i]); @@ -496,7 +512,7 @@ PX4IO::io_send() cmd.servo_command[i] = _outputs.output[i]; /* publish as we send */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); // XXX relays |