Merge branch 'master' into debian/master

1 files changed, 466 insertions, 0 deletions

diff --git a/backends/visca.c b/backends/visca.c
new file mode 100644
index 0000000..a36b139
--- /dev/null
+++ b/backends/visca.c
@@ -0,0 +1,466 @@
+#define BACKEND_NAME "visca"
+//#define DEBUG
+
+#include <string.h>
+#include <math.h>
+
+#ifndef _WIN32
+	#include <sys/ioctl.h>
+	#include <asm/termbits.h>
+#endif
+
+#include "visca.h"
+#include "libmmbackend.h"
+
+/* TODO
+ *	VISCA server
+ *	Command output rate limiting / deduplication
+ *	Inquiry
+ *	Reconnect on connection close
+ *	Speed updates should send motor outputs
+ *
+ */
+
+MM_PLUGIN_API int init(){
+	backend ptz = {
+		.name = BACKEND_NAME,
+		.conf = ptz_configure,
+		.create = ptz_instance,
+		.conf_instance = ptz_configure_instance,
+		.channel = ptz_channel,
+		.handle = ptz_set,
+		.process = ptz_handle,
+		.start = ptz_start,
+		.shutdown = ptz_shutdown
+	};
+
+	//register backend
+	if(mm_backend_register(ptz)){
+		LOG("Failed to register backend");
+		return 1;
+	}
+	return 0;
+}
+
+static int ptz_configure(char* option, char* value){
+	LOG("No backend configuration possible");
+	return 1;
+}
+
+static int ptz_configure_instance(instance* inst, char* option, char* value){
+	char* host = NULL, *port = NULL, *options = NULL;
+	ptz_instance_data* data = (ptz_instance_data*) inst->impl;
+	uint8_t mode = 0;
+
+	if(!strcmp(option, "id")){
+		data->cam_address = strtoul(value, NULL, 10);
+		return 0;
+	}
+	else if(!strcmp(option, "connect")){
+		if(data->fd >= 0){
+			LOGPF("Instance %s already connected", inst->name);
+			return 1;
+		}
+
+		mmbackend_parse_hostspec(value, &host, &port, &options);
+		if(!host || !port){
+			LOGPF("Invalid destination address specified for instance %s", inst->name);
+			return 1;
+		}
+
+		if(options && !strcmp(options, "udp")){
+			mode = 1;
+		}
+
+		data->fd = mmbackend_socket(host, port, mode ? SOCK_DGRAM : SOCK_STREAM, 0, 0, 1);
+		if(data->fd < 0){
+			LOGPF("Failed to connect instance %s", inst->name);
+			return 1;
+		}
+		return 0;
+	}
+	else if(!strcmp(option, "device")){
+		if(data->fd >= 0){
+			LOGPF("Instance %s already connected", inst->name);
+			return 1;
+		}
+
+		#ifdef _WIN32
+		LOG("Direct device connections are not possible on Windows");
+		return 1;
+		#else
+
+		struct termios2 device_config;
+
+		options = strchr(value, ' ');
+		if(options){
+			//terminate port name
+			*options = 0;
+			options++;
+		}
+
+		data->fd = open(value, O_RDWR | O_NONBLOCK);
+		if(data->fd < 0){
+			LOGPF("Failed to connect instance %s to device %s", inst->name, value);
+			return 1;
+		}
+		data->direct_device = 1;
+
+		//configure baudrate
+		if(options){
+			//get current port config
+			if(ioctl(data->fd, TCGETS2, &device_config)){
+				LOGPF("Failed to get port configuration data for %s: %s", value, strerror(errno));
+				return 0;
+			}
+
+			device_config.c_cflag &= ~CBAUD;
+			device_config.c_cflag |= BOTHER;
+			device_config.c_ispeed = strtoul(options, NULL, 10);
+			device_config.c_ospeed = strtoul(options, NULL, 10);
+
+			//set updated config
+			if(ioctl(data->fd, TCSETS2, &device_config)){
+				LOGPF("Failed to set port configuration data for %s: %s", value, strerror(errno));
+			}
+		}
+		return 0;
+		#endif
+	}
+	else if(!strcmp(option, "deadzone")){
+		data->deadzone = strtod(value, NULL);
+		return 0;
+	}
+
+	LOGPF("Unknown instance configuration parameter %s for instance %s", option, inst->name);
+	return 1;
+}
+
+static int ptz_instance(instance* inst){
+	ptz_instance_data* data = calloc(1, sizeof(ptz_instance_data));
+	if(!data){
+		LOG("Failed to allocate memory");
+		return 1;
+	}
+
+	data->fd = -1;
+	data->cam_address = 1;
+	//start with maximum speeds
+	data->max_pan = ptz_channels[panspeed].max;
+	data->max_tilt = ptz_channels[tiltspeed].max;
+	//start with a bit of slack/deadzone in relative movement axes
+	data->deadzone = 0.1;
+
+	inst->impl = data;
+	return 0;
+}
+
+static channel* ptz_channel(instance* inst, char* spec, uint8_t flags){
+	uint64_t command = 0;
+
+	if(flags & mmchannel_input){
+		LOG("This backend currently only supports output channels");
+		return NULL;
+	}
+
+	for(command = 0; command < sentinel; command++){
+		if(!strncmp(spec, ptz_channels[command].name, strlen(ptz_channels[command].name))){
+			break;
+		}
+	}
+
+	DBGPF("Matched spec %s as %s", spec, ptz_channels[command].name ? ptz_channels[command].name : "sentinel");
+
+	if(command == sentinel){
+		LOGPF("Unknown channel spec %s", spec);
+		return NULL;
+	}
+
+	//store the memory to be called above the command type
+	if(command == call || command == store){
+		command |= (strtoul(spec + strlen(ptz_channels[command].name), NULL, 10) << 8);
+	}
+
+	//store relative move direction
+	else if(command == relmove){
+		if(!strcmp(spec + strlen(ptz_channels[relmove].name), ".up")
+				|| !strcmp(spec + strlen(ptz_channels[relmove].name), ".y")){
+			command |= (rel_up << 8);
+		}
+		else if(!strcmp(spec + strlen(ptz_channels[relmove].name), ".left")
+				|| !strcmp(spec + strlen(ptz_channels[relmove].name), ".x")){
+			command |= (rel_left << 8);
+		}
+
+		if(!strcmp(spec + strlen(ptz_channels[relmove].name), ".down")
+				|| !strcmp(spec + strlen(ptz_channels[relmove].name), ".y")){
+			command |= (rel_down << 8);
+		}
+		else if(!strcmp(spec + strlen(ptz_channels[relmove].name), ".right")
+				|| !strcmp(spec + strlen(ptz_channels[relmove].name), ".x")){
+			command |= (rel_right << 8);
+		}
+
+		if(command >> 8 == 0){
+			LOGPF("Could not parse relative movement command %s", spec);
+			return NULL;
+		}
+	}
+
+	return mm_channel(inst, command, 1);
+}
+
+static size_t ptz_set_pantilt(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+	ptz_instance_data* data = (ptz_instance_data*) inst->impl;
+
+	if(c->ident == pan){
+		data->x = ((ptz_channels[pan].max - ptz_channels[pan].min) * v->normalised) + ptz_channels[pan].min - ptz_channels[pan].offset;
+	}
+	else{
+		data->y = ((ptz_channels[tilt].max - ptz_channels[tilt].min) * v->normalised) + ptz_channels[tilt].min - ptz_channels[tilt].offset;
+	}
+
+	//absolute movements happen with maximum speed
+	msg[4] = data->max_pan;
+	msg[5] = data->max_tilt;
+
+	//either i'm doing this wrong or visca is just weird.
+	msg[6] = ((data->x & 0xF000) >> 12);
+	msg[7] = ((data->x & 0x0F00) >> 8);
+	msg[8] = ((data->x & 0xF0) >> 4);
+	msg[9] = (data->x & 0x0F);
+
+	msg[10] = ((data->y & 0xF000) >> 12);
+	msg[11] = ((data->y & 0x0F00) >> 8);
+	msg[12] = ((data->y & 0xF0) >> 4);
+	msg[13] = (data->y & 0x0F);
+
+	return ptz_channels[pan].bytes;
+}
+
+static size_t ptz_set_ptspeed(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+	ptz_instance_data* data = (ptz_instance_data*) inst->impl;
+	if(c->ident == panspeed){
+		data->max_pan = ((ptz_channels[panspeed].max - ptz_channels[panspeed].min) * v->normalised) + ptz_channels[panspeed].min - ptz_channels[panspeed].offset;
+	}
+	else{
+		data->max_tilt = ((ptz_channels[tiltspeed].max - ptz_channels[tiltspeed].min) * v->normalised) + ptz_channels[tiltspeed].min - ptz_channels[tiltspeed].offset;
+	}
+
+	return 0;
+}
+
+static size_t ptz_set_relmove(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+	ptz_instance_data* data = (ptz_instance_data*) inst->impl;
+
+	uint8_t direction = c->ident >> 8, movement = data->relative_movement;
+	double speed_factor = v->normalised;
+
+	if(direction == rel_x
+			|| direction == rel_y){
+		//select only one move event
+		direction &= (speed_factor > 0.5) ? (rel_up | rel_left) : (rel_down | rel_right);
+
+		//scale event value to full axis
+		speed_factor = fabs((speed_factor - 0.5) * 2);
+
+		//clamp to deadzone
+		speed_factor = (speed_factor < 2 * data->deadzone) ? 0 : speed_factor;
+	}
+
+	//clear modified axis
+	if(direction & rel_x){
+		movement &= ~rel_x;
+		data->factor_tilt = speed_factor;
+	}
+	else{
+		movement &= ~rel_y;
+		data->factor_pan = speed_factor;
+	}
+
+	if(speed_factor){
+		movement |= direction;
+	}
+
+	//only transmit if something actually changed
+	if(!movement && !data->relative_movement){
+		return 0;
+	}
+	data->relative_movement = movement;
+
+	//set stored axis speed
+	msg[4] = data->max_pan * data->factor_pan;
+	msg[5] = data->max_tilt * data->factor_tilt;
+
+	//update motor control from movement data
+	msg[6] |= (movement & (rel_left | rel_right)) >> 2;
+	msg[7] |= movement & (rel_up | rel_down);
+
+	//stop motors if unset
+	msg[6] = msg[6] ? msg[6] : 3;
+	msg[7] = msg[7] ? msg[7] : 3;
+
+	DBGPF("Moving axis %d with factor %f, total movement now %02X, commanding %d / %d, %d / %d",
+			direction, speed_factor, data->relative_movement,
+			msg[6], msg[4], msg[7], msg[5]);
+
+	return ptz_channels[relmove].bytes;
+}
+
+static size_t ptz_set_zoom(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+	uint16_t position = ((ptz_channels[zoom].max - ptz_channels[zoom].min) * v->normalised) + ptz_channels[zoom].min - ptz_channels[zoom].offset;
+	msg[4] = ((position & 0xF000) >> 12);
+	msg[5] = ((position & 0x0F00) >> 8);
+	msg[6] = ((position & 0xF0) >> 4);
+	msg[7] = (position & 0x0F);
+	return ptz_channels[zoom].bytes;
+}
+
+static size_t ptz_set_focus(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+	uint16_t position = ((ptz_channels[focus].max - ptz_channels[focus].min) * v->normalised) + ptz_channels[focus].min - ptz_channels[focus].offset;
+	msg[4] = ((position & 0xF000) >> 12);
+	msg[5] = ((position & 0x0F00) >> 8);
+	msg[6] = ((position & 0xF0) >> 4);
+	msg[7] = (position & 0x0F);
+	return ptz_channels[focus].bytes;
+}
+
+static size_t ptz_set_focus_mode(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+	msg[4] = (v->normalised > 0.9) ? 2 : 3;
+	return ptz_channels[focus_mode].bytes;
+}
+
+static size_t ptz_set_wb_mode(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+	msg[4] = (v->normalised > 0.9) ? 0 : 5;
+	return ptz_channels[wb_mode].bytes;
+}
+
+static size_t ptz_set_wb(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+	uint8_t command = c->ident & 0xFF;
+	uint8_t value = ((ptz_channels[command].max - ptz_channels[command].min) * v->normalised) + ptz_channels[command].min - ptz_channels[command].offset;
+	msg[6] = value >> 4;
+	msg[7] = value & 0x0F;
+	return ptz_channels[command].bytes;
+}
+
+static size_t ptz_set_memory(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+	if(v->normalised < 0.9){
+		return 0;
+	}
+
+	msg[5] = (c->ident >> 8);
+	return ptz_channels[call].bytes;
+}
+
+static size_t ptz_set_memory_store(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+	if(v->normalised < 0.9){
+		return 0;
+	}
+
+	msg[5] = (c->ident >> 8);
+	return ptz_channels[store].bytes;
+}
+
+static int ptz_write_serial(int fd, uint8_t* data, size_t bytes){
+	ssize_t total = 0, sent;
+
+	while(total < bytes){
+		sent = write(fd, data + total, bytes - total);
+		if(sent < 0){
+			LOGPF("Failed to write to serial port: %s", strerror(errno));
+			return 1;
+		}
+		total += sent;
+	}
+
+	return 0;
+}
+
+static int ptz_set(instance* inst, size_t num, channel** c, channel_value* v){
+	ptz_instance_data* data = (ptz_instance_data*) inst->impl;
+	size_t n = 0, bytes = 0;
+	uint8_t tx[VISCA_BUFFER_LENGTH] = "";
+	uint8_t command = 0;
+
+	for(n = 0; n < num; n++){
+		bytes = 0;
+		command = c[n]->ident & 0xFF;
+
+		if(ptz_channels[command].bytes){
+			memcpy(tx, ptz_channels[command].pattern, ptz_channels[command].bytes);
+			//if no handler function set, assume a parameterless command and send verbatim
+			bytes = ptz_channels[command].bytes;
+		}
+		tx[0] = 0x80 | (data->cam_address & 0xF);
+
+		if(ptz_channels[command].set){
+			bytes = ptz_channels[command].set(inst, c[n], v + n, tx);
+		}
+
+		if(data->direct_device && bytes && ptz_write_serial(data->fd, tx, bytes)){
+			LOGPF("Failed to write %s command on instance %s", ptz_channels[command].name, inst->name);	
+		}
+		else if(!data->direct_device && bytes && mmbackend_send(data->fd, tx, bytes)){
+			LOGPF("Failed to push %s command on instance %s", ptz_channels[command].name, inst->name);
+		}
+	}
+	return 0;
+}
+
+static int ptz_handle(size_t num, managed_fd* fds){
+	uint8_t recv_buf[VISCA_BUFFER_LENGTH];
+	size_t u;
+	ssize_t bytes_read;
+	instance* inst = NULL;
+
+	//read and ignore any responses for now
+	for(u = 0; u < num; u++){
+		inst = (instance*) fds[u].impl;
+		bytes_read = recv(fds[u].fd, recv_buf, sizeof(recv_buf), 0);
+		if(bytes_read <= 0){
+			LOGPF("Failed to receive on signaled fd for instance %s", inst->name);
+			//TODO handle failure
+		}
+		else{
+			DBGPF("Ignored %" PRIsize_t " incoming bytes for instance %s", bytes_read, inst->name);
+		}
+	}
+
+	return 0;
+}
+
+static int ptz_start(size_t n, instance** inst){
+	size_t u, fds = 0;
+	ptz_instance_data* data = NULL;
+
+	for(u = 0; u < n; u++){
+		data = (ptz_instance_data*) inst[u]->impl;
+		if(data->fd >= 0){
+			if(mm_manage_fd(data->fd, BACKEND_NAME, 1, inst[u])){
+				LOGPF("Failed to register descriptor for instance %s", inst[u]->name);
+				return 1;
+			}
+			fds++;
+		}
+	}
+
+	LOGPF("Registered %" PRIsize_t " descriptors to core", fds);
+	return 0;
+}
+
+static int ptz_shutdown(size_t n, instance** inst){
+	size_t u;
+	ptz_instance_data* data = NULL;
+
+	for(u = 0; u < n; u++){
+		data = (ptz_instance_data*) inst[u]->impl;
+		if(data->fd >= 0){
+			close(data->fd);
+		}
+		free(data);
+		inst[u]->impl = NULL;
+	}
+
+	LOG("Backend shut down");
+	return 0;
+}