diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-11-16 16:06:23 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-11-16 16:06:23 +0400 |
commit | 45e158b88c1b4dec802c419265d656e706dbe5e6 (patch) | |
tree | f55279cd5e5a78d9338b27d9ffefcc5ce53ba723 | |
parent | e46d60ba6de3c3809cd7e1e8e1f0485f0290980b (diff) | |
download | px4-firmware-45e158b88c1b4dec802c419265d656e706dbe5e6.tar.gz px4-firmware-45e158b88c1b4dec802c419265d656e706dbe5e6.tar.bz2 px4-firmware-45e158b88c1b4dec802c419265d656e706dbe5e6.zip |
Fixed actuator_controls_effective on FMU
-rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 7 |
1 files changed, 4 insertions, 3 deletions
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0441566e9..6ccb8acdb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -542,7 +542,7 @@ PX4FMU::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { @@ -589,8 +589,9 @@ PX4FMU::task_main() pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output actual limited values */ - for (unsigned i = 0; i < num_outputs; i++) { - controls_effective.control_effective[i] = (float)pwm_limited[i]; + // XXX copy control values as is, need to do backward mixing of actual limited values properly + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + controls_effective.control_effective[i] = _controls.control[i]; } orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); |