r4457 - in developers/alphaone/u-blox/libubx: . include src

alphaone at docs.openmoko.org alphaone at docs.openmoko.org
Wed May 28 01:27:43 CEST 2008


Author: alphaone
Date: 2008-05-28 01:27:42 +0200 (Wed, 28 May 2008)
New Revision: 4457

Modified:
   developers/alphaone/u-blox/libubx/ChangeLog
   developers/alphaone/u-blox/libubx/include/libubx.h
   developers/alphaone/u-blox/libubx/src/libubx.c
   developers/alphaone/u-blox/libubx/src/ubx-get.c
Log:
* libubx/include/libubx.h, libubx/src/libubx.c: Add funtions for parsing and
  generating almanac and ephemeris packets.
* libubx/src/ubx-get.c: Add support to get almanac from GPS device.


Modified: developers/alphaone/u-blox/libubx/ChangeLog
===================================================================
--- developers/alphaone/u-blox/libubx/ChangeLog	2008-05-27 23:27:30 UTC (rev 4456)
+++ developers/alphaone/u-blox/libubx/ChangeLog	2008-05-27 23:27:42 UTC (rev 4457)
@@ -1,3 +1,9 @@
+2008-05-20	Daniel Willmann <daniel at openmoko.org>
+
+	* libubx/include/libubx.h, libubx/src/libubx.c: Add funtions for parsing and
+	generating almanac and ephemeris packets.
+	* libubx/src/ubx-get.c: Add support to get almanac from GPS device.
+
 2008-05-12	Daniel Willmann <daniel at openmoko.org>
 
 	* libubx/src/Makefile.am, libubx/src/ubx-get.c: Add boilerplate code for

Modified: developers/alphaone/u-blox/libubx/include/libubx.h
===================================================================
--- developers/alphaone/u-blox/libubx/include/libubx.h	2008-05-27 23:27:30 UTC (rev 4456)
+++ developers/alphaone/u-blox/libubx/include/libubx.h	2008-05-27 23:27:42 UTC (rev 4457)
@@ -91,7 +91,7 @@
  * If the checksum fields are both zero calculates it */
 uint8_t checksum(ubx_hdr *header, uint8_t *msg);
 
-uint16_t ubx_msg_encode(ubx_hdr *header, uint8_t *msg, uint8_t **data);
+uint16_t ubx_msg_encode(ubx_msg *msg, uint8_t **data);
 ubx_msg *ubx_msg_decode(uint16_t length, uint8_t *data);
 
 #endif /* _LIBUBX_H_ */

Modified: developers/alphaone/u-blox/libubx/src/libubx.c
===================================================================
--- developers/alphaone/u-blox/libubx/src/libubx.c	2008-05-27 23:27:30 UTC (rev 4456)
+++ developers/alphaone/u-blox/libubx/src/libubx.c	2008-05-27 23:27:42 UTC (rev 4457)
@@ -28,7 +28,7 @@
  * If the checksum fields are both zero calculates it */
 uint8_t checksum(ubx_hdr *header, uint8_t *msg)
 {
-	uint8_t ck_a = 0, ck_b = 0;
+	uint8_t ck_a = 0, ck_b = 0, i;
 	uint16_t *len = &header->length;
 
 	ck_a += header->cls;
@@ -40,6 +40,11 @@
 	ck_a += ((uint8_t *)len)[1];
 	ck_b += ck_a;
 
+	for (i=0; i < header->length; i++) {
+		ck_a += msg[i];
+		ck_b += ck_a;
+	}
+
 	if ((ck_a == header->ck_a)&&(ck_b == header->ck_b)) {
 		return 1;
 	}
@@ -50,9 +55,55 @@
 	return 0;
 }
 
-uint16_t ubx_msg_encode(ubx_hdr *header, uint8_t *msg, uint8_t **data)
+void ubx_aid_encode(uint8_t *dst, ubx_msg *src)
 {
+	ubx_hdr *hdr = src->header;
+	switch (hdr->id) {
+		case UBX_ID_ALM:
+			/* UBX AID_ALM */
+			if (hdr->length == 0 || hdr->length == 1 ||
+					hdr->length == 8 || hdr->length == 40) {
+				memcpy(dst, src->payload, hdr->length);
+			} else {
+				printf("Malformed UBX AID_ALM packet\n");
+			}
+			return;
+			break;
+		case UBX_ID_EPH:
+			/* UBX_AID_EPH */
+			if (hdr->length == 0 || hdr->length == 1 ||
+					hdr->length == 8 || hdr->length == 104) {
+				memcpy(dst, src->payload, hdr->length);
+			} else {
+				printf("Malformed UBX AID_EPH packet\n");
+			}
+			return;
+			break;
+		default:
+			printf("Unknown UBX AID Id 0x%x\n", hdr->id);
+	}
+}
+
+void ubx_msg_payload_encode(uint8_t *dst, ubx_msg *src)
+{
+	ubx_hdr *hdr = src->header;
+	switch (hdr->cls) {
+		case UBX_CL_AID:
+			ubx_aid_encode(dst, src);
+			break;
+		case UBX_CL_CFG:
+//			ubx_cfg_encode(dst, src);
+		default:
+			printf("Unknown UBX class 0x%x\n", hdr->cls);
+	}
+}
+
+
+uint16_t ubx_msg_encode(ubx_msg *msg, uint8_t **data)
+{
+	ubx_hdr *header = msg->header;
 	uint8_t *temp;
+
 	*data = malloc(header->length + 8);
 	if (!*data)
 		return 0;
@@ -63,16 +114,56 @@
 	*temp++ = UBX_SYNC2;
 	// Copy the header
 	// XXX: Handle Endianess
-	memcpy(temp, header, 4);
+	memcpy(temp, msg->header, 4);
 	temp += 4;
 	// And the message
-	memcpy(temp, msg, header->length);
+	ubx_msg_payload_encode(temp, msg);
 	temp += header->length;
 	*temp++ = header->ck_a;
 	*temp++ = header->ck_b;
 	return header->length + 8;
 }
 
+uint8_t *ubx_aid_decode(uint8_t id, uint16_t length, uint8_t *data)
+{
+	uint8_t *value;
+
+	switch (id) {
+		case UBX_ID_ALM:
+			/* AID_ALM */
+			if (length == 0 || length == 1 ||
+					length == 8 || length == 40) {
+				value = malloc(sizeof(ubx_msg_aid_alm));
+				memcpy(value, data, length);
+			}
+			break;
+		case UBX_ID_EPH:
+			/* AID_EPH*/
+			if (length == 0 || length == 1 ||
+					length == 8 || length == 104) {
+				value = malloc(sizeof(ubx_msg_aid_eph));
+				memcpy(value, data, length);
+			}
+			break;
+		default:
+			printf("Unknown AID ID 0x%x\n", id);
+			return NULL;
+	}
+	return value;
+}
+
+uint8_t *ubx_msg_payload_decode(uint8_t cls, uint8_t id, uint16_t length, uint8_t *data)
+{
+	switch (cls) {
+		case UBX_CL_AID:
+			return ubx_aid_decode(id, length, data);
+			break;
+		default:
+			printf("Unknown class 0x%x\n", cls);
+	}
+	return 0;
+}
+
 ubx_msg *ubx_msg_decode(uint16_t length, uint8_t *data)
 {
 	ubx_hdr *header;
@@ -89,14 +180,16 @@
 	header->length = (uint16_t)*data;
 	data += 2;
 
-	if (length < header->length + 8)
+	if (length < header->length + 8) {
+		free(header);
 		return 0;
+	}
 
 	header->ck_a = data[header->length];
 	header->ck_b = data[header->length+1];
 
 	if (!checksum(header, data)) {
-		free (header);
+		free(header);
 		return 0;
 	}
 
@@ -104,7 +197,7 @@
 	msg = malloc(sizeof(ubx_msg));
 	msg->header = header;
 
-	msg->payload = ubx_parse_msg(header->cls, header->id, header->length, data);
+	msg->payload = ubx_msg_payload_decode(header->cls, header->id, header->length, data);
 
 	return msg;
 }

Modified: developers/alphaone/u-blox/libubx/src/ubx-get.c
===================================================================
--- developers/alphaone/u-blox/libubx/src/ubx-get.c	2008-05-27 23:27:30 UTC (rev 4456)
+++ developers/alphaone/u-blox/libubx/src/ubx-get.c	2008-05-27 23:27:42 UTC (rev 4457)
@@ -19,92 +19,179 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
 
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <errno.h>
 #include <getopt.h>
 #include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
+#include <termios.h>
+#include <unistd.h>
+#include <fcntl.h>
 #include <libubx.h>
 
 
 /* Flag set by `--verbose'. */
-static int verbose_flag;
+static int verbose;
 static int get_alm, get_eph;
-static char *gpsname, *filename;
+static char *gpsname, *filename = NULL;
 
+static int gpsopen(const char *path, speed_t speed);
+
+void usage ()
+{
+	printf("Usage:\n");
+	printf("\t--verbose Display more information about what's going on\n");
+	printf("\t--gps <name> Specify GPS device path\n");
+	printf("\t--file <name> Specify file name to output response\n");
+	printf("\t--almanac Request almanac from GPS\n");
+	printf("\t--ephemeris Request ephemeris from GPS\n");
+	printf("\t--help Show this help\n");
+	exit(1);
+}
+
+void request_almanac(int fd)
+{
+	uint8_t *data, len, i;
+	ubx_msg msg;
+	msg.header = malloc(sizeof(ubx_hdr));
+	msg.header->cls = UBX_CL_AID;
+	msg.header->id = UBX_ID_ALM;
+	msg.header->length = 0;
+	msg.payload = NULL;
+
+	len = ubx_msg_encode(&msg, &data);
+
+	if (verbose) {
+		printf("Writing almanac request: ");
+		for (i=0;i<len; i++) {
+			printf("0x%x ", data[i]);
+		}
+		printf("\n");
+	}
+
+	write(fd, data, len);
+}
+
 int main (int argc, char *argv[])
 {
-  int c;
+	int c, gpsfd;
 
-  while (1)
-    {
-      static struct option long_options[] =
-        {
-          {"verbose", no_argument,       0, 'v'},
-					{"gps",			required_argument, 0, 'g'},
-          {"file",    required_argument, 0, 'f'},
-          {"almanac", no_argument,			 0, 'a'},
-          {"ephemeris", no_argument,		 0, 'e'},
-          {0, 0, 0, 0}
-        };
-      /* getopt_long stores the option index here. */
-      int option_index = 0;
+	while (1)
+	{
+		static struct option long_options[] =
+		{
+			{"verbose", no_argument,       0, 'v'},
+			{"gps",			required_argument, 0, 'g'},
+			{"file",    required_argument, 0, 'f'},
+			{"almanac", no_argument,			 0, 'a'},
+			{"ephemeris", no_argument,		 0, 'e'},
+			{"help",		no_argument,			 0, 'h'},
+			{0, 0, 0, 0}
+		};
 
-      c = getopt_long (argc, argv, "vg:f:ae",
-                       long_options, &option_index);
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
 
-      /* Detect the end of the options. */
-      if (c == -1)
-        break;
+		c = getopt_long (argc, argv, "vg:f:aeh",
+				long_options, &option_index);
 
-      switch (c)
-        {
-        case 0:
-          /* If this option set a flag, do nothing else now. */
-          if (long_options[option_index].flag != 0)
-            break;
-          printf ("option %s", long_options[option_index].name);
-          if (optarg)
-            printf (" with arg %s", optarg);
-          printf ("\n");
-          break;
+		/* Detect the end of the options. */
+		if (c == -1)
+			break;
 
-        case 'v':
-          verbose_flag = 1;
-          break;
+		switch (c)
+		{
+			case 'v':
+				verbose = 1;
+				break;
 
-        case 'g':
-          gpsname = optarg;
-          break;
+			case 'g':
+				gpsname = optarg;
+				break;
 
-        case 'f':
-          filename = optarg;
-          break;
+			case 'f':
+				filename = optarg;
+				break;
 
-        case 'a':
-					get_alm = 1;
-          break;
+			case 'a':
+				get_alm = 1;
+				break;
 
-        case 'e':
-					get_eph = 1;
-          break;
+			case 'e':
+				get_eph = 1;
+				break;
 
-        case '?':
-          /* getopt_long already printed an error message. */
-          break;
+			case 'h':
+				usage();
+				break;
 
-        default:
-          abort ();
-        }
-    }
+			default:
+				usage();
+		}
+	}
 
-  /* Print any remaining command line arguments (not options). */
-  if (optind < argc)
-    {
-      printf ("non-option ARGV-elements: ");
-      while (optind < argc)
-        printf ("%s ", argv[optind++]);
-      putchar ('\n');
-    }
+	if (optind < argc) {
+		usage();
+	}
 
-  exit (0);
+	if (gpsname == NULL) {
+		printf("Please specify a GPS device\n");
+		usage();
+	}
+
+	gpsfd = gpsopen(gpsname, 9600);
+
+	if (gpsfd < 0) {
+		printf("GPS device could not be opened at %s\n", gpsname);
+		printf("Reason: %s\n", strerror(errno));
+		exit(1);
+	}
+	gpsfd=1;
+
+	if (get_alm) {
+		request_almanac(gpsfd);
+	}
+
 }
 
+static int gpsopen(const char *path, speed_t speed)
+{
+	int fd;
+	struct termios term;
+
+	fd = open(path, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	if (fd < 0)
+		return fd;
+
+	if (speed == B0)
+		return fd;
+
+	if (tcgetattr(fd, &term) < 0) {
+//		close(fd);
+		printf("tcgetattr failed!\n");
+		return fd;
+	}
+
+	term.c_cflag &= ~(PARENB | PARODD | CRTSCTS);
+	term.c_cflag |= CREAD | CLOCAL;
+
+	term.c_iflag &= ~(PARMRK | INPCK);
+	term.c_cflag &= ~(CSIZE | CSTOPB | PARENB | PARODD);
+	term.c_cflag |= CS8;
+
+	term.c_lflag &= ~ICANON;
+	term.c_lflag &= ~ECHO;
+
+	cfsetspeed(&term, speed);
+
+	if (tcsetattr(fd, TCSANOW, &term) < 0) {
+		printf("tcsetattr failed\n");
+		close(fd);
+
+		return -1;
+	}
+
+  return fd;
+}





More information about the commitlog mailing list