Some more co_sensor changes to bring block in line with AVR code.

Small fixes to trace code and trace server.

controller/controller/controller_trace.c

@@ -443,8 +443,11 @@ static void *controller_trace_server(void *arg)
 				trace_hdl[i].free = 0;
 				break;
 			}
-			if (i == nr_trace_hdl)
+			if (i == nr_trace_hdl) {
+				log_send(LOG_T_ERROR, 
+				    "No free trace handlers, closing trace connection");
 				close(client);
+			}
 		} else {
 			log_send(LOG_T_ERROR, "accept() failed: %s",
 			    strerror(errno));

controller/dt_ctrl.c

@@ -89,7 +89,7 @@ int main(int argc, char **argv)
 	blocks = controller_block_nr();
 	outputs = 0;
 	for (i = 0; i < blocks; i++) {
-		outputs += controller_block_get(i)->inputs;
+		outputs += controller_block_get(i)->outputs;
 	}
 	controller_trace_server_start(CTRL_TRACE_PORT, outputs);
 

controller/vesp/block_vesp_sensor_co.c

@@ -39,16 +39,23 @@
 
 struct controller_block_private {
 	float ppm;
+	float r;
+
+	uint8_t control;
+	uint8_t status;
 
 	void *data_rx;
 	void *data_tx;
 
+	uint8_t address;
+
 	struct controller_bus *bus;
 	struct controller_bus_client *client;
 };
 
 struct rx_packet {
-	uint16_t vdiv;
+	uint16_t ppm;
+	uint16_t r;
 	uint8_t status;
 } __packed;
 
@@ -57,9 +64,15 @@ struct tx_packet {
 } __packed;
 
 #define VESP_SENSOR_CO_CMD_CONTROL	4
+
 #define VESP_SENSOR_CO_CTRL_ENABLE	1
 #define VESP_SENSOR_CO_CTRL_RAW		2
+#define VESP_SENSOR_CO_CTRL_PREHEAT	4
+#define VESP_SENSOR_CO_STAT_TOGGLE	128
 
+#define VESP_SENSOR_CO_EEPROM_T_OFF	2
+#define VESP_SENSOR_CO_EEPROM_T_OFF_L	2
+#define VESP_SENSOR_CO_EEPROM_T_OFF_H	3
 
 static void calculate(struct controller_block *sensor)
 {
@@ -68,10 +81,14 @@ static void calculate(struct controller_block *sensor)
 	struct tx_packet *packet_tx = priv->data_tx;
 	
 	/* no calibration yet, just a direct conversion */
-	priv->ppm = be16toh(packet_rx->vdiv);
+	priv->ppm = be16toh(packet_rx->ppm);
+	priv->r = be16toh(packet_rx->r);
+	if (priv->status != (packet_rx->status & VESP_SENSOR_CO_STAT_TOGGLE)) {
+		priv->status = packet_rx->status & VESP_SENSOR_CO_STAT_TOGGLE;
+		log_send(LOG_T_DEBUG, "Got new ppm data");
+	}
 	
-	packet_tx->control = 
-	    VESP_SENSOR_CO_CTRL_ENABLE | VESP_SENSOR_CO_CTRL_RAW;
+	packet_tx->control = priv->control;
 }
 
 static void rx_callback(struct controller_block *block, void *data)
@@ -84,11 +101,66 @@ static void tx_callback(struct controller_block *block, void *data)
 	block->private->data_tx = data;
 }
 
+static int param_set_enable(struct controller_block *block, char *param, 
+    int argc, va_list val)
+{
+	if (va_arg(val, int))
+		__sync_fetch_and_or(&block->private->control, VESP_SENSOR_CO_CTRL_ENABLE);
+	else
+		__sync_fetch_and_and(&block->private->control, ~VESP_SENSOR_CO_CTRL_ENABLE);
+	return 0;
+}
+
+static int param_set_raw(struct controller_block *block, char *param, 
+    int argc, va_list val)
+{
+	if (va_arg(val, int))
+		__sync_fetch_and_or(&block->private->control, VESP_SENSOR_CO_CTRL_RAW);
+	else
+		__sync_fetch_and_and(&block->private->control, ~VESP_SENSOR_CO_CTRL_RAW);
+	return 0;
+}
+
+static int param_set_preheat(struct controller_block *block, char *param, 
+    int argc, va_list val)
+{
+	if (va_arg(val, int))
+		__sync_fetch_and_or(&block->private->control, VESP_SENSOR_CO_CTRL_PREHEAT);
+	else
+		__sync_fetch_and_and(&block->private->control, ~VESP_SENSOR_CO_CTRL_PREHEAT);
+	return 0;
+}
+
+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;
+	
+	vesp_command_write_eeprom(block->private->bus,
+	    block->private->address, VESP_SENSOR_CO_EEPROM_T_OFF_L,
+	    ms & 0xff);
+	vesp_command_write_eeprom(block->private->bus,
+	    block->private->address, VESP_SENSOR_CO_EEPROM_T_OFF_H,
+	    (ms >> 8) & 0xff);
+	
+	return 0;
+}
+
 static struct controller_block_outterm_list outterms[] = {
 	{ "ppm", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, ppm) },
+	{ "r", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, r) },
 	{ NULL }
 };
 
+static struct controller_block_param_list params[] = {
+	{ "enable", false, param_set_enable, .args = { "int", NULL } },
+	{ "preheat", false, param_set_preheat, .args = { "int", NULL } },
+	{ "raw", false, param_set_raw, .args = { "int", NULL } },
+	{ "t_off", false, param_set_toff, .args = { "double", NULL } },
+	{ NULL },
+};
+
 static struct controller_block * block_vesp_sensor_co_create(char *name,
     int argc, va_list ap)
 {
@@ -108,9 +180,10 @@ static struct controller_block * block_vesp_sensor_co_create(char *name,
 	sensor->private->bus = controller_bus_find(busname);
 	if (!sensor->private->bus)
 		goto err_bus;
+	
+	sensor->private->address = address;
 
-	//TODO: check device presence
-	if (vesp_command_get_details(sensor->private->bus, address, &vid, &dev)) {
+	while (vesp_command_get_details(sensor->private->bus, address, &vid, &dev)) {
 		log_send(LOG_T_ERROR, "%s: Could not get details of device 0x%02x",
 		    name, address);
 		goto err_dev;
@@ -131,12 +204,16 @@ static struct controller_block * block_vesp_sensor_co_create(char *name,
 	    sizeof(struct tx_packet), tx_callback))
 		goto err_bus_output;
 
+	if (controller_block_param_list_add(sensor, params))
+		goto err_params;
+
 	if (controller_block_add(sensor))
 		goto err_add;
 		
 	return sensor;
 
 err_add:
+err_params:
 err_bus_output:
 err_bus_input:
 err_dev:

controller/vesp/block_vesp_tty.c

@@ -69,13 +69,18 @@ static int vesp_tty_fd_recv(struct controller_block *vesp)
 	int fd = priv->fd;
 	void *rx_buffer = priv->rx_buffer;
 	
-	log_send(LOG_T_DEBUG, "%s: try to receive %zd", vesp->name, priv->rx_size);
+	if (fd < 0)
+		return -1;
+		
+//	log_send(LOG_T_DEBUG, "%s: try to receive %zd from fd %d", 
+//	    vesp->name, priv->rx_size, fd);
 	ret = read(fd, rx_buffer, priv->rx_size);
 	if (ret > 0) {
-		log_send(LOG_T_DEBUG, "received %zd bytes", ret);
+//		log_send(LOG_T_DEBUG, "received %zd bytes", ret);
 		vesp_input_process(priv->bus, rx_buffer, ret);
 	} else {
-		if (ret < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
+		if (ret == 0 ||
+		    (ret < 0 && errno != EAGAIN && errno != EWOULDBLOCK)) {
 			log_send(LOG_T_ERROR, 
 			    "%s: Error reading from device(%zd): %s",
 			    vesp->name, ret, strerror(errno));
@@ -157,13 +162,14 @@ static int vesp_tty_send(struct controller_bus *bus, void *data, size_t size)
 	if (fd < 0)
 		goto err;
 
-	log_send(LOG_T_DEBUG, "%s: send %zd bytes", bus->name, size);
 	ret = write(fd, data, size);
 	if (ret == size)
 		return 0;
 
 	log_send(LOG_T_ERROR, "%s: failed to send %zd bytes: %s", 
 	    vesp->name, ret, strerror(errno));
+	close(fd);
+	priv->fd = -1;
 err:
 	return -1;
 }
@@ -189,16 +195,17 @@ static int vesp_dev_open(struct controller_block *block)
 {
 	struct controller_block_private *priv = block->private;
         struct termios tty;
+	int fd;
 	
-	priv->fd = open(priv->devname, O_RDWR | O_NOCTTY | O_NONBLOCK);
-	if (priv->fd < 0) {
+	fd = open(priv->devname, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	if (fd < 0) {
 		log_send(LOG_T_ERROR, "%s: Opening VESP device %s failed",
 		    block->name, priv->devname);
 		goto err;
 	}
 
 	memset (&tty, 0, sizeof tty);
-	if (tcgetattr (priv->fd, &tty) != 0) {
+	if (tcgetattr (fd, &tty) != 0) {
 		log_send(LOG_T_ERROR, "%s: tcgetattr(): %s", 
 		    block->name, strerror(errno));
 		goto err;
@@ -228,7 +235,7 @@ static int vesp_dev_open(struct controller_block *block)
 	tty.c_cflag &= ~CSTOPB;
 	tty.c_cflag &= ~CRTSCTS;
 
-	if (tcsetattr (priv->fd, TCSANOW, &tty) != 0) {
+	if (tcsetattr (fd, TCSANOW, &tty) != 0) {
 		log_send(LOG_T_DEBUG, "%s: tcsetattr() %s",
 		    block->name, strerror(errno));
                 goto err;
@@ -264,19 +271,20 @@ static int vesp_dev_open(struct controller_block *block)
 	/* Set this flag if you want to receive data even whilst sending data */
 	//rs485conf.flags |= SER_RS485_RX_DURING_TX;
 
-	if (ioctl (priv->fd, TIOCSRS485, &rs485conf) < 0) {
-		log_send(LOG_T_WARNING, "%s: ioctl(TIOCSRS485): %s", 
-		    block->name, strerror(errno));
+	if (ioctl (fd, TIOCSRS485, &rs485conf) < 0) {
+		log_send(LOG_T_WARNING, "%s: ioctl(%d, TIOCSRS485): %s", 
+		    block->name, fd, strerror(errno));
 	}
 #endif
 
 	log_send(LOG_T_INFO, "%s: Opened %s", block->name, priv->devname);
+	priv->fd = fd;
 
 	return 0;
 
 err:
-	if (priv->fd >= 0) {
-		close(priv->fd);
+	if (fd >= 0) {
+		close(fd);
 		priv->fd = -1;
 	}
 	return -1;
@@ -290,11 +298,9 @@ static enum controller_bus_state vesp_tty_poll(struct controller_bus *bus)
 	if (fd >= 0)
 		return CONTROLLER_BUS_STATE_OK;
 	
-	fd = vesp_dev_open(block);
-	if (fd < 0)
+	if (vesp_dev_open(block))
 		return CONTROLLER_BUS_STATE_ERROR;
 	
-	block->private->fd = fd;
 	log_send(LOG_T_WARNING, "%s: re-opened %s", block->name,
 	    block->private->devname);
 

controller/vesp/vesp.c

@@ -226,9 +226,11 @@ int vesp_input_process(struct controller_bus *bus, void *data, size_t size)
 	for (p = 0; p < size; p++) {
 		uint8_t b = data8[p];
 		
-		log_send(LOG_T_DEBUG, "%s: recv %zd: 0x%02x", bus->name, p, b);
+//		log_send(LOG_T_DEBUG, "%s: recv %zd: 0x%02x", bus->name, p, b);
 		
 		if (b == VESP_PKT_END || priv->rx_cur >= priv->rx_size) {
+//			log_send(LOG_T_DEBUG, "Got end, %zd bytes, crc: 0x%02x",
+//			    priv->rx_cur, priv->rx_crc);
 			/* crc must be ok and the packet must have atleast 2 bytes (add + resp) */
 			if (!priv->rx_crc && priv->rx_cur > 2) {
 				vesp_input_process_pkt(bus, priv->rx_buffer, priv->rx_cur - 1);
@@ -251,6 +253,7 @@ int vesp_input_process(struct controller_bus *bus, void *data, size_t size)
 				priv->rx_esc = false;
 				priv->rx_buffer[priv->rx_cur] = b;
 				priv->rx_crc = vesp_crc8(priv->rx_crc, b);
+				priv->rx_cur++;
 			}
 		}
 	}
@@ -301,9 +304,9 @@ int vesp_output_frame(struct controller_bus *bus, void *data, size_t size)
 	}
 	frame[pf++] = VESP_PKT_END;
 
-	for (pp = 0; pp < pf; pp++) {
-		log_send(LOG_T_DEBUG, "%s: send[%zd]: 0x%02x", bus->name, pp, frame[pp]);
-	}
+//	for (pp = 0; pp < pf; pp++) {
+//		log_send(LOG_T_DEBUG, "%s: send[%zd]: 0x%02x", bus->name, pp, frame[pp]);
+//	}
 
 	return priv->send(bus, frame, pf);
 }

controller/vesp/vesp_tty.ctrl

@@ -20,6 +20,11 @@ links {
 params {
 #	{ "operational", "value", 4, (int) { true, true, true, true } }
 
+	{ "co_sensor", "enable",  true }
+	{ "co_sensor", "raw",     true }
+	{ "co_sensor", "preheat", false }
+	{ "co_sensor", "t_off",   0.1 }
+
 #	{ "co_sensor_ppm", "value", 4,
 #	    (float){ 0.0, 0.0, 0.0, 0.0 },
 #	    (float){ 0.0, 0.0, 0.0, 0.0 }