Fix VISCA dialect

3 files changed, 86 insertions, 27 deletions

diff --git a/backends/visca.c b/backends/visca.c
index 619211d..d408b80 100644
--- a/backends/visca.c
+++ b/backends/visca.c
@@ -7,6 +7,7 @@
 
 /* TODO
  *	VISCA server
+ *	Rate limiting
  */
 
 MM_PLUGIN_API int init(){
@@ -128,8 +129,8 @@ static channel* ptz_channel(instance* inst, char* spec, uint8_t flags){
 	}
 
 	//store the memory to be called above the command type
-	if(ident == call){
-		ident |= (strtoul(spec + strlen(ptz_channels[call].name), NULL, 10) << 8);
+	if(ident == call || ident == store){
+		ident |= (strtoul(spec + strlen(ptz_channels[ident].name), NULL, 10) << 8);
 	}
 
 	return mm_channel(inst, ident, 1);
@@ -137,20 +138,27 @@ static channel* ptz_channel(instance* inst, char* spec, uint8_t flags){
 
 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;
-	uint32_t* x = (uint32_t*) (msg + 6);
-	uint32_t* y = (uint32_t*) (msg + 10);
-	
+
 	if(c->ident == pan){
-		data->x = ((ptz_channels[pan].max - ptz_channels[pan].min) * v->normalised) + ptz_channels[pan].min;
+		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;
+		data->y = ((ptz_channels[tilt].max - ptz_channels[tilt].min) * v->normalised) + ptz_channels[tilt].min - ptz_channels[tilt].offset;
 	}
 
 	msg[4] = data->panspeed;
 	msg[5] = data->tiltspeed;
-	*x = htobe32(data->x);
-	*y = htobe32(data->y);
+
+	//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;
 }
@@ -158,24 +166,30 @@ static size_t ptz_set_pantilt(instance* inst, channel* c, channel_value* v, uint
 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->panspeed = ((ptz_channels[panspeed].max - ptz_channels[panspeed].min) * v->normalised) + ptz_channels[panspeed].min;
+		data->panspeed = ((ptz_channels[panspeed].max - ptz_channels[panspeed].min) * v->normalised) + ptz_channels[panspeed].min - ptz_channels[panspeed].offset;
 	}
 	else{
-		data->tiltspeed = ((ptz_channels[tiltspeed].max - ptz_channels[tiltspeed].min) * v->normalised) + ptz_channels[tiltspeed].min;
+		data->tiltspeed = ((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_zoom(instance* inst, channel* c, channel_value* v, uint8_t* msg){
-	uint32_t* position = (uint32_t*) (msg + 4);
-	*position = htobe32(((ptz_channels[zoom].max - ptz_channels[zoom].min) * v->normalised) + ptz_channels[zoom].min);
+	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){
-	uint32_t* position = (uint32_t*) (msg + 4);
-	*position = htobe32(((ptz_channels[focus].max - ptz_channels[focus].min) * v->normalised) + ptz_channels[focus].min);
+	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;
 }
 
@@ -188,6 +202,15 @@ static size_t ptz_set_memory(instance* inst, channel* c, channel_value* v, uint8
 	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_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;
@@ -215,12 +238,43 @@ static int ptz_set(instance* inst, size_t num, channel** c, channel_value* v){
 }
 
 static int ptz_handle(size_t num, managed_fd* fds){
-	//no events generated here
+	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){
-	//no startup needed yet
+	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;
 }
 
diff --git a/backends/visca.h b/backends/visca.h
index 54c80f7..09c9057 100644
--- a/backends/visca.h
+++ b/backends/visca.h
@@ -15,8 +15,8 @@ static int ptz_shutdown(size_t n, instance** inst);
 typedef struct /*_ptz_instance_data*/ {
 	int fd;
 	uint8_t cam_address;
-	uint32_t x;
-	uint32_t y;
+	uint16_t x;
+	uint16_t y;
 	uint8_t panspeed;
 	uint8_t tiltspeed;
 } ptz_instance_data;
@@ -29,6 +29,7 @@ enum /*ptz_channels*/ {
 	zoom,
 	focus,
 	call,
+	store,
 	sentinel
 };
 
@@ -38,20 +39,23 @@ static size_t ptz_set_ptspeed(instance* inst, channel* c, channel_value* v, uint
 static size_t ptz_set_zoom(instance* inst, channel* c, channel_value* v, uint8_t* msg);
 static size_t ptz_set_focus(instance* inst, channel* c, channel_value* v, uint8_t* msg);
 static size_t ptz_set_memory(instance* inst, channel* c, channel_value* v, uint8_t* msg);
+static size_t ptz_set_memory_store(instance* inst, channel* c, channel_value* v, uint8_t* msg);
 
 static struct {
 	char* name;
 	size_t bytes;
 	uint8_t pattern[VISCA_BUFFER_LENGTH];
-	size_t min;
+	size_t min; //channel range = max - min
 	size_t max;
+	size_t offset; //channel value = normalised * range - offset
 	ptz_channel_set set;
 } ptz_channels[] = {
-	[pan] = {"pan", 14, {0x80, 0x01, 0x06, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFF}, 0, 4000, ptz_set_pantilt},
-	[tilt] = {"tilt", 14, {0x80, 0x01, 0x06, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFF}, 0, 4000, ptz_set_pantilt},
-	[panspeed] = {"panspeed", 0, {0}, 0x01, 0x18, ptz_set_ptspeed},
-	[tiltspeed] = {"tiltspeed", 0, {0}, 0x01, 0x14, ptz_set_ptspeed},
-	[zoom] = {"zoom", 9, {0x80, 0x01, 0x04, 0x47, 0, 0, 0, 0, 0xFF}, 0, 4000, ptz_set_zoom},
-	[focus] = {"focus", 9, {0x80, 0x01, 0x04, 0x48, 0, 0, 0, 0, 0xFF}, 0, 4000, ptz_set_focus},
-	[call] = {"memory", 7, {0x80, 0x01, 0x04, 0x3F, 0x02, 0, 0xFF}, 0, 254, ptz_set_memory}
+	[pan] = {"pan", 15, {0x80, 0x01, 0x06, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFF}, 0, 0x990 * 2, 0x990, ptz_set_pantilt},
+	[tilt] = {"tilt", 15, {0x80, 0x01, 0x06, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFF}, 0, 0x510 * 2, 0x510, ptz_set_pantilt},
+	[panspeed] = {"panspeed", 0, {0}, 0x01, 0x18, 0, ptz_set_ptspeed},
+	[tiltspeed] = {"tiltspeed", 0, {0}, 0x01, 0x14, 0, ptz_set_ptspeed},
+	[zoom] = {"zoom", 9, {0x80, 0x01, 0x04, 0x47, 0, 0, 0, 0, 0xFF}, 0, 0x4000, 0, ptz_set_zoom},
+	[focus] = {"focus", 9, {0x80, 0x01, 0x04, 0x48, 0, 0, 0, 0, 0xFF}, 0, 0x4000, 0, ptz_set_focus},
+	[call] = {"memory", 7, {0x80, 0x01, 0x04, 0x3F, 0x02, 0, 0xFF}, 0, 254, 0, ptz_set_memory},
+	[store] = {"store", 7, {0x80, 0x01, 0x04, 0x3F, 0x01, 0, 0xFF}, 0, 254, 0, ptz_set_memory_store}
 };
diff --git a/backends/visca.md b/backends/visca.md
index 6d978e2..4fe1933 100644
--- a/backends/visca.md
+++ b/backends/visca.md
@@ -30,6 +30,7 @@ Each instance exposes the following channels
 * `zoom`: Zoom position
 * `focus`: Focus position
 * `memory<n>`: Call memory <n> (if incoming event value is greater than 0.9)
+* `store<n>`: Store current pan/tilt/zoom setup to memory <n> (if incoming event value is greater than 0.9)
 
 Example mappings: