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 | |
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
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 25 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 6 |
2 files changed, 23 insertions, 8 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 ? diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 05897b4ce..35ef5fcf6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -185,7 +185,7 @@ mixer_tick(void) r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ - r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; + r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } @@ -201,6 +201,10 @@ mixer_tick(void) for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; + + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + } } if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { |