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