Implement VISCA relative movement

3 files changed, 96 insertions, 9 deletions

diff --git a/backends/visca.c b/backends/visca.c
index 8465c14..2e82515 100644
--- a/backends/visca.c
+++ b/backends/visca.c
@@ -2,6 +2,7 @@
 #define DEBUG
 
 #include <string.h>
+#include <math.h>	
 #include "visca.h"
 #include "libmmbackend.h"
 
@@ -87,6 +88,10 @@ static int ptz_configure_instance(instance* inst, char* option, char* value){
 		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;
@@ -104,6 +109,8 @@ static int ptz_instance(instance* inst){
 	//start with maximum speeds
 	data->panspeed = ptz_channels[panspeed].max;
 	data->tiltspeed = ptz_channels[tiltspeed].max;
+	//start with a bit of slack/deadzone in relative movement axes
+	data->deadzone = 0.1;
 
 	inst->impl = data;
 	return 0;
@@ -135,6 +142,32 @@ static channel* ptz_channel(instance* inst, char* spec, uint8_t flags){
 		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);
 }
 
@@ -177,11 +210,53 @@ static size_t ptz_set_ptspeed(instance* inst, channel* c, channel_value* v, uint
 	return 0;
 }
 
-static size_t ptz_set_stop(instance* inst, channel* c, channel_value* v, uint8_t* msg){
+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;
+	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){
+		data->relative_movement &= ~rel_x;
+	}
+	else{
+		data->relative_movement &= ~rel_y;
+	}
+
+	if(speed_factor){
+		data->relative_movement |= direction;
+	}
+
+	//set stored axis speed
 	msg[4] = data->panspeed;
 	msg[5] = data->tiltspeed;
-	return ptz_channels[stop].bytes;
+
+	//update motor control from movement data
+	msg[6] |= (data->relative_movement & (rel_left | rel_right)) >> 2;
+	msg[7] |= data->relative_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){
diff --git a/backends/visca.h b/backends/visca.h
index 160398d..47ada19 100644
--- a/backends/visca.h
+++ b/backends/visca.h
@@ -12,6 +12,15 @@ static int ptz_shutdown(size_t n, instance** inst);
 
 #define VISCA_BUFFER_LENGTH 50
 
+enum /*_ptz_relmove_channel */ {
+	rel_up = 1,
+	rel_down = 2,
+	rel_left = 4,
+	rel_right = 8,
+	rel_x = 3,
+	rel_y = 12
+};
+
 typedef struct /*_ptz_instance_data*/ {
 	int fd;
 	uint8_t cam_address;
@@ -19,6 +28,8 @@ typedef struct /*_ptz_instance_data*/ {
 	uint16_t y;
 	uint8_t panspeed;
 	uint8_t tiltspeed;
+	uint8_t relative_movement;
+	double deadzone;
 } ptz_instance_data;
 
 enum /*ptz_channels*/ {
@@ -38,6 +49,7 @@ enum /*ptz_channels*/ {
 	store,
 	home,
 	stop,
+	relmove,
 	sentinel
 };
 
@@ -51,9 +63,7 @@ static size_t ptz_set_wb_mode(instance* inst, channel* c, channel_value* v, uint
 static size_t ptz_set_wb(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);
-
-//relative move test
-static size_t ptz_set_stop(instance* inst, channel* c, channel_value* v, uint8_t* msg);
+static size_t ptz_set_relmove(instance* inst, channel* c, channel_value* v, uint8_t* msg);
 
 static struct {
 	char* name;
@@ -77,6 +87,6 @@ static struct {
 	[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},
 	[home] = {"home", 5, {0x80, 0x01, 0x06, 0x04, 0xFF}, 0, 0, 0, NULL},
-	//relative move test
-	[stop] = {"stop", 9, {0x80, 0x01, 0x06, 0x01, 0, 0, 0x03, 0x03, 0xFF}, 0, 0, 0, ptz_set_stop}
+	[relmove] = {"move", 9, {0x80, 0x01, 0x06, 0x01, 0, 0, 0, 0, 0xFF}, 0, 1, 0, ptz_set_relmove},
+	[stop] = {"stop", 9, {0x80, 0x01, 0x06, 0x01, 0, 0, 0x03, 0x03, 0xFF}, 0, 0, 0, ptz_set_relmove}
 };
diff --git a/backends/visca.md b/backends/visca.md
index 1dd516e..cf5906d 100644
--- a/backends/visca.md
+++ b/backends/visca.md
@@ -19,13 +19,14 @@ The `visca` backend does not take any global configuration.
 | `id`		| `5`			| `1`			| VISCA Camera address (normally 1 for network communication	|
 | `connect`	| `10.10.10.1 5678`	| none			| Camera network address and port. Default connection is TCP, when optionally suffixed with the `udp` keyword, connection will be UDP |
 | `device`	| `/dev/ttyUSB0`	| none			| (Linux only) Device node for a serial port adapter connecting to the camera |
+| `deadzone`	| `0.1`			| `0.1`			| Amount of event value variation to be ignored for relative movement commands |
 
 #### Channel specification
 
 Each instance exposes the following channels
 
-* `pan`: Pan axis
-* `tilt`: Tilt axis
+* `pan`: Pan axis (absolute)
+* `tilt`: Tilt axis (absolute)
 * `panspeed`: Pan speed
 * `tiltspeed`: Tilt speed
 * `zoom`: Zoom position
@@ -36,6 +37,7 @@ Each instance exposes the following channels
 * `home`: Return to home 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)
+* `move.left`, `move.right`, `move.up`, `move.down`: Relative movement with the currently set `panspeed` and `tiltspeed`
 
 Example mappings: