Add controller block

controller/vesp/block_vesp_controller.c

+/*
+	Copyright Jeroen Vreeken (jeroen@vreeken.net), 2015
+
+	This program is free software: you can redistribute it and/or modify
+	it under the terms of the GNU General Public License as published by
+	the Free Software Foundation, either version 3 of the License, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <controller/controller_block.h>
+#include <controller/controller_bus.h>
+#include <log/log.h>
+#include <vesp/vesp.h>
+#include <vesp/vesp_ids.h>
+
+/*
+  	 outputs
+  	 nr name
+
+      -----------------
+      |	              |
+  ----| led0    valid |----
+  ----|	led1	      |
+  ----| led2    sw0   |----
+  ----| led3    sw1   |----
+      |	        sw2   |----
+      |		      |
+      |	        adc   |----
+      |		      |
+      -----------------
+ */
+
+struct controller_block_private {
+	bool *led0;
+	bool *led1;
+	bool *led2;
+	bool *led3;
+	float adc;
+	bool sw0;
+	bool sw1;
+	bool sw2;
+	bool valid;
+
+	bool toggle;
+
+	void *data_rx;
+	void *data_tx;
+
+	uint8_t address;
+
+	struct controller_bus *bus;
+};
+
+struct rx_packet {
+	uint16_t adc;
+	uint8_t switches;
+	uint8_t status;
+} __packed;
+
+struct tx_packet {
+	uint8_t leds;
+} __packed;
+
+#define VESP_CONTROLLER_CMD_CONTROL	4
+
+#define VESP_CONTROLLER_STAT_TOGGLE	128
+
+#define VESP_CONTROLLER_EEPROM_T_OFF	2
+#define VESP_CONTROLLER_EEPROM_T_OFF_L	2
+#define VESP_CONTROLLER_EEPROM_T_OFF_H	3
+
+static void calculate_controller_rx(struct controller_block *controller)
+{
+	struct controller_block_private *priv = controller->private;
+	struct rx_packet *packet_rx = priv->data_rx;
+	uint8_t switches;
+	uint16_t adc;
+	bool toggle;
+	
+	switches = packet_rx->switches;
+	adc = be16toh(packet_rx->adc);
+	
+	/* 0     -> -1.0 
+	   16383 ->  0.0
+	   16384 ->  0.0
+	   32767 ->  1.0
+	 */
+	if (adc & 0x8000)
+		priv->adc = ((float)(adc) - 16384.0) / 16387.0;
+	else
+		priv->adc = ((float)(adc) - 16383.0) / 16387.0;
+	priv->sw0 = switches & 0x01;
+	priv->sw1 = switches & 0x02;
+	priv->sw2 = switches & 0x04;
+
+	toggle = packet_rx->status & VESP_CONTROLLER_STAT_TOGGLE;
+	priv->valid = toggle != priv->toggle;
+	priv->toggle = toggle;
+}
+
+static void calculate_controller_tx(struct controller_block *controller)
+{
+	struct controller_block_private *priv = controller->private;
+	struct tx_packet *packet_tx = priv->data_tx;
+
+	packet_tx->leds = 
+	    (*priv->led0 << 0) |
+	    (*priv->led1 << 1) |
+	    (*priv->led2 << 2) |
+	    (*priv->led3 << 3);
+}
+
+static void rx_callback(struct controller_block *block, void *data)
+{
+	block->private->data_rx = data;
+}
+
+static void tx_callback(struct controller_block *block, void *data)
+{
+	block->private->data_tx = data;
+}
+
+static int param_set_toff(struct controller_block *block, char *param, 
+    int argc, va_list val)
+{
+	double t_off = va_arg(val, double);
+	uint16_t ms = t_off * 1000;
+	
+	if (vesp_command_write_eeprom(block->private->bus,
+	    block->private->address, VESP_CONTROLLER_EEPROM_T_OFF_L,
+	    ms & 0xff))
+		goto err;
+	if (vesp_command_write_eeprom(block->private->bus,
+	    block->private->address, VESP_CONTROLLER_EEPROM_T_OFF_H,
+	    (ms >> 8) & 0xff))
+		goto err;
+	
+	return 0;
+err:
+	log_send(LOG_T_ERROR, "%s: Failed to set t_off", block->name);
+	return -1;
+}
+
+static struct controller_block_outterm_list outterms[] = {
+	{ "valid", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, valid) },
+	{ "sw0", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, sw0) },
+	{ "sw1", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, sw1) },
+	{ "sw2", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, sw2) },
+	{ "adc", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, adc) },
+	{ NULL }
+};
+
+static struct controller_block_interm_list interms[] = {
+	{ "led0", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, led0) },
+	{ "led1", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, led1) },
+	{ "led2", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, led2) },
+	{ "led3", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, led3) },
+	{ NULL }
+};
+
+static struct controller_block_param_list params[] = {
+	{ "t_off", false, param_set_toff, .args = { "double", NULL } },
+	{ NULL },
+};
+
+static struct controller_block * block_vesp_controller_create(char *name,
+    int argc, va_list ap)
+{
+	struct controller_block *controller;
+	struct controller_block *controller_tx;
+	char *busname = va_arg(ap, char *);
+	uint8_t address = va_arg(ap, int);
+	uint8_t vid, dev;
+	char *name_tx;
+	int i;
+	
+	asprintf(&name_tx, "%s_tx", name);
+	
+	if (!(controller = controller_block_alloc("vesp_controller", name, sizeof(struct controller_block_private))))
+		return NULL;
+	if (!(controller_tx = controller_block_alloc("vesp_controller_tx", name_tx, 0)))
+		goto err_controller;
+	free(name_tx);
+
+	controller->calculate = calculate_controller_rx;
+	controller_tx->calculate = calculate_controller_tx;
+
+	if (controller_block_outterm_list_init(controller, outterms))
+		goto err_block;
+
+	if (controller_block_interm_list_init(controller_tx, interms))
+		goto err_block;
+
+	controller->inputs = controller_tx->inputs;
+	controller->input = calloc(controller->inputs, sizeof(struct controller_block_interm));
+	if (!controller->input)
+		goto err_input;
+
+	for (i = 0; i < controller->inputs; i++) {
+		controller->input[i].name = controller_tx->input[i].name;
+		controller->input[i].type = controller_tx->input[i].type;
+		controller->input[i].value = controller_tx->input[i].value;
+		controller->input[i].ghostof =
+		    calloc(2, sizeof(struct controller_block_interm *));
+		controller->input[i].ghostof[0] = &controller_tx->input[i];
+	}
+
+	controller->private->bus = controller_bus_find(busname);
+	if (!controller->private->bus)
+		goto err_bus;
+	
+	controller->private->address = address;
+
+	while (vesp_command_get_details(controller->private->bus, address, &vid, &dev)) {
+		log_send(LOG_T_ERROR, "%s: Could not get details of device 0x%02x",
+		    name, address);
+		goto err_dev;
+	}
+	if (vid != VESP_VID_VREEKEN || dev != VESP_DEV_VREEKEN_CONTROLLER) {
+		log_send(LOG_T_ERROR,
+		    "%s: VID/DEV ids do not match: (0x%02x, 0x%02x) != (0x%02x, 0x%02x)",
+		    name, vid, dev, VESP_VID_VREEKEN, VESP_DEV_VREEKEN_CONTROLLER);
+		goto err_dev;
+	}
+
+	if (vesp_bus_add_input(controller, controller->private->bus,
+	    address, VESP_RESP_OOB, sizeof(struct rx_packet), rx_callback))
+		goto err_bus_input;
+	
+	if (vesp_bus_add_output(controller_tx, controller->private->bus,
+	    address, VESP_CONTROLLER_CMD_CONTROL,
+	    sizeof(struct tx_packet), tx_callback))
+		goto err_bus_output;
+
+	if (controller_block_param_list_add(controller, params))
+		goto err_params;
+
+	if (controller_block_add(controller))
+		goto err_add;
+		
+	return controller;
+
+err_add:
+err_params:
+err_bus_output:
+err_bus_input:
+err_dev:
+err_bus:
+err_input:
+err_block:
+	controller_block_free(controller_tx);
+err_controller:
+	controller_block_free(controller);
+	return NULL;
+}
+
+
+BLOCK_CREATE(vesp_controller) = {
+	.create = block_vesp_controller_create,
+	.args = { "char*,int", NULL },
+};

controller/vesp/build.mk

@@ -2,6 +2,7 @@
 VESP_BLOCKS := \
 	vesp_sim \
 	vesp_tty \
+	vesp_controller \
 	vesp_sensor_co
 
 

controller/vesp/vesp_ids.h

@@ -22,6 +22,7 @@
 #define VESP_VID_VITSCH		0x04
 #define VESP_VID_VREEKEN	0x0a
 
-#define VESP_DEV_VREEKEN_CO	0x01
+#define VESP_DEV_VREEKEN_CO		0x01
+#define VESP_DEV_VREEKEN_CONTROLLER	0x02
 
 #endif /* _INCLUDE_VESP_IDS_H_ */