px4io driver: don’t use PX4IO_PAGE_ACTUATORS page for actuator_controls_effective

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) {