aboutsummaryrefslogtreecommitdiffhomepage
path: root/backends/visca.c
diff options
context:
space:
mode:
authorcbdev <cb@cbcdn.com>2021-06-30 03:03:48 +0200
committercbdev <cb@cbcdn.com>2021-06-30 03:03:48 +0200
commitbc3d80e9e6c038c87a64432586670c663a23e53d (patch)
tree5a28b0004a7f3492455316f34bb2c783e670f944 /backends/visca.c
parent8a0a413f1dd5593189dd6b651babcff9b2495451 (diff)
parentf16f7db86662fcdbf45b6373257c90c824b0b4b0 (diff)
downloadmidimonster-bc3d80e9e6c038c87a64432586670c663a23e53d.tar.gz
midimonster-bc3d80e9e6c038c87a64432586670c663a23e53d.tar.bz2
midimonster-bc3d80e9e6c038c87a64432586670c663a23e53d.zip
Merge branch 'master' into debian/master
Diffstat (limited to 'backends/visca.c')
-rw-r--r--backends/visca.c466
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;
+}