[OSADL-svn-commits] r112 - trunks/fddi-trunk/src

[OSADL repository commits]

Author: robert
Date: Thu Sep 13 22:19:18 2007
New Revision: 112

Log:


Modified:
   trunks/fddi-trunk/src/libfddi_can.c

Modified: trunks/fddi-trunk/src/libfddi_can.c
==============================================================================
--- trunks/fddi-trunk/src/libfddi_can.c	(original)
+++ trunks/fddi-trunk/src/libfddi_can.c	Thu Sep 13 22:19:18 2007
@@ -27,6 +27,7 @@
 #include <linux/can/raw.h>
 
 #include <osadl/fddi.h>
+#include <libpv.h>
 
 static fddi_tpu_t tpu1 = {
 	.id = {
@@ -152,37 +153,102 @@
 
 can_iface_t can_iface1;
 
+
+struct priv {
+	pv_engine_t	pv_engine;
+
+	float		*pv_v_tractor;
+	float		*pv_n_diesel;
+	float		*pv_u_dc;
+	float		*pv_i_dc;
+	float		*pv_agitation_rate;
+	float		*pv_spray_rate;
+};
+
+struct priv priv;
+
 #define MAXDEV 6
 
 void* _can_thread(void *arg)
 {
 	fd_set rdfs;
-	int fd;
-	int bytes;
+	int fd = can_iface1.socket;
+	ssize_t bytes;
 	int retval;
 	struct can_frame frame;
+	struct timeval to;
 
 	FD_ZERO(&rdfs);
-	FD_SET(fd, &rdfs);
 
 	while (!(can_tpu.endme)) {
+		to.tv_sec = 1;
+		to.tv_usec = 0;
+		FD_SET(fd, &rdfs);
+
+		retval = select(fd + 1, &rdfs, NULL, NULL, &to);
+
+		/* timeout */
+		if (retval == 0)
+			continue;
+
+		if (retval == -1) {
+			retval = -errno;
+			if (retval == -EINTR)
+				continue;
 
-		if ((retval = select(fd, &rdfs, NULL, NULL, NULL)) < 0) {
 			perror("select");
 			can_tpu.endme = 1;
 			continue;
 		}
 
 		bytes = recv(fd, &frame, sizeof(struct can_frame), 0);
-		if (bytes < sizeof(struct can_frame)) {
+		if (bytes < (ssize_t)sizeof(struct can_frame)) {
 			fprintf(stderr, "error: received short frame");
 			exit(-1);
 		}
 
+		switch (frame.can_id) {
+		case (0x18E0F108 & CAN_EFF_MASK) | CAN_EFF_FLAG: {
+			int v_tractor_can =
+				frame.data[0] << 8 |
+				frame.data[1] << 0;
+			int n_diesel_can =
+				frame.data[2] << 8 |
+				frame.data[3] << 0;
+			int u_dc_can =
+				frame.data[4] << 8 |
+				frame.data[5] << 0;
+			int i_dc_can =
+				frame.data[6] << 8 |
+				frame.data[7] << 0;
+
+			pv_lock(&priv.pv_engine);
+			*(priv.pv_v_tractor)	= v_tractor_can;
+			*(priv.pv_n_diesel)	= n_diesel_can;
+			*(priv.pv_u_dc)		= u_dc_can;
+			*(priv.pv_i_dc)		= i_dc_can;
+			pv_unlock(&priv.pv_engine);
+			break;
+		}
+		case (0x18E0F208 & CAN_EFF_MASK) | CAN_EFF_FLAG: {
+			int agitation_rate_can =
+				frame.data[0] << 8 |
+				frame.data[1] << 0;
+			int spray_rate_can =
+				frame.data[2] << 8 |
+				frame.data[3] << 0;
+
+			pv_lock(&priv.pv_engine);
+			*(priv.pv_agitation_rate)	= agitation_rate_can;
+			*(priv.pv_spray_rate)		= spray_rate_can;
+			pv_unlock(&priv.pv_engine);
+			break;
+		}
+		default:
+			fprintf(stderr, "Unknown can_id 0x%08x\n", frame.can_id & CAN_EFF_MASK);
+		}
 
-
-
-		sleep(1);
+		
 	}
 
 	return NULL;
@@ -246,6 +312,8 @@
 {
 	can_iface_t *can_iface = (can_iface_t*)(iface->priv);
 
+	pv_destroy(&priv.pv_engine);
+
 	close(can_iface->socket);
 
 	return 0;
@@ -253,9 +321,6 @@
 
 int fddi_backend_configure(fddi_iface_t *iface, const char *configfile, void *data)
 {
-	int ret;
-	can_iface_t *can_iface = (can_iface_t*)(iface->priv);
-
 	printf("parsing configfile: %s\n", configfile);
 //	printf("for interface:      %s\n", iface->id.id_as_string); /* FIXME private */
 
@@ -269,7 +334,7 @@
 
 	can_iface1.addr.can_family = AF_CAN;
 
-	strcpy(can_iface1.ifr.ifr_name, "vcan0");
+	strcpy(can_iface1.ifr.ifr_name, "can0");
 	if (ioctl(can_iface1.socket, SIOCGIFINDEX, &can_iface1.ifr) < 0) {
 		perror("attaching to can interface");
 		return 1;
@@ -301,6 +366,35 @@
 	pv_arate_can.next = &pv_srate_can;
 	pv_srate_can.previous = &pv_arate_can;
 
+	{
+		pv_engine_attr_t	attr;
+		int			err;
+
+		err = pv_engine_attr_init_from_env(&attr);
+		if (err)
+			return err;
+
+		err = pv_init(&priv.pv_engine, &attr);
+		if (err) {
+			printf("couldn't init pv engine\n");
+			return err;
+		}
+		priv.pv_v_tractor =      pv_register_float(&priv.pv_engine, "v_tractor");
+		priv.pv_n_diesel =       pv_register_float(&priv.pv_engine, "n_diesel");
+		priv.pv_u_dc =           pv_register_float(&priv.pv_engine, "u_dc");
+		priv.pv_i_dc =           pv_register_float(&priv.pv_engine, "i_dc");
+		priv.pv_agitation_rate = pv_register_float(&priv.pv_engine, "agitation_rate");
+		priv.pv_spray_rate =     pv_register_float(&priv.pv_engine, "spray_rate");
+
+		if ((priv.pv_v_tractor == NULL) || (priv.pv_n_diesel == NULL) || (priv.pv_u_dc == NULL) ||
+		    (priv.pv_i_dc == NULL) || (priv.pv_agitation_rate == NULL) || (priv.pv_spray_rate == NULL))  {
+			fprintf(stderr, "could not register variables\n");
+			pv_destroy(&priv.pv_engine);
+			return -EINVAL;
+		}
+	}
+
+
 	/* we are ready now and go into preoperational state */
 	fddi_backend_setstate(iface, STATE_PREOPERATIONAL);