Changeset 4457


Ignore:
Timestamp:
05/28/08 01:27:42 (5 years ago)
Author:
alphaone
Message:
  • 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.
Location:
developers/alphaone/u-blox/libubx
Files:
4 edited

Legend:

Unmodified
Added
Removed

  • developers/alphaone/u-blox/libubx/ChangeLog

    r4456 r4457  
     12008-05-20      Daniel Willmann <daniel@openmoko.org> 
     2 
     3        * libubx/include/libubx.h, libubx/src/libubx.c: Add funtions for parsing and 
     4        generating almanac and ephemeris packets. 
     5        * libubx/src/ubx-get.c: Add support to get almanac from GPS device. 
     6 
    172008-05-12      Daniel Willmann <daniel@openmoko.org> 
    28 

  • developers/alphaone/u-blox/libubx/include/libubx.h

    r4409 r4457  
    9292uint8_t checksum(ubx_hdr *header, uint8_t *msg); 
    9393 
    94 uint16_t ubx_msg_encode(ubx_hdr *header, uint8_t *msg, uint8_t **data); 
     94uint16_t ubx_msg_encode(ubx_msg *msg, uint8_t **data); 
    9595ubx_msg *ubx_msg_decode(uint16_t length, uint8_t *data); 
    9696 

  • developers/alphaone/u-blox/libubx/src/libubx.c

    r4455 r4457  
    2929uint8_t checksum(ubx_hdr *header, uint8_t *msg) 
    3030{ 
    31         uint8_t ck_a = 0, ck_b = 0; 
     31        uint8_t ck_a = 0, ck_b = 0, i; 
    3232        uint16_t *len = &header->length; 
    3333 
    … …  
    4040        ck_a += ((uint8_t *)len)[1]; 
    4141        ck_b += ck_a; 
     42 
     43        for (i=0; i < header->length; i++) { 
     44                ck_a += msg[i]; 
     45                ck_b += ck_a; 
     46        } 
    4247 
    4348        if ((ck_a == header->ck_a)&&(ck_b == header->ck_b)) { 
    … …  
    5156} 
    5257 
    53 uint16_t ubx_msg_encode(ubx_hdr *header, uint8_t *msg, uint8_t **data) 
    54 { 
     58void ubx_aid_encode(uint8_t *dst, ubx_msg *src) 
     59{ 
     60        ubx_hdr *hdr = src->header; 
     61        switch (hdr->id) { 
     62                case UBX_ID_ALM: 
     63                        /* UBX AID_ALM */ 
     64                        if (hdr->length == 0 || hdr->length == 1 || 
     65                                        hdr->length == 8 || hdr->length == 40) { 
     66                                memcpy(dst, src->payload, hdr->length); 
     67                        } else { 
     68                                printf("Malformed UBX AID_ALM packet\n"); 
     69                        } 
     70                        return; 
     71                        break; 
     72                case UBX_ID_EPH: 
     73                        /* UBX_AID_EPH */ 
     74                        if (hdr->length == 0 || hdr->length == 1 || 
     75                                        hdr->length == 8 || hdr->length == 104) { 
     76                                memcpy(dst, src->payload, hdr->length); 
     77                        } else { 
     78                                printf("Malformed UBX AID_EPH packet\n"); 
     79                        } 
     80                        return; 
     81                        break; 
     82                default: 
     83                        printf("Unknown UBX AID Id 0x%x\n", hdr->id); 
     84        } 
     85} 
     86 
     87void ubx_msg_payload_encode(uint8_t *dst, ubx_msg *src) 
     88{ 
     89        ubx_hdr *hdr = src->header; 
     90        switch (hdr->cls) { 
     91                case UBX_CL_AID: 
     92                        ubx_aid_encode(dst, src); 
     93                        break; 
     94                case UBX_CL_CFG: 
     95//                      ubx_cfg_encode(dst, src); 
     96                default: 
     97                        printf("Unknown UBX class 0x%x\n", hdr->cls); 
     98        } 
     99} 
     100 
     101 
     102uint16_t ubx_msg_encode(ubx_msg *msg, uint8_t **data) 
     103{ 
     104        ubx_hdr *header = msg->header; 
    55105        uint8_t *temp; 
     106 
    56107        *data = malloc(header->length + 8); 
    57108        if (!*data) 
    … …  
    64115        // Copy the header 
    65116        // XXX: Handle Endianess 
    66         memcpy(temp, header, 4); 
     117        memcpy(temp, msg->header, 4); 
    67118        temp += 4; 
    68119        // And the message 
    69         memcpy(temp, msg, header->length); 
     120        ubx_msg_payload_encode(temp, msg); 
    70121        temp += header->length; 
    71122        *temp++ = header->ck_a; 
    72123        *temp++ = header->ck_b; 
    73124        return header->length + 8; 
     125} 
     126 
     127uint8_t *ubx_aid_decode(uint8_t id, uint16_t length, uint8_t *data) 
     128{ 
     129        uint8_t *value; 
     130 
     131        switch (id) { 
     132                case UBX_ID_ALM: 
     133                        /* AID_ALM */ 
     134                        if (length == 0 || length == 1 || 
     135                                        length == 8 || length == 40) { 
     136                                value = malloc(sizeof(ubx_msg_aid_alm)); 
     137                                memcpy(value, data, length); 
     138                        } 
     139                        break; 
     140                case UBX_ID_EPH: 
     141                        /* AID_EPH*/ 
     142                        if (length == 0 || length == 1 || 
     143                                        length == 8 || length == 104) { 
     144                                value = malloc(sizeof(ubx_msg_aid_eph)); 
     145                                memcpy(value, data, length); 
     146                        } 
     147                        break; 
     148                default: 
     149                        printf("Unknown AID ID 0x%x\n", id); 
     150                        return NULL; 
     151        } 
     152        return value; 
     153} 
     154 
     155uint8_t *ubx_msg_payload_decode(uint8_t cls, uint8_t id, uint16_t length, uint8_t *data) 
     156{ 
     157        switch (cls) { 
     158                case UBX_CL_AID: 
     159                        return ubx_aid_decode(id, length, data); 
     160                        break; 
     161                default: 
     162                        printf("Unknown class 0x%x\n", cls); 
     163        } 
     164        return 0; 
    74165} 
    75166 
    … …  
    90181        data += 2; 
    91182 
    92         if (length < header->length + 8) 
    93                 return 0; 
     183        if (length < header->length + 8) { 
     184                free(header); 
     185                return 0; 
     186        } 
    94187 
    95188        header->ck_a = data[header->length]; 
    … …  
    97190 
    98191        if (!checksum(header, data)) { 
    99                 free (header); 
     192                free(header); 
    100193                return 0; 
    101194        } 
    … …  
    105198        msg->header = header; 
    106199 
    107         msg->payload = ubx_parse_msg(header->cls, header->id, header->length, data); 
     200        msg->payload = ubx_msg_payload_decode(header->cls, header->id, header->length, data); 
    108201 
    109202        return msg; 

  • developers/alphaone/u-blox/libubx/src/ubx-get.c

    r4456 r4457  
    2020 */ 
    2121 
     22#include <sys/types.h> 
     23#include <sys/stat.h> 
     24#include <errno.h> 
    2225#include <getopt.h> 
    2326#include <stdlib.h> 
    2427#include <stdio.h> 
     28#include <string.h> 
     29#include <termios.h> 
     30#include <unistd.h> 
     31#include <fcntl.h> 
    2532#include <libubx.h> 
    2633 
    2734 
    2835/* Flag set by `--verbose'. */ 
    29 static int verbose_flag; 
     36static int verbose; 
    3037static int get_alm, get_eph; 
    31 static char *gpsname, *filename; 
     38static char *gpsname, *filename = NULL; 
     39 
     40static int gpsopen(const char *path, speed_t speed); 
     41 
     42void usage () 
     43{ 
     44        printf("Usage:\n"); 
     45        printf("\t--verbose Display more information about what's going on\n"); 
     46        printf("\t--gps <name> Specify GPS device path\n"); 
     47        printf("\t--file <name> Specify file name to output response\n"); 
     48        printf("\t--almanac Request almanac from GPS\n"); 
     49        printf("\t--ephemeris Request ephemeris from GPS\n"); 
     50        printf("\t--help Show this help\n"); 
     51        exit(1); 
     52} 
     53 
     54void request_almanac(int fd) 
     55{ 
     56        uint8_t *data, len, i; 
     57        ubx_msg msg; 
     58        msg.header = malloc(sizeof(ubx_hdr)); 
     59        msg.header->cls = UBX_CL_AID; 
     60        msg.header->id = UBX_ID_ALM; 
     61        msg.header->length = 0; 
     62        msg.payload = NULL; 
     63 
     64        len = ubx_msg_encode(&msg, &data); 
     65 
     66        if (verbose) { 
     67                printf("Writing almanac request: "); 
     68                for (i=0;i<len; i++) { 
     69                        printf("0x%x ", data[i]); 
     70                } 
     71                printf("\n"); 
     72        } 
     73 
     74        write(fd, data, len); 
     75} 
    3276 
    3377int main (int argc, char *argv[]) 
    3478{ 
    35   int c; 
     79        int c, gpsfd; 
    3680 
    37   while (1) 
    38     { 
    39       static struct option long_options[] = 
    40         { 
    41           {"verbose", no_argument,       0, 'v'}, 
    42                                         {"gps",                 required_argument, 0, 'g'}, 
    43           {"file",    required_argument, 0, 'f'}, 
    44           {"almanac", no_argument,                       0, 'a'}, 
    45           {"ephemeris", no_argument,             0, 'e'}, 
    46           {0, 0, 0, 0} 
    47         }; 
    48       /* getopt_long stores the option index here. */ 
    49       int option_index = 0; 
     81        while (1) 
     82        { 
     83                static struct option long_options[] = 
     84                { 
     85                        {"verbose", no_argument,       0, 'v'}, 
     86                        {"gps",                 required_argument, 0, 'g'}, 
     87                        {"file",    required_argument, 0, 'f'}, 
     88                        {"almanac", no_argument,                         0, 'a'}, 
     89                        {"ephemeris", no_argument,               0, 'e'}, 
     90                        {"help",                no_argument,                     0, 'h'}, 
     91                        {0, 0, 0, 0} 
     92                }; 
    5093 
    51       c = getopt_long (argc, argv, "vg:f:ae", 
    52                        long_options, &option_index); 
     94                /* getopt_long stores the option index here. */ 
     95                int option_index = 0; 
    5396 
    54       /* Detect the end of the options. */ 
    55       if (c == -1) 
    56         break; 
     97                c = getopt_long (argc, argv, "vg:f:aeh", 
     98                                long_options, &option_index); 
    5799 
    58       switch (c) 
    59         { 
    60         case 0: 
    61           /* If this option set a flag, do nothing else now. */ 
    62           if (long_options[option_index].flag != 0) 
    63             break; 
    64           printf ("option %s", long_options[option_index].name); 
    65           if (optarg) 
    66             printf (" with arg %s", optarg); 
    67           printf ("\n"); 
    68           break; 
     100                /* Detect the end of the options. */ 
     101                if (c == -1) 
     102                        break; 
    69103 
    70         case 'v': 
    71           verbose_flag = 1; 
    72           break; 
     104                switch (c) 
     105                { 
     106                        case 'v': 
     107                                verbose = 1; 
     108                                break; 
    73109 
    74         case 'g': 
    75           gpsname = optarg; 
    76           break; 
     110                        case 'g': 
     111                                gpsname = optarg; 
     112                                break; 
    77113 
    78         case 'f': 
    79           filename = optarg; 
    80           break; 
     114                        case 'f': 
     115                                filename = optarg; 
     116                                break; 
    81117 
    82         case 'a': 
    83                                         get_alm = 1; 
    84           break; 
     118                        case 'a': 
     119                                get_alm = 1; 
     120                                break; 
    85121 
    86         case 'e': 
    87                                         get_eph = 1; 
    88           break; 
     122                        case 'e': 
     123                                get_eph = 1; 
     124                                break; 
    89125 
    90         case '?': 
    91           /* getopt_long already printed an error message. */ 
    92           break; 
     126                        case 'h': 
     127                                usage(); 
     128                                break; 
    93129 
    94         default: 
    95           abort (); 
    96         } 
    97     } 
     130                        default: 
     131                                usage(); 
     132                } 
     133        } 
    98134 
    99   /* Print any remaining command line arguments (not options). */ 
    100   if (optind < argc) 
    101     { 
    102       printf ("non-option ARGV-elements: "); 
    103       while (optind < argc) 
    104         printf ("%s ", argv[optind++]); 
    105       putchar ('\n'); 
    106     } 
     135        if (optind < argc) { 
     136                usage(); 
     137        } 
    107138 
    108   exit (0); 
     139        if (gpsname == NULL) { 
     140                printf("Please specify a GPS device\n"); 
     141                usage(); 
     142        } 
     143 
     144        gpsfd = gpsopen(gpsname, 9600); 
     145 
     146        if (gpsfd < 0) { 
     147                printf("GPS device could not be opened at %s\n", gpsname); 
     148                printf("Reason: %s\n", strerror(errno)); 
     149                exit(1); 
     150        } 
     151        gpsfd=1; 
     152 
     153        if (get_alm) { 
     154                request_almanac(gpsfd); 
     155        } 
     156 
    109157} 
    110158 
     159static int gpsopen(const char *path, speed_t speed) 
     160{ 
     161        int fd; 
     162        struct termios term; 
     163 
     164        fd = open(path, O_RDWR | O_NOCTTY | O_NONBLOCK); 
     165        if (fd < 0) 
     166                return fd; 
     167 
     168        if (speed == B0) 
     169                return fd; 
     170 
     171        if (tcgetattr(fd, &term) < 0) { 
     172//              close(fd); 
     173                printf("tcgetattr failed!\n"); 
     174                return fd; 
     175        } 
     176 
     177        term.c_cflag &= ~(PARENB | PARODD | CRTSCTS); 
     178        term.c_cflag |= CREAD | CLOCAL; 
     179 
     180        term.c_iflag &= ~(PARMRK | INPCK); 
     181        term.c_cflag &= ~(CSIZE | CSTOPB | PARENB | PARODD); 
     182        term.c_cflag |= CS8; 
     183 
     184        term.c_lflag &= ~ICANON; 
     185        term.c_lflag &= ~ECHO; 
     186 
     187        cfsetspeed(&term, speed); 
     188 
     189        if (tcsetattr(fd, TCSANOW, &term) < 0) { 
     190                printf("tcsetattr failed\n"); 
     191                close(fd); 
     192 
     193                return -1; 
     194        } 
     195 
     196  return fd; 
     197} 
Note: See TracChangeset for help on using the changeset viewer.