Fmu interface to Capture

src/drivers/px4fmu/fmu.cpp

@@ -57,6 +57,7 @@
 #include <drivers/device/device.h>
 #include <drivers/device/i2c.h>
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_input_capture.h>
 #include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 
@@ -70,6 +71,7 @@
 #include <systemlib/param/param.h>
 #include <drivers/drv_mixer.h>
 #include <drivers/drv_rc_input.h>
+#include <drivers/drv_input_capture.h>
 
 #include <lib/rc/sbus.h>
 #include <lib/rc/dsm.h>
@@ -99,9 +101,15 @@ public:
 	enum Mode {
 		MODE_NONE,
 		MODE_2PWM,
+		MODE_2PWM2CAP,
+		MODE_3PWM,
+		MODE_3PWM1CAP,
 		MODE_4PWM,
 		MODE_6PWM,
 		MODE_8PWM,
+		MODE_4CAP,
+		MODE_5CAP,
+		MODE_6CAP,
 	};
 	PX4FMU();
 	virtual ~PX4FMU();
@@ -118,6 +126,10 @@ public:
 
 	int		set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
 
+	static void	capture_trampoline(void *context, uint32_t chan_index,
+					       hrt_abstime edge_time, uint32_t edge_state,
+					       uint32_t overflow);
+
 private:
 	enum RC_SCAN {
 		RC_SCAN_PPM = 0,
@@ -185,7 +197,6 @@ private:
 	static bool	arm_nothrottle() { return (_armed.prearmed && !_armed.armed); }
 
 	static void	cycle_trampoline(void *arg);
-
 	void		cycle();
 	void		work_start();
 	void		work_stop();
@@ -194,6 +205,8 @@ private:
 					 uint8_t control_group,
 					 uint8_t control_index,
 					 float &input);
+	void	capture_callback(uint32_t chan_index,
+					       hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
 	void		subscribe();
 	int		set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
 	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
@@ -217,6 +230,8 @@ private:
 	uint32_t	gpio_read(void);
 	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
 
+	int		capture_ioctl(file *filp, int cmd, unsigned long arg);
+
 	/* do not allow to copy due to ptr data members */
 	PX4FMU(const PX4FMU &);
 	PX4FMU operator=(const PX4FMU &);
@@ -411,6 +426,11 @@ PX4FMU::set_mode(Mode mode)
 	 * are presented on the output pins.
 	 */
 	switch (mode) {
+	case MODE_2PWM2CAP:	// v1 multi-port with flow control lines as PWM
+		up_input_capture_set(2, Rising, 0, NULL, NULL);
+		up_input_capture_set(3, Rising, 0, NULL, NULL);
+			DEVICE_DEBUG("MODE_2PWM2CAP");
+		// no break
 	case MODE_2PWM:	// v1 multi-port with flow control lines as PWM
 		DEVICE_DEBUG("MODE_2PWM");
 
@@ -425,6 +445,23 @@ PX4FMU::set_mode(Mode mode)
 
 		break;
 
+	case MODE_3PWM1CAP:	// v1 multi-port with flow control lines as PWM
+		DEVICE_DEBUG("MODE_3PWM1CAP");
+		up_input_capture_set(3, Rising, 0, NULL, NULL);
+		// no break
+	case MODE_3PWM:	// v1 multi-port with flow control lines as PWM
+		DEVICE_DEBUG("MODE_3PWM");
+
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0x7);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+		break;
+
 	case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs
 		DEVICE_DEBUG("MODE_4PWM");
 
@@ -641,6 +678,19 @@ PX4FMU::cycle_trampoline(void *arg)
 	dev->cycle();
 }
 
+void
+PX4FMU::capture_trampoline(void *context, uint32_t chan_index,
+	       hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
+{
+	PX4FMU *dev = reinterpret_cast<PX4FMU *>(context);
+	dev->capture_callback(chan_index, edge_time, edge_state, overflow);
+}
+
+void PX4FMU::capture_callback(uint32_t chan_index,
+	       hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
+{
+	fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state,overflow);
+}
 void PX4FMU::fill_rc_in(uint16_t raw_rc_count,
 			uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS],
 			hrt_abstime now, bool frame_drop, bool failsafe,
@@ -812,9 +862,15 @@ PX4FMU::cycle()
 
 			switch (_mode) {
 			case MODE_2PWM:
+			case MODE_2PWM2CAP:
 				num_outputs = 2;
 				break;
 
+			case MODE_3PWM:
+			case MODE_3PWM1CAP:
+				num_outputs = 3;
+				break;
+
 			case MODE_4PWM:
 				num_outputs = 4;
 				break;
@@ -1208,10 +1264,20 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 		return ret;
 	}
 
+	/* try it as a Capture ioctl next */
+	ret = capture_ioctl(filp, cmd, arg);
+
+	if (ret != -ENOTTY) {
+		return ret;
+	}
+
 	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
 	switch (_mode) {
 	case MODE_2PWM:
+	case MODE_3PWM:
 	case MODE_4PWM:
+	case MODE_2PWM2CAP:
+	case MODE_3PWM1CAP:
 	case MODE_6PWM:
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
 	case MODE_8PWM:
@@ -1469,12 +1535,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 	/* FALLTHROUGH */
 	case PWM_SERVO_SET(3):
-	case PWM_SERVO_SET(2):
 		if (_mode < MODE_4PWM) {
 			ret = -EINVAL;
 			break;
 		}
 
+	/* FALLTHROUGH */
+	case PWM_SERVO_SET(2):
+		if (_mode < MODE_3PWM) {
+			ret = -EINVAL;
+			break;
+		}
+
 	/* FALLTHROUGH */
 	case PWM_SERVO_SET(1):
 	case PWM_SERVO_SET(0):
@@ -1507,11 +1579,16 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 	/* FALLTHROUGH */
 	case PWM_SERVO_GET(3):
-	case PWM_SERVO_GET(2):
 		if (_mode < MODE_4PWM) {
 			ret = -EINVAL;
 			break;
 		}
+	/* FALLTHROUGH */
+	case PWM_SERVO_GET(2):
+		if (_mode < MODE_3PWM) {
+			ret = -EINVAL;
+			break;
+		}
 
 	/* FALLTHROUGH */
 	case PWM_SERVO_GET(1):
@@ -1550,7 +1627,13 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			*(unsigned *)arg = 4;
 			break;
 
+		case MODE_3PWM:
+		case MODE_3PWM1CAP:
+			*(unsigned *)arg = 3;
+			break;
+
 		case MODE_2PWM:
+		case MODE_2PWM2CAP:
 			*(unsigned *)arg = 2;
 			break;
 
@@ -1578,6 +1661,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 				set_mode(MODE_2PWM);
 				break;
 
+			case 3:
+				set_mode(MODE_3PWM);
+				break;
+
 			case 4:
 				set_mode(MODE_4PWM);
 				break;
@@ -2052,6 +2139,124 @@ PX4FMU::gpio_read(void)
 	return bits;
 }
 
+int
+PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int	ret = -EINVAL;
+
+	lock();
+
+	input_capture_config_t *pconfig = 0;
+
+	input_capture_stats_t *stats = (input_capture_stats_t *)arg;
+
+	if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP)
+	{
+
+		pconfig = (input_capture_config_t *)arg;
+	}
+
+	switch (cmd) {
+
+	case INPUT_CAP_SET:
+		if (pconfig)
+		{
+			ret =  up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter,
+										pconfig->callback, pconfig->context);
+		}
+		break;
+
+	case INPUT_CAP_SET_CALLBACK:
+		if (pconfig)
+		{
+			ret =  up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context);
+		}
+		break;
+
+	case INPUT_CAP_GET_CALLBACK:
+		if (pconfig)
+		{
+			ret =  up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context);
+		}
+		break;
+
+	case INPUT_CAP_GET_STATS:
+		if (arg)
+		{
+			ret =  up_input_capture_get_stats(stats->chan_in_edges_out, stats, false);
+		}
+		break;
+
+	case INPUT_CAP_GET_CLR_STATS:
+		if (arg)
+		{
+			ret =  up_input_capture_get_stats(stats->chan_in_edges_out, stats, true);
+		}
+		break;
+
+	case INPUT_CAP_SET_EDGE:
+		if (pconfig)
+		{
+			ret =  up_input_capture_set_trigger(pconfig->channel, pconfig->edge);
+		}
+		break;
+
+	case INPUT_CAP_GET_EDGE:
+		if (pconfig)
+		{
+			ret =  up_input_capture_get_trigger(pconfig->channel, &pconfig->edge);
+		}
+		break;
+	case INPUT_CAP_SET_FILTER:
+		if (pconfig)
+		{
+			ret =  up_input_capture_set_filter(pconfig->channel, pconfig->filter);
+		}
+		break;
+	case INPUT_CAP_GET_FILTER:
+		if (pconfig)
+		{
+			ret =  up_input_capture_get_filter(pconfig->channel, &pconfig->filter);
+		}
+		break;
+
+	case INPUT_CAP_GET_COUNT:
+		ret = OK;
+		switch(_mode) {
+		case MODE_3PWM1CAP:
+			*(unsigned *)arg = 1;
+		break;
+		case MODE_2PWM2CAP:
+			*(unsigned *)arg = 2;
+		break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+		break;
+
+	case INPUT_CAP_SET_COUNT:
+		ret = OK;
+		switch(_mode) {
+		case MODE_3PWM1CAP:
+			set_mode(MODE_3PWM1CAP);
+		break;
+		case MODE_2PWM2CAP:
+			set_mode(MODE_2PWM2CAP);
+		break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+		break;
+	default:
+		ret = -ENOTTY;
+		break;
+		}
+	unlock();
+	return ret;
+}
+
 int
 PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 {
@@ -2135,6 +2340,11 @@ enum PortMode {
 	PORT_PWM_AND_SERIAL,
 	PORT_PWM_AND_GPIO,
 	PORT_PWM4,
+	PORT_PWM3,
+	PORT_PWM2,
+	PORT_PWM3CAP1,
+	PORT_PWM2CAP2,
+	PORT_CAPTURE,
 };
 
 PortMode g_port_mode;
@@ -2176,6 +2386,26 @@ fmu_new_mode(PortMode new_mode)
 		/* select 4-pin PWM mode */
 		servo_mode = PX4FMU::MODE_4PWM;
 		break;
+
+	case PORT_PWM3:
+		/* select 4-pin PWM mode */
+		servo_mode = PX4FMU::MODE_3PWM;
+		break;
+
+	case PORT_PWM3CAP1:
+		/* select 3-pin PWM mode 1 capture */
+		servo_mode = PX4FMU::MODE_3PWM1CAP;
+		break;
+
+	case PORT_PWM2:
+		/* select 2-pin PWM mode */
+		servo_mode = PX4FMU::MODE_2PWM;
+		break;
+
+	case PORT_PWM2CAP2:
+		/* select 2-pin PWM mode 2 capture */
+		servo_mode = PX4FMU::MODE_2PWM2CAP;
+		break;
 #endif
 
 		/* mixed modes supported on v1 board only */
@@ -2304,9 +2534,15 @@ test(void)
 {
 	int	 fd;
 	unsigned servo_count = 0;
+	unsigned capture_count = 0;
 	unsigned pwm_value = 1000;
 	int	 direction = 1;
 	int	 ret;
+	uint32_t rate_limit = 0;
+	struct input_capture_t {
+		bool valid;
+		input_capture_config_t  chan;
+	} capture_conf[INPUT_CAPTURE_MAX_CHANNELS];
 
 	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
 
@@ -2320,7 +2556,32 @@ test(void)
 		err(1, "Unable to get servo count\n");
 	}
 
-	warnx("Testing %u servos", (unsigned)servo_count);
+	if (ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) {
+		fprintf(stdout,"Not in a capture mode\n");
+	}
+
+	warnx("Testing %u servos and %u input captures", (unsigned)servo_count, capture_count);
+	memset(capture_conf, 0, sizeof(capture_conf));
+	if (capture_count !=0 ) {
+		for(unsigned i = 0; i < capture_count; i++) {
+			// Map to channel number
+			capture_conf[i].chan.channel = i+ servo_count;
+			/* Save handler */
+			if (ioctl(fd, INPUT_CAP_GET_CALLBACK, (unsigned long)&capture_conf[i].chan.channel) != 0) {
+				err(1, "Unable to get capture callback for chan %u\n", capture_conf[i].chan.channel);
+			} else {
+				input_capture_config_t conf = capture_conf[i].chan;
+				conf.callback = &PX4FMU::capture_trampoline;
+				conf.context = g_fmu;
+				if (ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) {
+					capture_conf[i].valid = true;
+				} else {
+					err(1, "Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel);
+				}
+			}
+
+		}
+	}
 
 	struct pollfd fds;
 	fds.fd = 0; /* stdin */
@@ -2382,7 +2643,26 @@ test(void)
 				errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
 			}
 		}
+		if (capture_count !=0 && (++rate_limit % 500 == 0)) {
+			for(unsigned i = 0; i < capture_count; i++) {
+				if (capture_conf[i].valid) {
+					input_capture_stats_t stats;
+					stats.chan_in_edges_out = capture_conf[i].chan.channel;
+					if (ioctl(fd, INPUT_CAP_GET_STATS, (unsigned long)&stats) != 0) {
+						err(1, "Unable to get stats for chan %u\n", capture_conf[i].chan.channel);
+					} else {
+						fprintf(stdout, "FMU: Status chan:%u edges: %d last time:%lld last state:%d overflows:%d lantency:%u\n",
+								capture_conf[i].chan.channel,
+								stats.chan_in_edges_out,
+								stats.last_time,
+								stats.last_edge,
+								stats.overflows,
+								stats.latnecy);
+					}
+				}
+			}
 
+		}
 		/* Check if user wants to quit */
 		char c;
 		ret = poll(&fds, 1, 0);
@@ -2398,8 +2678,21 @@ test(void)
 		}
 	}
 
+	if (capture_count !=0 ) {
+		for(unsigned i = 0; i < capture_count; i++) {
+			// Map to channel number
+			if (capture_conf[i].valid) {
+			/* Save handler */
+				if (ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&capture_conf[i].chan) != 0) {
+					err(1, "Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel);
+				}
+			}
+		}
+	}
+
 	close(fd);
 
+
 	exit(0);
 }
 
@@ -2481,6 +2774,14 @@ fmu_main(int argc, char *argv[])
 
 	} else if (!strcmp(verb, "mode_pwm4")) {
 		new_mode = PORT_PWM4;
+	} else if (!strcmp(verb, "mode_pwm2")) {
+		new_mode = PORT_PWM2;
+	} else if (!strcmp(verb, "mode_pwm3")) {
+		new_mode = PORT_PWM3;
+	} else if (!strcmp(verb, "mode_pwm3cap1")) {
+		new_mode = PORT_PWM3CAP1;
+	} else if (!strcmp(verb, "mode_pwm2cap2")) {
+		new_mode = PORT_PWM2CAP2;
 #endif
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)