Merge pull request #2636 from schn27/fix-fmu_actuator_outputs

Fix fmu actuator outputs

ROMFS/px4fmu_common/init.d/rc.usb

@@ -15,6 +15,7 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100
 mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30
 mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS -r 5
 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
+mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_1 -r 20
 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
 mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30
 mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5

src/drivers/px4fmu/fmu.cpp

@@ -151,7 +151,6 @@ private:
 	int		_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	orb_id_t	_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
-	int		_actuator_output_topic_instance;
 	pollfd	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	unsigned	_poll_fds_num;
 
@@ -178,6 +177,7 @@ private:
 	int		set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
 	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
 	void		update_pwm_rev_mask();
+	void	publish_pwm_outputs(uint16_t *values, size_t numvalues);
 
 	struct GPIOConfig {
 		uint32_t	input;
@@ -274,7 +274,6 @@ PX4FMU::PX4FMU() :
 	_groups_required(0),
 	_groups_subscribed(0),
 	_control_subs{-1},
-	_actuator_output_topic_instance(-1),
 	_poll_fds_num(0),
 	_failsafe_pwm{0},
 	_disarmed_pwm{0},
@@ -574,6 +573,25 @@ PX4FMU::update_pwm_rev_mask()
 	}
 }
 
+void
+PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) {
+	actuator_outputs_s outputs;
+	outputs.noutputs = numvalues;
+	outputs.timestamp = hrt_absolute_time();
+
+	for (size_t i = 0; i < _max_actuators; ++i) {
+		outputs.output[i] = i < numvalues ? (float)values[i] : 0;
+	}
+
+	if (_outputs_pub == nullptr) {
+		int instance = -1;
+		_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT);
+	} else {
+		orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
+	}
+}
+
+
 void
 PX4FMU::task_main()
 {
@@ -583,10 +601,6 @@ PX4FMU::task_main()
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	_param_sub = orb_subscribe(ORB_ID(parameter_update));
 
-	/* advertise the mixed control outputs */
-	actuator_outputs_s outputs;
-	memset(&outputs, 0, sizeof(outputs));
-
 #ifdef HRT_PPM_CHANNEL
 	// rc input, published to ORB
 	struct rc_input_values rc_in;
@@ -672,7 +686,7 @@ PX4FMU::task_main()
 			/* can we mix? */
 			if (_mixers != nullptr) {
 
-				unsigned num_outputs;
+				size_t num_outputs;
 
 				switch (_mode) {
 				case MODE_2PWM:
@@ -696,33 +710,27 @@ PX4FMU::task_main()
 				}
 
 				/* do mixing */
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
-				outputs.timestamp = hrt_absolute_time();
+				float outputs[_max_actuators];
+				num_outputs = _mixers->mix(outputs, num_outputs, NULL);
 
 				/* disable unused ports by setting their output to NaN */
-				for (unsigned i = 0; i < num_outputs; i++) {
-					if (i >= outputs.noutputs) {
-						outputs.output[i] = NAN_VALUE;
+				for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) {
+					if (i >= num_outputs) {
+						outputs[i] = NAN_VALUE;
 					}
 				}
 
-				uint16_t pwm_limited[num_outputs];
+				uint16_t pwm_limited[_max_actuators];
 
 				/* the PWM limit call takes care of out of band errors, NaN and constrains */
-				pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
+				pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
 
 				/* output to the servos */
-				for (unsigned i = 0; i < num_outputs; i++) {
+				for (size_t i = 0; i < num_outputs; i++) {
 					up_pwm_servo_set(i, pwm_limited[i]);
 				}
 
-				/* publish mixed control outputs */
-				if (_outputs_pub != nullptr) {
-					_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT);
-				} else {
-
-					orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
-				}
+				publish_pwm_outputs(pwm_limited, num_outputs);
 			}
 		}