Make it possible to run fmu and px4io simultaneously with full control over both sets of possible PWM outputs. First started wins.

apps/drivers/device/cdev.cpp

@@ -74,7 +74,7 @@ static int	cdev_poll(struct file *filp, struct pollfd *fds, bool setup);
  * Note that we use the GNU extension syntax here because we don't get designated
  * initialisers in gcc 4.6.
  */
-static const struct file_operations cdev_fops = {
+const struct file_operations CDev::fops = {
 open	: cdev_open,
 close	: cdev_close,
 read	: cdev_read,
@@ -118,7 +118,7 @@ CDev::init()
 		goto out;
 
 	// now register the driver
-	ret = register_driver(_devname, &cdev_fops, 0666, (void *)this);
+	ret = register_driver(_devname, &fops, 0666, (void *)this);
 
 	if (ret != OK)
 		goto out;

apps/drivers/device/device.h

@@ -286,6 +286,12 @@ public:
        bool            is_open() { return _open_count > 0; }
 
 protected:
+	/**
+	 * Pointer to the default cdev file operations table; useful for
+	 * registering clone devices etc.
+	 */
+	static const struct file_operations	fops;
+
 	/**
 	 * Check the current state of the device for poll events from the
 	 * perspective of the file.

apps/px4/fmu/fmu.cpp

@@ -79,7 +79,7 @@ public:
 	FMUServo(Mode mode, int update_rate);
 	~FMUServo();
 
-	virtual int	ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
 
 	virtual int	init();
 
@@ -93,6 +93,7 @@ private:
 	int		_t_armed;
 	orb_advert_t	_t_outputs;
 	unsigned	_num_outputs;
+	bool		_primary_pwm_device;
 
 	volatile bool	_task_should_exit;
 	bool		_armed;
@@ -118,7 +119,7 @@ FMUServo	*g_servo;
 } // namespace
 
 FMUServo::FMUServo(Mode mode, int update_rate) :
-	CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH),
+	CDev("fmuservo", "/dev/px4fmu"),
 	_mode(mode),
 	_update_rate(update_rate),
 	_task(-1),
@@ -126,6 +127,7 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
 	_t_armed(-1),
 	_t_outputs(0),
 	_num_outputs(0),
+	_primary_pwm_device(false),
 	_task_should_exit(false),
 	_armed(false),
 	_mixers(nullptr)
@@ -135,18 +137,16 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
 FMUServo::~FMUServo()
 {
 	if (_task != -1) {
-
+		/* tell the task we want it to go away */
-		/* task should wake up every 100ms or so at least */
 		_task_should_exit = true;
 
-		unsigned i = 0;
+		unsigned i = 10;
-
 		do {
-			/* wait 20ms */
+			/* wait 50ms - it should wake every 100ms or so worst-case */
-			usleep(20000);
+			usleep(50000);
 
 			/* if we have given up, kill it */
-			if (++i > 10) {
+			if (--i == 0) {
 				task_delete(_task);
 				break;
 			}
@@ -154,6 +154,10 @@ FMUServo::~FMUServo()
 		} while (_task != -1);
 	}
 
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
 	g_servo = nullptr;
 }
 
@@ -170,6 +174,13 @@ FMUServo::init()
 	if (ret != OK)
 		return ret;
 
+	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
 	/* start the IO interface task */
 	_task = task_spawn("fmuservo",
 			   SCHED_DEFAULT,
@@ -216,8 +227,12 @@ FMUServo::task_main()
 		break;
 	}
 
-	/* subscribe to objects that we are interested in watching */
+	/*
-	_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
 	/* convert the update rate in hz to milliseconds, rounding down if necessary */
 	int update_rate_in_ms = int(1000 / _update_rate);
 	orb_set_interval(_t_actuators, update_rate_in_ms);
@@ -226,11 +241,13 @@ FMUServo::task_main()
 	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
-	struct actuator_outputs_s outputs;
+	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
-	_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+	/* advertise the mixed control outputs */
+	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+				   &outputs);
 
-	struct pollfd fds[2];
+	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
 	fds[1].fd = _t_armed;
@@ -282,7 +299,7 @@ FMUServo::task_main()
 
 		/* how about an arming update? */
 		if (fds[1].revents & POLLIN) {
-			struct actuator_armed_s aa;
+			actuator_armed_s aa;
 
 			/* get new value */
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
@@ -320,7 +337,7 @@ FMUServo::control_callback(uintptr_t handle,
 }
 
 int
-FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
+FMUServo::ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
 	int channel;
@@ -569,7 +586,7 @@ fake(int argc, char *argv[])
 		exit(1);
 	}
 
-	struct actuator_controls_s ac;
+	actuator_controls_s ac;
 
 	ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
 

apps/px4/px4io/driver/px4io.cpp

@@ -85,10 +85,10 @@ public:
 
 	virtual int		init();
 
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 
 private:
-	static const unsigned	_max_actuators = 8;
+	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
 
 	int			_serial_fd;	///< serial interface to PX4IO
 	hx_stream_t		_io_stream;	///< HX protocol stream
@@ -96,22 +96,23 @@ private:
 	int			_task;		///< worker task
 	volatile bool		_task_should_exit;
 
-	int			_t_actuators;	///< ORB subscription for actuator outputs
+	int			_t_actuators;	///< actuator output topic
-	int			_t_armed;
+	actuator_controls_s	_controls;	///< actuator outputs
-	orb_advert_t		_t_outputs;
 
-	MixerGroup		*_mixers;
+	int			_t_armed;	///< system armed control topic
-	actuator_controls_s	_controls;
+	actuator_armed_s	_armed;		///< system armed state
-	actuator_armed_s	_armed;
-	actuator_outputs_s	_outputs;
 
-	orb_advert_t		_t_input;
+	orb_advert_t		_t_outputs;	///< mixed outputs topic
-	rc_input_values		_input;
+	actuator_outputs_s	_outputs;	///< mixed outputs
+
+	MixerGroup		*_mixers;	///< loaded mixers
+
+	bool			_primary_pwm_device;	///< true if we are the default PWM output
 
 	volatile bool		_switch_armed;	///< PX4IO switch armed state
 						// XXX how should this work?
 
-	bool			_send_needed;
+	bool			_send_needed;	///< If true, we need to send a packet to IO
 
 	/**
 	 * Trampoline to the worker task
@@ -123,15 +124,30 @@ private:
 	 */
 	void			task_main();
 
+	/**
+	 * Handle receiving bytes from PX4IO
+	 */
 	void			io_recv();
 
 	/**
-	 * HX protocol callback.
+	 * HX protocol callback trampoline.
 	 */
 	static void		rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received);
+
+	/**
+	 * Callback invoked when we receive a whole packet from PX4IO
+	 */
 	void			rx_callback(const uint8_t *buffer, size_t bytes_received);
 
+	/**
+	 * Send an update packet to PX4IO
+	 */
 	void			io_send();
+
+	/**
+	 * Mixer control callback; invoked to fetch a control from a specific
+	 * group/index during mixing.
+	 */
 	static int		control_callback(uintptr_t handle,
 						 uint8_t control_group,
 						 uint8_t control_index,
@@ -156,7 +172,7 @@ PX4IO::PX4IO() :
 	_t_armed(-1),
 	_t_outputs(-1),
 	_mixers(nullptr),
-	_t_input(-1),
+	_primary_pwm_device(false),
 	_switch_armed(false),
 	_send_needed(false)
 {
@@ -169,22 +185,29 @@ PX4IO::PX4IO() :
 PX4IO::~PX4IO()
 {
 	if (_task != -1) {
-		/* task should wake up every 100ms or so at least */
+		/* tell the task we want it to go away */
 		_task_should_exit = true;
 
-		unsigned i = 0;
+		/* spin waiting for the thread to stop */
+		unsigned i = 10;
 		do {
-			/* wait 20ms */
+			/* wait 50ms - it should wake every 100ms or so worst-case */
-			usleep(20000);
+			usleep(50000);
 
 			/* if we have given up, kill it */
-			if (++i > 10) {
+			if (--i == 0) {
 				task_delete(_task);
 				break;
 			}
 
 		} while (_task != -1);
 	}
+
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+	/* kill the HX stream */
 	if (_io_stream != nullptr)
 		hx_stream_free(_io_stream);
 
@@ -203,6 +226,13 @@ PX4IO::init()
 	if (ret != OK)
 		return ret;
 
+	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
 	/* start the IO interface task */
 	_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr);
 	if (_task < 0) {
@@ -244,8 +274,12 @@ PX4IO::task_main()
 
 	/* XXX verify firmware/protocol version */
 
-	/* subscribe to objects that we are interested in watching */
+	/*
-	_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
 	/* convert the update rate in hz to milliseconds, rounding down if necessary */
 	//int update_rate_in_ms = int(1000 / _update_rate);
 	orb_set_interval(_t_actuators, 20);	/* XXX 50Hz hardcoded for now */
@@ -254,13 +288,11 @@ PX4IO::task_main()
 	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
-	_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &_outputs);
+	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+				   &_outputs);
 
-	/* advertise the PX4IO R/C input */
+	/* poll descriptor */
-	_t_input = orb_advertise(ORB_ID(input_rc), &_input);
+	pollfd fds[3];
-
-	/* poll descriptor(s) */
-	struct pollfd fds[3];
 	fds[0].fd = _serial_fd;
 	fds[0].events = POLLIN;
 	fds[1].fd = _t_actuators;
@@ -358,10 +390,10 @@ PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_receiv
 void
 PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 {
-	const struct px4io_report *rep = (const struct px4io_report *)buffer;
+	const px4io_report *rep = (const px4io_report *)buffer;
 
 	/* sanity-check the received frame size */
-	if (bytes_received != sizeof(struct px4io_report))
+	if (bytes_received != sizeof(px4io_report))
 		return;
 
 	/* XXX handle R/C inputs here ... needs code sharing/library */
@@ -395,7 +427,7 @@ PX4IO::io_send()
 }
 
 int
-PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
+PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 {
 	int ret = OK;
 
@@ -416,6 +448,7 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
+		/* fake an update to the selected servo channel */
 		if ((arg >= 900) && (arg <= 2100)) {
 			_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
 			_send_needed = true;
@@ -425,6 +458,7 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
+		/* copy the current output value from the channel */
 		*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
 		break;
 
@@ -442,18 +476,22 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
 	case MIXERIOCADDSIMPLE: {
 			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
 
+			/* build the new mixer from the supplied argument */
 			SimpleMixer *mixer = new SimpleMixer(control_callback,
 							     (uintptr_t)&_controls, mixinfo);
 
+			/* validate the new mixer */
 			if (mixer->check()) {
 				delete mixer;
 				ret = -EINVAL;
 
 			} else {
+				/* if we don't have a group yet, allocate one */
 				if (_mixers == nullptr)
 					_mixers = new MixerGroup(control_callback,
 								 (uintptr_t)&_controls);
 
+				/* add the new mixer to the group */
 				_mixers->add_mixer(mixer);
 			}
 
@@ -466,21 +504,22 @@ PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCLOADFILE: {
+			MixerGroup *newmixers;
 			const char *path = (const char *)arg;
 
-			if (_mixers != nullptr) {
+			/* allocate a new mixer group and load it from the file */
-				delete _mixers;
+			newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
-				_mixers = nullptr;
+			if (newmixers->load_from_file(path) != 0) {
-			}
+				delete newmixers;
-
-			_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
-
-			if (_mixers->load_from_file(path) != 0) {
-				delete _mixers;
-				_mixers = nullptr;
 				ret = -EINVAL;
 			}
 
+			/* swap the new mixers in for the old */
+			if (_mixers != nullptr) {
+				delete _mixers;
+			}
+			_mixers = newmixers;
+
 		}
 		break;
 

apps/px4io/protocol.h

@@ -39,6 +39,8 @@
  * messages and the corresponding complexity involved.
  */
 
+#pragma once
+
 /*
  * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL
  * TREES ARE MERGED.