Commit 465d5003 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add controller block

parent 0677a8ed
/*
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 },
};
......@@ -2,6 +2,7 @@
VESP_BLOCKS := \
vesp_sim \
vesp_tty \
vesp_controller \
vesp_sensor_co
......
......@@ -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_ */
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment