git.gag.com Git - fw/altos/blob - src/ao_gps.c

   1 /*
   2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #ifndef AO_GPS_TEST
  19 #include "ao.h"
  20 #endif
  21
  22 __xdata uint8_t ao_gps_mutex;
  23 __xdata struct ao_gps_data      ao_gps_data;
  24
  25 const char ao_gps_set_binary[] = {
  26         '$', 'P', 'S', 'R', 'F', '1', '0', '0', ',', '0', ',',
  27         '9', '6', '0', '0', ',', '8', ',', '1', ',', '0', '*',
  28         '0', 'C', '\r','\n',
  29
  30         0xa0, 0xa2, 0x00, 0x09, /* length 9 bytes */
  31         134,                    /* Set binary serial port */
  32         0, 0, 0x25, 0x80,       /* 9600 baud */
  33         8,                      /* data bits */
  34         1,                      /* stop bits */
  35         0,                      /* parity */
  36         0,                      /* pad */
  37         0x01, 0x34, 0xb0, 0xb3,
  38 };
  39
  40 const char ao_gps_config[] = {
  41         0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */
  42         136,                    /* mode control */
  43         0, 0,                   /* reserved */
  44         4,                      /* degraded mode (disabled) */
  45         0, 0,                   /* reserved */
  46         0, 0,                   /* user specified altitude */
  47         2,                      /* alt hold mode (disabled, require 3d fixes) */
  48         0,                      /* alt hold source (use last computed altitude) */
  49         0,                      /* reserved */
  50         0,                      /* Degraded time out (disabled) */
  51         0,                      /* Dead Reckoning time out (disabled) */
  52         0,                      /* Track smoothing (disabled) */
  53         0x00, 0x8e, 0xb0, 0xb3,
  54
  55         0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */
  56         143,                    /* static navigation */
  57         0,                      /* disable */
  58         0x00, 0x8f, 0xb0, 0xb3,
  59
  60         0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */
  61         166,                    /* Set message rate */
  62         2,                      /* enable/disable all messages */
  63         0,                      /* message id (ignored) */
  64         0,                      /* update rate (0 = disable) */
  65         0, 0, 0, 0,             /* reserved */
  66         0x00, 0xa8, 0xb0, 0xb3,
  67
  68         0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */
  69         166,                    /* Set message rate */
  70         0,                      /* enable/disable one message */
  71         41,                     /* message 41 */
  72         1,                      /* once per second */
  73         0, 0, 0, 0,             /* reserved */
  74         0x00, 0xd0, 0xb0, 0xb3,
  75 };
  76
  77 #define NAV_TYPE_GPS_FIX_TYPE_MASK                      (7 << 0)
  78 #define NAV_TYPE_NO_FIX                                 (0 << 0)
  79 #define NAV_TYPE_SV_KF                                  (1 << 0)
  80 #define NAV_TYPE_2_SV_KF                                (2 << 0)
  81 #define NAV_TYPE_3_SV_KF                                (3 << 0)
  82 #define NAV_TYPE_4_SV_KF                                (4 << 0)
  83 #define NAV_TYPE_2D_LEAST_SQUARES                       (5 << 0)
  84 #define NAV_TYPE_3D_LEAST_SQUARES                       (6 << 0)
  85 #define NAV_TYPE_DR                                     (7 << 0)
  86 #define NAV_TYPE_TRICKLE_POWER                          (1 << 3)
  87 #define NAV_TYPE_ALTITUDE_HOLD_MASK                     (3 << 4)
  88 #define NAV_TYPE_ALTITUDE_HOLD_NONE                     (0 << 4)
  89 #define NAV_TYPE_ALTITUDE_HOLD_KF                       (1 << 4)
  90 #define NAV_TYPE_ALTITUDE_HOLD_USER                     (2 << 4)
  91 #define NAV_TYPE_ALTITUDE_HOLD_ALWAYS                   (3 << 4)
  92 #define NAV_TYPE_DOP_LIMIT_EXCEEDED                     (1 << 6)
  93 #define NAV_TYPE_DGPS_APPLIED                           (1 << 7)
  94 #define NAV_TYPE_SENSOR_DR                              (1 << 8)
  95 #define NAV_TYPE_OVERDETERMINED                         (1 << 9)
  96 #define NAV_TYPE_DR_TIMEOUT_EXCEEDED                    (1 << 10)
  97 #define NAV_TYPE_FIX_MI_EDIT                            (1 << 11)
  98 #define NAV_TYPE_INVALID_VELOCITY                       (1 << 12)
  99 #define NAV_TYPE_ALTITUDE_HOLD_DISABLED                 (1 << 13)
 100 #define NAV_TYPE_DR_ERROR_STATUS_MASK                   (3 << 14)
 101 #define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY               (0 << 14)
 102 #define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS            (1 << 14)
 103 #define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR        (2 << 14)
 104 #define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST             (3 << 14)
 105
 106 struct sirf_geodetic_nav_data {
 107         uint16_t        nav_type;
 108         uint16_t        utc_year;
 109         uint8_t         utc_month;
 110         uint8_t         utc_day;
 111         uint8_t         utc_hour;
 112         uint8_t         utc_minute;
 113         uint16_t        utc_second;
 114         int32_t         lat;
 115         int32_t         lon;
 116         int32_t         alt_msl;
 117         uint16_t        ground_speed;
 118         uint16_t        course;
 119         int16_t         climb_rate;
 120         uint32_t        h_error;
 121         uint32_t        v_error;
 122         uint8_t         num_sv;
 123         uint8_t         hdop;
 124 };
 125
 126 static __xdata struct sirf_geodetic_nav_data    ao_sirf_data;
 127
 128 static __pdata uint16_t ao_sirf_cksum;
 129 static __pdata uint16_t ao_sirf_len;
 130
 131 #define ao_sirf_byte()  ((uint8_t) ao_serial_getchar())
 132
 133 static uint8_t data_byte(void)
 134 {
 135         uint8_t c = ao_sirf_byte();
 136         --ao_sirf_len;
 137         ao_sirf_cksum += c;
 138         return c;
 139 }
 140
 141 static void sirf_u16(uint8_t offset)
 142 {
 143         uint16_t __xdata *ptr = (uint16_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset);
 144         uint16_t val;
 145
 146         val = data_byte() << 8;
 147         val |= data_byte ();
 148         *ptr = val;
 149 }
 150
 151 static void sirf_u8(uint8_t offset)
 152 {
 153         uint8_t __xdata *ptr = (uint8_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset);
 154         uint8_t val;
 155
 156         val = data_byte ();
 157         *ptr = val;
 158 }
 159
 160 static void sirf_u32(uint8_t offset)
 161 {
 162         uint32_t __xdata *ptr = (uint32_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset);
 163         uint32_t val;
 164
 165         val = ((uint32_t) data_byte ()) << 24;
 166         val |= ((uint32_t) data_byte ()) << 16;
 167         val |= ((uint32_t) data_byte ()) << 8;
 168         val |= ((uint32_t) data_byte ());
 169         *ptr = val;
 170 }
 171
 172 static void sirf_discard(uint8_t len)
 173 {
 174         while (len--)
 175                 data_byte();
 176 }
 177
 178 #define SIRF_END        0
 179 #define SIRF_DISCARD    1
 180 #define SIRF_U8         2
 181 #define SIRF_U16        3
 182 #define SIRF_U32        4
 183
 184 struct sirf_packet_parse {
 185         uint8_t type;
 186         uint8_t offset;
 187 };
 188
 189 static const struct sirf_packet_parse geodetic_nav_data_packet[] = {
 190         { SIRF_DISCARD, 2 },                                                    /* 1 nav valid */
 191         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) },        /* 3 */
 192         { SIRF_DISCARD, 6 },                                                    /* 5 week number, time of week */
 193         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) },        /* 11 */
 194         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) },        /* 13 */
 195         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) },          /* 14 */
 196         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) },         /* 15 */
 197         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) },       /* 16 */
 198         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) },      /* 17 */
 199         { SIRF_DISCARD, 4 },    /* satellite id list */                         /* 19 */
 200         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) },             /* 23 */
 201         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) },             /* 27 */
 202         { SIRF_DISCARD, 4 },    /* altitude from ellipsoid */                   /* 31 */
 203         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) },         /* 35 */
 204         { SIRF_DISCARD, 1 },    /* map datum */                                 /* 39 */
 205         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) },    /* 40 */
 206         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) },          /* 42 */
 207         { SIRF_DISCARD, 2 },    /* magnetic variation */                        /* 44 */
 208         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) },      /* 46 */
 209         { SIRF_DISCARD, 2 },    /* turn rate */                                 /* 48 */
 210         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) },         /* 50 */
 211         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) },         /* 54 */
 212         { SIRF_DISCARD, 30 },   /* time error, h_vel error, clock_bias,
 213                                    clock bias error, clock drift,
 214                                    clock drift error, distance,
 215                                    distance error, heading error */             /* 58 */
 216         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) },           /* 88 */
 217         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) },             /* 89 */
 218         { SIRF_DISCARD, 1 },    /* additional mode info */                      /* 90 */
 219         { SIRF_END, 0 },                                                        /* 91 */
 220 };
 221
 222 static void
 223 ao_sirf_parse_41(void)
 224 {
 225         uint8_t i, offset;
 226
 227         printf("parse 41\n");
 228         for (i = 0; ; i++) {
 229                 offset = geodetic_nav_data_packet[i].offset;
 230                 switch (geodetic_nav_data_packet[i].type) {
 231                 case SIRF_END:
 232                         printf("parse 41 done\n");
 233                         return;
 234                 case SIRF_DISCARD:
 235                         printf("parse 41 discard %d\n", offset);
 236                         sirf_discard(offset);
 237                         break;
 238                 case SIRF_U8:
 239                         printf("parse 41 u8 %d\n", offset);
 240                         sirf_u8(offset);
 241                         break;
 242                 case SIRF_U16:
 243                         printf("parse 41 u16 %d\n", offset);
 244                         sirf_u16(offset);
 245                         break;
 246                 case SIRF_U32:
 247                         printf("parse 41 u32 %d\n", offset);
 248                         sirf_u32(offset);
 249                         break;
 250                 }
 251         }
 252 }
 253
 254 void
 255 ao_gps_setup(void) __reentrant
 256 {
 257         uint8_t i, j;
 258         for (j = 0; j < 2; j++) {
 259 #ifdef AO_GPS_TEST
 260                 ao_serial_set_speed(j);
 261 #endif
 262                 for (i = 0; i < sizeof (ao_gps_set_binary); i++)
 263                         ao_serial_putchar(ao_gps_set_binary[i]);
 264         }
 265 }
 266
 267 void
 268 ao_gps(void) __reentrant
 269 {
 270         uint8_t i;
 271         uint16_t cksum;
 272
 273         for (i = 0; i < sizeof (ao_gps_config); i++)
 274                 ao_serial_putchar(ao_gps_config[i]);
 275         for (;;) {
 276                 /* Locate the begining of the next record */
 277                 while (ao_sirf_byte() != 0xa0)
 278                         ;
 279                 if (ao_sirf_byte() != 0xa2)
 280                         continue;
 281
 282                 /* Length */
 283                 ao_sirf_len = ao_sirf_byte() << 8;
 284                 ao_sirf_len |= ao_sirf_byte();
 285                 if (ao_sirf_len > 1023)
 286                         continue;
 287
 288                 ao_sirf_cksum = 0;
 289
 290                 /* message ID */
 291                 i = data_byte ();                                                       /* 0 */
 292                 printf ("message %d len %d\n", i, ao_sirf_len);
 293
 294                 switch (i) {
 295                 case 41:
 296                         if (ao_sirf_len < 90)
 297                                 break;
 298                         ao_sirf_parse_41();
 299                         break;
 300                 }
 301                 if (ao_sirf_len != 0)
 302                         continue;
 303
 304                 /* verify checksum and end sequence */
 305                 ao_sirf_cksum &= 0x7fff;
 306                 cksum = ao_sirf_byte() << 8;
 307                 cksum |= ao_sirf_byte();
 308                 if (ao_sirf_cksum != cksum)
 309                         continue;
 310                 if (ao_sirf_byte() != 0xb0)
 311                         continue;
 312                 if (ao_sirf_byte() != 0xb3)
 313                         continue;
 314
 315                 switch (i) {
 316                 case 41:
 317                         ao_mutex_get(&ao_gps_mutex);
 318                         ao_gps_data.hour = ao_sirf_data.utc_hour;
 319                         ao_gps_data.minute = ao_sirf_data.utc_minute;
 320                         ao_gps_data.second = ao_sirf_data.utc_second / 1000;
 321                         ao_gps_data.flags = (ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK;
 322                         if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF)
 323                                 ao_gps_data.flags |= AO_GPS_VALID;
 324                         ao_gps_data.latitude = ao_sirf_data.lat;
 325                         ao_gps_data.longitude = ao_sirf_data.lon;
 326                         ao_gps_data.altitude = ao_sirf_data.alt_msl / 100;
 327                         ao_gps_data.ground_speed = ao_sirf_data.ground_speed;
 328                         ao_gps_data.course = ao_sirf_data.course / 200;
 329                         ao_gps_data.hdop = ao_sirf_data.hdop;
 330                         ao_gps_data.climb_rate = ao_sirf_data.climb_rate;
 331                         if (ao_sirf_data.h_error > 6553500)
 332                                 ao_gps_data.h_error = 65535;
 333                         else
 334                                 ao_gps_data.h_error = ao_sirf_data.h_error / 100;
 335                         if (ao_sirf_data.v_error > 6553500)
 336                                 ao_gps_data.v_error = 65535;
 337                         else
 338                                 ao_gps_data.v_error = ao_sirf_data.v_error / 100;
 339                         ao_gps_data.h_error = ao_sirf_data.h_error;
 340                         ao_mutex_put(&ao_gps_mutex);
 341                         ao_wakeup(&ao_gps_data);
 342                         break;
 343                 }
 344         }
 345 }
 346
 347 __xdata struct ao_task ao_gps_task;
 348
 349 static void
 350 gps_dump(void) __reentrant
 351 {
 352         ao_mutex_get(&ao_gps_mutex);
 353         ao_gps_print(&ao_gps_data);
 354         ao_mutex_put(&ao_gps_mutex);
 355 }
 356
 357 __code struct ao_cmds ao_gps_cmds[] = {
 358         { 'g', gps_dump,        "g                                  Display current GPS values" },
 359         { 0, gps_dump, NULL },
 360 };
 361
 362 void
 363 ao_gps_init(void)
 364 {
 365         ao_add_task(&ao_gps_task, ao_gps, "gps");
 366         ao_cmd_register(&ao_gps_cmds[0]);
 367 }