diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-11-15 11:32:05 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-11-15 11:32:05 +0400 |
| commit | e46d60ba6de3c3809cd7e1e8e1f0485f0290980b (patch) | |
| tree | 9147742716c57d6f7df6f0dc3b81239535b1701a /src/drivers | |
| parent | 2116966b1e481c20eee3fe95f1d8d9671fafc1f2 (diff) | |
| download | px4-firmware-e46d60ba6de3c3809cd7e1e8e1f0485f0290980b.tar.gz px4-firmware-e46d60ba6de3c3809cd7e1e8e1f0485f0290980b.tar.bz2 px4-firmware-e46d60ba6de3c3809cd7e1e8e1f0485f0290980b.zip | |
px4io driver: don’t use PX4IO_PAGE_ACTUATORS page for actuator_controls_effective
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/px4io/px4io.cpp | 25 |
1 files changed, 18 insertions, 7 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08e697b9f..1ab18fe7b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1378,18 +1378,29 @@ PX4IO::io_publish_mixed_controls() actuator_controls_effective_s controls_effective; controls_effective.timestamp = hrt_absolute_time(); - /* get actuator controls from IO */ - uint16_t act[_max_actuators]; - int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); + // XXX PX4IO_PAGE_ACTUATORS page contains actuator outputs, not inputs + // need to implement backward mixing in IO's mixed and add another page + // by now simply use control inputs here + /* get actuator outputs from IO */ + //uint16_t act[_max_actuators]; + //int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); - if (ret != OK) - return ret; + //if (ret != OK) + // return ret; /* convert from register format to float */ + //for (unsigned i = 0; i < _max_actuators; i++) + // controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); + + actuator_controls_s controls; + + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &controls); + for (unsigned i = 0; i < _max_actuators; i++) - controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); + controls_effective.control_effective[i] = controls.control[i]; - /* laxily advertise on first publication */ + /* lazily advertise on first publication */ if (_to_actuators_effective == 0) { _to_actuators_effective = orb_advertise((_primary_pwm_device ? |