More work on the mixer architecture.

Solve the multiple publishers issue with 'control groups', one group per controller.  Mixer inputs now specify both group and control offset within the group.

Avoid using %f when loading/saving mixers; use scaled integers instead.

1 files changed, 52 insertions, 18 deletions

diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index 8e468c012..57eb12f4e 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Driver/configurator for the PX4 FMU multi-purpose port.
+ * @file fmu.cpp
+ *
+ * Driver/configurator for the PX4 FMU multi-purpose port.
  */
 
 #include <nuttx/config.h>
@@ -89,13 +91,14 @@ private:
 	volatile bool	_task_should_exit;
 	bool		_armed;
 
-	MixMixer	*_mixer[_max_actuators];
+	mixer_s	*_mixer[_max_actuators];
 
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main();	
+	void		task_main();
 };
 
-namespace {
+namespace
+{
 
 FMUServo	*g_servo;
 
@@ -122,6 +125,7 @@ FMUServo::~FMUServo()
 		_task_should_exit = true;
 
 		unsigned i = 0;
+
 		do {
 			/* wait 20ms */
 			usleep(20000);
@@ -147,11 +151,13 @@ FMUServo::init()
 
 	/* do regular cdev init */
 	ret = CDev::init();
+
 	if (ret != OK)
 		return ret;
 
 	/* start the IO interface task */
 	_task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr);
+
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
 		return -errno;
@@ -176,18 +182,20 @@ FMUServo::task_main()
 		/* XXX magic numbers */
 		up_pwm_servo_init(0x3);
 		break;
+
 	case MODE_4PWM:
 		/* multi-port as 4 PWM outs */
 		/* XXX magic numbers */
 		up_pwm_servo_init(0xf);
 		break;
+
 	case MODE_NONE:
 		/* we should never get here... */
 		break;
 	}
 
 	/* subscribe to objects that we are interested in watching */
-	_t_actuators = orb_subscribe(ORB_ID(actuator_controls));
+	_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(_t_actuators, 20);		/* 50Hz update rate */
 
 	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
@@ -217,9 +225,10 @@ FMUServo::task_main()
 		/* do we have a control update? */
 		if (fds[0].revents & POLLIN) {
 			struct actuator_controls_s ac;
+			float *controls[1] = { &ac.control[0] };
 
 			/* get controls */
-			orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac);
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &ac);
 
 			/* iterate actuators */
 			for (unsigned i = 0; i < num_outputs; i++) {
@@ -228,7 +237,7 @@ FMUServo::task_main()
 				if (_mixer[i] != nullptr) {
 
 					/* mix controls to the actuator */
-					float output = mixer_mix(_mixer[i], &ac.control[0]);
+					float output = mixer_mix(_mixer[i], &controls[0]);
 
 					/* scale for PWM output 900 - 2100us */
 					up_pwm_servo_set(i, 1500 + (600 * output));
@@ -252,7 +261,7 @@ FMUServo::task_main()
 	::close(_t_armed);
 
 	/* make sure servos are off */
-	up_pwm_servo_deinit();	
+	up_pwm_servo_deinit();
 
 	/* note - someone else is responsible for restoring the GPIO config */
 
@@ -267,7 +276,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 	int ret = OK;
 	int channel;
 	struct MixInfo *mi;
-	struct MixMixer *mm, *tmm;
+	struct mixer_s *mm, *tmm;
 
 	switch (cmd) {
 	case PWM_SERVO_ARM:
@@ -284,15 +293,18 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 			ret = -EINVAL;
 			break;
 		}
+
 		/* FALLTHROUGH */
 	case PWM_SERVO_SET(0):
 	case PWM_SERVO_SET(1):
 		if (arg < 2100) {
 			channel = cmd - PWM_SERVO_SET(0);
 			up_pwm_servo_set(channel, arg);
+
 		} else {
 			ret = -EINVAL;
 		}
+
 		break;
 
 	case PWM_SERVO_GET(2):
@@ -301,20 +313,23 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 			ret = -EINVAL;
 			break;
 		}
+
 		/* FALLTHROUGH */
 	case PWM_SERVO_GET(0):
 	case PWM_SERVO_GET(1): {
-		channel = cmd - PWM_SERVO_SET(0);
-		*(servo_position_t *)arg = up_pwm_servo_get(channel);
-		break;
-	}
+			channel = cmd - PWM_SERVO_SET(0);
+			*(servo_position_t *)arg = up_pwm_servo_get(channel);
+			break;
+		}
 
 	case MIXERIOCGETMIXERCOUNT:
 		if (_mode == MODE_4PWM) {
 			*(unsigned *)arg = 4;
+
 		} else {
 			*(unsigned *)arg = 2;
 		}
+
 		break;
 
 	case MIXERIOCGETMIXER(3):
@@ -323,6 +338,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 			ret = -EINVAL;
 			break;
 		}
+
 		/* FALLTHROUGH */
 	case MIXERIOCGETMIXER(1):
 	case MIXERIOCGETMIXER(0):
@@ -340,10 +356,12 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* if MixInfo claims to be big enough, copy mixer info */
 		if (mi->num_controls >= _mixer[channel]->control_count) {
 			memcpy(&mi->mixer, _mixer[channel], MIXER_SIZE(_mixer[channel]->control_count));
+
 		} else {
 			/* just update MixInfo with actual size of the mixer */
 			mi->mixer.control_count = _mixer[channel]->control_count;
 		}
+
 		break;
 
 	case MIXERIOCSETMIXER(3):
@@ -352,22 +370,25 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 			ret = -EINVAL;
 			break;
 		}
+
 		/* FALLTHROUGH */
 	case MIXERIOCSETMIXER(1):
 	case MIXERIOCSETMIXER(0):
 		channel = cmd - MIXERIOCSETMIXER(0);
 
 		/* get the caller-supplied mixer and check */
-		mm = (struct MixMixer *)arg;
-		if (mixer_check(mm, NUM_ACTUATOR_CONTROLS)) {
+		mm = (struct mixer_s *)arg;
+
+		if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) {	/* only the attitude group is supported */
 			ret = -EINVAL;
 			break;
 		}
 
 		/* allocate local storage and copy from the caller*/
 		if (mm != nullptr) {
-			tmm = (struct MixMixer *)malloc(MIXER_SIZE(mm->control_count));
+			tmm = (struct mixer_s *)malloc(MIXER_SIZE(mm->control_count));
 			memcpy(tmm, mm, MIXER_SIZE(mm->control_count));
+
 		} else {
 			tmm = nullptr;
 		}
@@ -379,16 +400,19 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* if there was an old mixer, free it */
 		if (mm != nullptr)
 			free(mm);
+
 		break;
 
 	default:
 		ret = -ENOTTY;
 		break;
 	}
+
 	return ret;
 }
 
-namespace {
+namespace
+{
 
 enum PortMode {
 	PORT_MODE_UNSET = 0,
@@ -403,7 +427,7 @@ enum PortMode {
 PortMode g_port_mode;
 
 int
-fmu_new_mode(PortMode new_mode) 
+fmu_new_mode(PortMode new_mode)
 {
 	int fd;
 	int ret = OK;
@@ -412,6 +436,7 @@ fmu_new_mode(PortMode new_mode)
 
 	/* get hold of the GPIO configuration descriptor */
 	fd = open(GPIO_DEVICE_PATH, 0);
+
 	if (fd < 0)
 		return -errno;
 
@@ -464,15 +489,19 @@ fmu_new_mode(PortMode new_mode)
 	/* adjust GPIO config for serial mode(s) */
 	if (gpio_bits != 0)
 		ioctl(fd, GPIO_SET_ALT_1, gpio_bits);
+
 	close(fd);
 
 	/* create new PWM driver if required */
 	if (servo_mode != FMUServo::MODE_NONE) {
 		g_servo = new FMUServo(servo_mode);
+
 		if (g_servo == nullptr) {
 			ret = -ENOMEM;
+
 		} else {
 			ret = g_servo->init();
+
 			if (ret != OK) {
 				delete g_servo;
 				g_servo = nullptr;
@@ -499,14 +528,19 @@ fmu_main(int argc, char *argv[])
 	 */
 	if (!strcmp(argv[1], "mode_gpio")) {
 		new_mode = PORT_FULL_GPIO;
+
 	} else if (!strcmp(argv[1], "mode_serial")) {
 		new_mode = PORT_FULL_SERIAL;
+
 	} else if (!strcmp(argv[1], "mode_pwm")) {
 		new_mode = PORT_FULL_PWM;
+
 	} else if (!strcmp(argv[1], "mode_gpio_serial")) {
 		new_mode = PORT_GPIO_AND_SERIAL;
+
 	} else if (!strcmp(argv[1], "mode_pwm_serial")) {
 		new_mode = PORT_PWM_AND_SERIAL;
+
 	} else if (!strcmp(argv[1], "mode_pwm_gpio")) {
 		new_mode = PORT_PWM_AND_GPIO;
 	}