Merged IO mixing branch

apps/drivers/px4io/px4io.cpp

@@ -102,7 +102,8 @@ public:
 	bool			dump_one;
 
 private:
-	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
+	// XXX
+	static const unsigned	_max_actuators = PX4IO_CONTROL_CHANNELS;
 	int 			_update_rate;	///< serial send rate in Hz
 
 	int			_serial_fd;	///< serial interface to PX4IO
@@ -129,7 +130,8 @@ private:
 	orb_advert_t		_t_outputs;	///< mixed outputs topic
 	actuator_outputs_s	_outputs;	///< mixed outputs
 
-	MixerGroup		*_mixers;	///< loaded mixers
+	const char *volatile	_mix_buf;	///< mixer text buffer
+	volatile unsigned	_mix_buf_len;	///< size of the mixer text buffer
 
 	bool			_primary_pwm_device;	///< true if we are the default PWM output
 
@@ -176,6 +178,11 @@ private:
 	 */
 	void			config_send();
 
+	/**
+	 * Send a buffer containing mixer text to PX4IO
+	 */
+	int			mixer_send(const char *buf, unsigned buflen);
+
 	/**
 	 * Mixer control callback; invoked to fetch a control from a specific
 	 * group/index during mixing.
@@ -208,7 +215,8 @@ PX4IO::PX4IO() :
 	_t_armed(-1),
 	_t_vstatus(-1),
 	_t_outputs(-1),
-	_mixers(nullptr),
+	_mix_buf(nullptr),
+	_mix_buf_len(0),
 	_primary_pwm_device(false),
 	_relays(0),
 	_switch_armed(false),
@@ -231,6 +239,7 @@ PX4IO::~PX4IO()
 		/* give it another 100ms */
 		usleep(100000);
 	}
+
 	/* well, kill it anyway, though this will probably crash */
 	if (_task != -1)
 		task_delete(_task);
@@ -261,6 +270,7 @@ PX4IO::init()
 
 	/* start the IO interface task */
 	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
 		return -errno;
@@ -272,13 +282,16 @@ PX4IO::init()
 			debug("PX4IO connected");
 			break;
 		}
+
 		usleep(100000);
 	}
+
 	if (!_connected) {
 		/* error here will result in everything being torn down */
 		log("PX4IO not responding");
 		return -EIO;
 	}
+
 	return OK;
 }
 
@@ -325,10 +338,12 @@ PX4IO::task_main()
 
 	/* protocol stream */
 	_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
+
 	if (_io_stream == nullptr) {
 		log("failed to allocate HX protocol stream");
 		goto out;
 	}
+
 	hx_stream_set_counters(_io_stream,
 			       perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
 			       perf_alloc(PC_COUNT, "PX4IO frames received"),
@@ -375,13 +390,18 @@ PX4IO::task_main()
 	fds[3].fd = _t_vstatus;
 	fds[3].events = POLLIN;
 
-	log("ready");
+	debug("ready");
+
+	/* lock against the ioctl handler */
+	lock();
 
 	/* loop handling received serial bytes */
 	while (!_task_should_exit) {
 
 		/* sleep waiting for data, but no more than 100ms */
+		unlock();
 		int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
+		lock();
 
 		/* this would be bad... */
 		if (ret < 0) {
@@ -398,47 +418,22 @@ PX4IO::task_main()
 			if (fds[0].revents & POLLIN)
 				io_recv();
 
-			/* if we have new data from the ORB, go handle it */
+			/* if we have new control data from the ORB, handle it */
 			if (fds[1].revents & POLLIN) {
 
 				/* get controls */
 				orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
 				     ORB_ID(actuator_controls_1), _t_actuators, &_controls);
 
-				/* mix */
-				if (_mixers != nullptr) {
-					_outputs.timestamp = hrt_absolute_time();
-					_outputs.noutputs = _mixers->mix(&_outputs.output[0], _max_actuators);
+				/* scale controls to PWM (temporary measure) */
+				for (unsigned i = 0; i < _max_actuators; i++)
+					_outputs.output[i] = 1500 + (600 * _controls.control[i]);
 
-					// XXX output actual limited values
-					memcpy(&_controls_effective, &_controls, sizeof(_controls_effective));
-
-					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective);
-
-					/* convert to PWM values */
-					for (unsigned i = 0; i < _max_actuators; i++) {
-						/* last resort: catch NaN, INF and out-of-band errors */
-						if (i < _outputs.noutputs &&
-							isfinite(_outputs.output[i]) &&
-							_outputs.output[i] >= -1.0f &&
-							_outputs.output[i] <= 1.0f) {
-							/* scale for PWM output 900 - 2100us */
-							_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
-						} else {
-							/*
-							 * Value is NaN, INF or out of band - set to the minimum value.
-							 * This will be clearly visible on the servo status and will limit the risk of accidentally
-							 * spinning motors. It would be deadly in flight.
-							 */
-							_outputs.output[i] = 900;
-						}
-					}
-
-					/* and flag for update */
-					_send_needed = true;
-				}
+				/* and flag for update */
+				_send_needed = true;
 			}
 
+			/* if we have an arming state update, handle it */
 			if (fds[2].revents & POLLIN) {
 
 				orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
@@ -462,14 +457,26 @@ PX4IO::task_main()
 			_config_needed = false;
 			config_send();
 		}
+
+		/* send a mixer update if needed */
+		if (_mix_buf != nullptr) {
+			mixer_send(_mix_buf, _mix_buf_len);
+
+			/* clear the buffer record so the ioctl handler knows we're done */
+			_mix_buf = nullptr;
+			_mix_buf_len = 0;
+		}
 	}
 
+	unlock();
+
 out:
 	debug("exiting");
 
 	/* kill the HX stream */
 	if (_io_stream != nullptr)
 		hx_stream_free(_io_stream);
+
 	::close(_serial_fd);
 
 	/* clean up the alternate device node */
@@ -522,25 +529,27 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 {
 	const px4io_report *rep = (const px4io_report *)buffer;
 
-	lock();
+//	lock();
 
 	/* sanity-check the received frame size */
 	if (bytes_received != sizeof(px4io_report)) {
 		debug("got %u expected %u", bytes_received, sizeof(px4io_report));
 		goto out;
 	}
+
 	if (rep->i2f_magic != I2F_MAGIC) {
 		debug("bad magic");
 		goto out;
 	}
+
 	_connected = true;
 
 	/* publish raw rc channel values from IO if valid channels are present */
 	if (rep->channel_count > 0) {
 		_input_rc.timestamp = hrt_absolute_time();
 		_input_rc.channel_count = rep->channel_count;
-		for (int i = 0; i < rep->channel_count; i++)
-		{
+
+		for (int i = 0; i < rep->channel_count; i++) {
 			_input_rc.values[i] = rep->rc_channel[i];
 		}
 
@@ -557,13 +566,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 		dump_one = false;
 
 		printf("IO: %s armed ", rep->armed ? "" : "not");
+
 		for (unsigned i = 0; i < rep->channel_count; i++)
 			printf("%d: %d ", i, rep->rc_channel[i]);
+
 		printf("\n");
 	}
 
 out:
-	unlock();
+//	unlock();
+	return;
 }
 
 void
@@ -576,13 +588,13 @@ PX4IO::io_send()
 
 	/* set outputs */
 	for (unsigned i = 0; i < _max_actuators; i++)
-		cmd.servo_command[i] = _outputs.output[i];
+		cmd.output_control[i] = _outputs.output[i];
 
 	/* publish as we send */
 	_outputs.timestamp = hrt_absolute_time();
+	/* XXX needs to be based off post-mix values from the IO side */
 	orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
 
-
 	/* update relays */
 	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
 		cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
@@ -597,6 +609,7 @@ PX4IO::io_send()
 	cmd.servo_rate = _update_rate;
 
 	ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
+
 	if (ret)
 		debug("send error %d", ret);
 }
@@ -610,10 +623,46 @@ PX4IO::config_send()
 	cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
 
 	ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
+
 	if (ret)
 		debug("config error %d", ret);
 }
 
+int
+PX4IO::mixer_send(const char *buf, unsigned buflen)
+{
+	uint8_t	frame[HX_STREAM_MAX_FRAME];
+	px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+
+	msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
+	msg->action = F2I_MIXER_ACTION_RESET;
+
+	do {
+		unsigned count = buflen;
+
+		if (count > F2I_MIXER_MAX_TEXT)
+			count = F2I_MIXER_MAX_TEXT;
+
+		if (count > 0) {
+			memcpy(&msg->text[0], buf, count);
+			buf += count;
+			buflen -= count;
+		}
+
+		int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count);
+
+		if (ret) {
+			log("mixer send error %d", ret);
+			return ret;
+		}
+
+		msg->action = F2I_MIXER_ACTION_APPEND;
+
+	} while (buflen > 0);
+
+	return 0;
+}
+
 int
 PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 {
@@ -637,7 +686,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
 
-		/* fake an update to the selected servo channel */
+		/* fake an update to the selected 'servo' channel */
 		if ((arg >= 900) && (arg <= 2100)) {
 			_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
 			_send_needed = true;
@@ -678,66 +727,30 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCGETOUTPUTCOUNT:
-		*(unsigned *)arg = _max_actuators;
+		*(unsigned *)arg = PX4IO_CONTROL_CHANNELS;
 		break;
 
 	case MIXERIOCRESET:
-		if (_mixers != nullptr) {
-			delete _mixers;
-			_mixers = nullptr;
-		}
-
+		ret = 0;	/* load always resets */
 		break;
 
-	case MIXERIOCADDSIMPLE: {
-			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
+	case MIXERIOCLOADBUF:
 
-			/* build the new mixer from the supplied argument */
-			SimpleMixer *mixer = new SimpleMixer(control_callback,
-							     (uintptr_t)&_controls, mixinfo);
+		/* set the buffer up for transfer */
+		_mix_buf = (const char *)arg;
+		_mix_buf_len = strnlen(_mix_buf, 1024);
 
-			/* validate the new mixer */
-			if (mixer->check()) {
-				delete mixer;
-				ret = -EINVAL;
+		/* drop the lock and wait for the thread to clear the transmit */
+		unlock();
 
-			} else {
-				/* if we don't have a group yet, allocate one */
-				if (_mixers == nullptr)
-					_mixers = new MixerGroup(control_callback,
-								 (uintptr_t)&_controls);
+		while (_mix_buf != nullptr)
+			usleep(1000);
 
-				/* add the new mixer to the group */
-				_mixers->add_mixer(mixer);
-			}
+		lock();
 
-		}
+		ret = 0;
 		break;
 
-	case MIXERIOCLOADBUF: {
-			const char *buf = (const char *)arg;
-			unsigned buflen = strnlen(buf, 1024);
-
-			if (_mixers == nullptr)
-				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
-
-			if (_mixers == nullptr) {
-				ret = -ENOMEM;
-
-			} else {
-
-				ret = _mixers->load_from_buf(buf, buflen);
-
-				if (ret != 0) {
-					debug("mixer load failed with %d", ret);
-					delete _mixers;
-					_mixers = nullptr;
-					ret = -EINVAL;
-				}
-			}
-			break;
-		}
-
 	default:
 		/* not a recognised value */
 		ret = -ENOTTY;
@@ -777,6 +790,13 @@ test(void)
 
 	close(fd);
 
+	actuator_armed_s aa;
+
+	aa.armed = true;
+	aa.lockdown = false;
+
+	orb_advertise(ORB_ID(actuator_armed), &aa);
+
 	exit(0);
 }
 
@@ -796,6 +816,7 @@ monitor(void)
 		if (fds[0].revents == POLLIN) {
 			int c;
 			read(0, &c, 1);
+
 			if (cancels-- == 0)
 				exit(0);
 		}
@@ -893,6 +914,7 @@ px4io_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "test"))
 		test();
+
 	if (!strcmp(argv[1], "monitor"))
 		monitor();
 

apps/drivers/px4io/uploader.cpp

@@ -190,6 +190,7 @@ PX4IO_Uploader::drain()
 
 	do {
 		ret = recv(c, 250);
+
 		if (ret == OK) {
 			//log("discard 0x%02x", c);
 		}

apps/px4io/comms.c

@@ -71,6 +71,8 @@ static struct px4io_report	report;
 
 static void			comms_handle_frame(void *arg, const void *buffer, size_t length);
 
+perf_counter_t			comms_rx_errors;
+
 static void
 comms_init(void)
 {
@@ -78,6 +80,9 @@ comms_init(void)
 	fmu_fd = open("/dev/ttyS1", O_RDWR);
 	stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
 
+	comms_rx_errors = perf_alloc(PC_COUNT, "rx_err");
+	hx_stream_set_counters(stream, 0, 0, comms_rx_errors);
+
 	/* default state in the report to FMU */
 	report.i2f_magic = I2F_MAGIC;
 
@@ -173,14 +178,12 @@ comms_handle_command(const void *buffer, size_t length)
 	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
-	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
-		system_state.fmu_channel_data[i] = cmd->servo_command[i];
-	}
+	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
+		system_state.fmu_channel_data[i] = cmd->output_control[i];
 
-	/* if IO is armed and FMU gets disarmed, IO must also disarm */
-	if (system_state.arm_ok && !cmd->arm_ok) {
+	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
+	if (system_state.arm_ok && !cmd->arm_ok)
 		system_state.armed = false;
-	}
 
 	system_state.arm_ok = cmd->arm_ok;
 	system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;

apps/px4io/mixer.cpp

@@ -56,6 +56,7 @@
 #include <systemlib/mixer/mixer.h>
 
 extern "C" {
+//#define DEBUG
 #include "px4io.h"
 }
 
@@ -94,15 +95,17 @@ mixer_tick(void)
 	/*
 	 * Decide which set of inputs we're using.
 	 */
+	 
 	/* this is for planes, where manual override makes sense */
 	if(system_state.manual_override_ok) {
 		/* if everything is ok */
 		if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
 			/* we have recent control data from the FMU */
-			control_count = PX4IO_OUTPUT_CHANNELS;
+			control_count = PX4IO_CONTROL_CHANNELS;
 			control_values = &system_state.fmu_channel_data[0];
-		/* when override is on or the fmu is not available */
+
 		} else if (system_state.rc_channels > 0) {
+			/* when override is on or the fmu is not available, but RC is present */
 			control_count = system_state.rc_channels;
 			control_values = &system_state.rc_channel_data[0];
 		} else {
@@ -116,7 +119,7 @@ mixer_tick(void)
 	} else {
 		/* if the fmu is available whe are good */
 		if(system_state.mixer_fmu_available) {
-			control_count = PX4IO_OUTPUT_CHANNELS;
+			control_count = PX4IO_CONTROL_CHANNELS;
 			control_values = &system_state.fmu_channel_data[0];
 		/* we better shut everything off */
 		} else {
@@ -183,20 +186,22 @@ mixer_callback(uintptr_t handle,
 		return -1;
 
 	/* scale from current PWM units (1000-2000) to mixer input values */
-	/* XXX this presents some ugly problems w.r.t failsafe and R/C input scaling that have to be addressed */
 	control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
 
 	return 0;
 }
 
+static char mixer_text[256];
+static unsigned mixer_text_length = 0;
+
 void
 mixer_handle_text(const void *buffer, size_t length)
 {
-	static char mixer_text[256];
-	static unsigned mixer_text_length = 0;
 
 	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
 
+	debug("mixer text %u", length);
+
 	if (length < sizeof(px4io_mixdata))
 		return;
 
@@ -204,11 +209,13 @@ mixer_handle_text(const void *buffer, size_t length)
 
 	switch (msg->action) {
 	case F2I_MIXER_ACTION_RESET:
+		debug("reset");
 		mixer_group.reset();
 		mixer_text_length = 0;
 
 		/* FALLTHROUGH */
 	case F2I_MIXER_ACTION_APPEND:
+		debug("append %d", length);
 
 		/* check for overflow - this is really fatal */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
@@ -218,14 +225,22 @@ mixer_handle_text(const void *buffer, size_t length)
 		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
 		mixer_text_length += text_length;
 		mixer_text[mixer_text_length] = '\0';
+		debug("buflen %u", mixer_text_length);
 
 		/* process the text buffer, adding new mixers as their descriptions can be parsed */
-		char *end = &mixer_text[mixer_text_length];
-		mixer_group.load_from_buf(&mixer_text[0], mixer_text_length);
+		unsigned resid = mixer_text_length;
+		mixer_group.load_from_buf(&mixer_text[0], resid);
 
-		/* copy any leftover text to the base of the buffer for re-use */
-		if (mixer_text_length > 0)
-			memcpy(&mixer_text[0], end - mixer_text_length, mixer_text_length);
+		/* if anything was parsed */
+		if (resid != mixer_text_length) {
+			debug("used %u", mixer_text_length - resid);
+
+			/* copy any leftover text to the base of the buffer for re-use */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+			mixer_text_length = resid;
+		}
 
 		break;
 	}

apps/px4io/protocol.h

@@ -41,7 +41,7 @@
 
 #pragma once
 
-#define PX4IO_OUTPUT_CHANNELS	8
+#define PX4IO_CONTROL_CHANNELS	8
 #define PX4IO_INPUT_CHANNELS	12
 #define PX4IO_RELAY_CHANNELS	4
 
@@ -55,7 +55,8 @@ struct px4io_command {
 #define F2I_MAGIC		0x636d
 
 	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];	/**< servo output channels */
-	uint16_t	servo_rate;				/**< PWM output rate in Hz */
+	uint16_t	servo_rate;
+	uint16_t	output_control[PX4IO_CONTROL_CHANNELS]; /**< PWM output rate in Hz */
 	bool		relay_state[PX4IO_RELAY_CHANNELS];	/**< relay states as requested by FMU */
 	bool		arm_ok;					/**< FMU allows full arming */
 	bool		vector_flight_mode_ok;			/**< FMU aquired a valid position lock, ready for pos control */
@@ -102,6 +103,6 @@ struct px4io_mixdata {
 };
 
 /* maximum size is limited by the HX frame size */
-#define F2I_MIXER_MAX_TEXT	(sizeof(struct px4io_mixdata) - HX_STREAM_MAX_FRAME)
+#define F2I_MIXER_MAX_TEXT	(HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata))
 
 #pragma pack(pop)

apps/px4io/px4io.h

@@ -82,7 +82,7 @@ struct sys_state_s {
 	/**
 	 * Control signals from FMU.
 	 */
-	uint16_t	fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
+	uint16_t	fmu_channel_data[PX4IO_CONTROL_CHANNELS];
 
 	/**
 	 * Mixed servo outputs

apps/systemlib/mixer/mixer_group.cpp

@@ -125,39 +125,58 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
 	int ret = -1;
 	const char *end = buf + buflen;
 
-	/* loop until we have consumed the buffer */
+	/*
+	 * Loop until either we have emptied the buffer, or we have failed to
+	 * allocate something when we expected to.
+	 */
 	while (buflen > 0) {
 		Mixer *m = nullptr;
 		const char *p = end - buflen;
+		unsigned resid = buflen;
 
-		/* use the next character as a hint to decide which mixer class to construct */
+		/*
+		 * Use the next character as a hint to decide which mixer class to construct.
+		 */
 		switch (*p) {
 		case 'Z':
-			m = NullMixer::from_text(p, buflen);
+			m = NullMixer::from_text(p, resid);
 			break;
 
 		case 'M':
-			m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen);
+			m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid);
 			break;
 
 		case 'R':
-			m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen);
+			m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid);
 			break;
 
 		default:
-			/* it's probably junk or whitespace */
-			break;
+			/* it's probably junk or whitespace, skip a byte and retry */
+			buflen--;
+			continue;
 		}
 
+		/*
+		 * If we constructed something, add it to the group.
+		 */
 		if (m != nullptr) {
 			add_mixer(m);
+
+			/* we constructed something */
 			ret = 0;
 
+			/* only adjust buflen if parsing was successful */
+			buflen = resid;
 		} else {
-			/* skip whitespace or junk in the buffer */
-			buflen--;
+
+			/*
+			 * There is data in the buffer that we expected to parse, but it didn't,
+			 * so give up for now.
+			 */
+			break;
 		}
 	}
 
+	/* nothing more in the buffer for us now */
 	return ret;
 }