git.gag.com Git - fw/altos/blob - src/drivers/ao_gps_sirf.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_new;
  23 __xdata uint8_t ao_gps_mutex;
  24 __pdata uint16_t ao_gps_tick;
  25 __xdata struct ao_telemetry_location    ao_gps_data;
  26 __xdata struct ao_telemetry_satellite   ao_gps_tracking_data;
  27
  28 static const char ao_gps_set_nmea[] = "\r\n$PSRF100,0,57600,8,1,0*37\r\n";
  29
  30 const char ao_gps_config[] = {
  31
  32         0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */
  33         136,                    /* mode control */
  34         0, 0,                   /* reserved */
  35         0,                      /* degraded mode (allow 1-SV navigation) */
  36         0, 0,                   /* reserved */
  37         0, 0,                   /* user specified altitude */
  38         2,                      /* alt hold mode (disabled, require 3d fixes) */
  39         0,                      /* alt hold source (use last computed altitude) */
  40         0,                      /* reserved */
  41         10,                     /* Degraded time out (10 sec) */
  42         10,                     /* Dead Reckoning time out (10 sec) */
  43         0,                      /* Track smoothing (disabled) */
  44         0x00, 0x8e, 0xb0, 0xb3,
  45
  46         0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */
  47         166,                    /* Set message rate */
  48         2,                      /* enable/disable all messages */
  49         0,                      /* message id (ignored) */
  50         0,                      /* update rate (0 = disable) */
  51         0, 0, 0, 0,             /* reserved */
  52         0x00, 0xa8, 0xb0, 0xb3,
  53
  54         0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */
  55         143,                    /* static navigation */
  56         0,                      /* disable */
  57         0x00, 0x8f, 0xb0, 0xb3,
  58 };
  59
  60 #define NAV_TYPE_GPS_FIX_TYPE_MASK                      (7 << 0)
  61 #define NAV_TYPE_NO_FIX                                 (0 << 0)
  62 #define NAV_TYPE_SV_KF                                  (1 << 0)
  63 #define NAV_TYPE_2_SV_KF                                (2 << 0)
  64 #define NAV_TYPE_3_SV_KF                                (3 << 0)
  65 #define NAV_TYPE_4_SV_KF                                (4 << 0)
  66 #define NAV_TYPE_2D_LEAST_SQUARES                       (5 << 0)
  67 #define NAV_TYPE_3D_LEAST_SQUARES                       (6 << 0)
  68 #define NAV_TYPE_DR                                     (7 << 0)
  69 #define NAV_TYPE_TRICKLE_POWER                          (1 << 3)
  70 #define NAV_TYPE_ALTITUDE_HOLD_MASK                     (3 << 4)
  71 #define NAV_TYPE_ALTITUDE_HOLD_NONE                     (0 << 4)
  72 #define NAV_TYPE_ALTITUDE_HOLD_KF                       (1 << 4)
  73 #define NAV_TYPE_ALTITUDE_HOLD_USER                     (2 << 4)
  74 #define NAV_TYPE_ALTITUDE_HOLD_ALWAYS                   (3 << 4)
  75 #define NAV_TYPE_DOP_LIMIT_EXCEEDED                     (1 << 6)
  76 #define NAV_TYPE_DGPS_APPLIED                           (1 << 7)
  77 #define NAV_TYPE_SENSOR_DR                              (1 << 8)
  78 #define NAV_TYPE_OVERDETERMINED                         (1 << 9)
  79 #define NAV_TYPE_DR_TIMEOUT_EXCEEDED                    (1 << 10)
  80 #define NAV_TYPE_FIX_MI_EDIT                            (1 << 11)
  81 #define NAV_TYPE_INVALID_VELOCITY                       (1 << 12)
  82 #define NAV_TYPE_ALTITUDE_HOLD_DISABLED                 (1 << 13)
  83 #define NAV_TYPE_DR_ERROR_STATUS_MASK                   (3 << 14)
  84 #define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY               (0 << 14)
  85 #define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS            (1 << 14)
  86 #define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR        (2 << 14)
  87 #define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST             (3 << 14)
  88
  89 struct sirf_geodetic_nav_data {
  90         uint16_t        nav_type;
  91         uint16_t        utc_year;
  92         uint8_t         utc_month;
  93         uint8_t         utc_day;
  94         uint8_t         utc_hour;
  95         uint8_t         utc_minute;
  96         uint16_t        utc_second;
  97         int32_t         lat;
  98         int32_t         lon;
  99         int32_t         alt_msl;
 100         uint16_t        ground_speed;
 101         uint16_t        course;
 102         int16_t         climb_rate;
 103         uint32_t        h_error;
 104         uint32_t        v_error;
 105         uint8_t         num_sv;
 106         uint8_t         hdop;
 107 };
 108
 109 static __xdata struct sirf_geodetic_nav_data    ao_sirf_data;
 110
 111 struct sirf_measured_sat_data {
 112         uint8_t         svid;
 113         uint8_t         c_n_1;
 114 };
 115
 116 struct sirf_measured_tracker_data {
 117         int16_t                         gps_week;
 118         uint32_t                        gps_tow;
 119         uint8_t                         channels;
 120         struct sirf_measured_sat_data   sats[12];
 121 };
 122
 123 static __xdata struct sirf_measured_tracker_data        ao_sirf_tracker_data;
 124
 125 static __pdata uint16_t ao_sirf_cksum;
 126 static __pdata uint16_t ao_sirf_len;
 127
 128 #ifndef ao_sirf_getchar
 129 #define ao_sirf_getchar         ao_serial1_getchar
 130 #define ao_sirf_putchar         ao_serial1_putchar
 131 #define ao_sirf_set_speed       ao_serial1_set_speed
 132 #endif
 133
 134 #define ao_sirf_byte()  ((uint8_t) ao_sirf_getchar())
 135
 136 static uint8_t data_byte(void)
 137 {
 138         uint8_t c = ao_sirf_byte();
 139         --ao_sirf_len;
 140         ao_sirf_cksum += c;
 141         return c;
 142 }
 143
 144 static char __xdata *sirf_target;
 145
 146 static void sirf_u16(uint8_t offset)
 147 {
 148         uint16_t __xdata *ptr = (uint16_t __xdata *) (sirf_target + offset);
 149         uint16_t val;
 150
 151         val = data_byte() << 8;
 152         val |= data_byte ();
 153         *ptr = val;
 154 }
 155
 156 static void sirf_u8(uint8_t offset)
 157 {
 158         uint8_t __xdata *ptr = (uint8_t __xdata *) (sirf_target + offset);
 159         uint8_t val;
 160
 161         val = data_byte ();
 162         *ptr = val;
 163 }
 164
 165 static void sirf_u32(uint8_t offset) __reentrant
 166 {
 167         uint32_t __xdata *ptr = (uint32_t __xdata *) (sirf_target + offset);
 168         uint32_t val;
 169
 170         val = ((uint32_t) data_byte ()) << 24;
 171         val |= ((uint32_t) data_byte ()) << 16;
 172         val |= ((uint32_t) data_byte ()) << 8;
 173         val |= ((uint32_t) data_byte ());
 174         *ptr = val;
 175 }
 176
 177 static void sirf_discard(uint8_t len)
 178 {
 179         while (len--)
 180                 data_byte();
 181 }
 182
 183 #define SIRF_END        0
 184 #define SIRF_DISCARD    1
 185 #define SIRF_U8         2
 186 #define SIRF_U16        3
 187 #define SIRF_U32        4
 188 #define SIRF_U8X10      5
 189
 190 struct sirf_packet_parse {
 191         uint8_t type;
 192         uint8_t offset;
 193 };
 194
 195 static void
 196 ao_sirf_parse(void __xdata *target, const struct sirf_packet_parse *parse) __reentrant
 197 {
 198         uint8_t i, offset, j;
 199
 200         sirf_target = target;
 201         for (i = 0; ; i++) {
 202                 offset = parse[i].offset;
 203                 switch (parse[i].type) {
 204                 case SIRF_END:
 205                         return;
 206                 case SIRF_DISCARD:
 207                         sirf_discard(offset);
 208                         break;
 209                 case SIRF_U8:
 210                         sirf_u8(offset);
 211                         break;
 212                 case SIRF_U16:
 213                         sirf_u16(offset);
 214                         break;
 215                 case SIRF_U32:
 216                         sirf_u32(offset);
 217                         break;
 218                 case SIRF_U8X10:
 219                         for (j = 10; j--;)
 220                                 sirf_u8(offset++);
 221                         break;
 222                 }
 223         }
 224 }
 225
 226 static const struct sirf_packet_parse geodetic_nav_data_packet[] = {
 227         { SIRF_DISCARD, 2 },                                                    /* 1 nav valid */
 228         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) },        /* 3 */
 229         { SIRF_DISCARD, 6 },                                                    /* 5 week number, time of week */
 230         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) },        /* 11 */
 231         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) },        /* 13 */
 232         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) },          /* 14 */
 233         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) },         /* 15 */
 234         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) },       /* 16 */
 235         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) },      /* 17 */
 236         { SIRF_DISCARD, 4 },    /* satellite id list */                         /* 19 */
 237         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) },             /* 23 */
 238         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) },             /* 27 */
 239         { SIRF_DISCARD, 4 },    /* altitude from ellipsoid */                   /* 31 */
 240         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) },         /* 35 */
 241         { SIRF_DISCARD, 1 },    /* map datum */                                 /* 39 */
 242         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) },    /* 40 */
 243         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) },          /* 42 */
 244         { SIRF_DISCARD, 2 },    /* magnetic variation */                        /* 44 */
 245         { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) },      /* 46 */
 246         { SIRF_DISCARD, 2 },    /* turn rate */                                 /* 48 */
 247         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) },         /* 50 */
 248         { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) },         /* 54 */
 249         { SIRF_DISCARD, 30 },   /* time error, h_vel error, clock_bias,
 250                                    clock bias error, clock drift,
 251                                    clock drift error, distance,
 252                                    distance error, heading error */             /* 58 */
 253         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) },           /* 88 */
 254         { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) },             /* 89 */
 255         { SIRF_DISCARD, 1 },    /* additional mode info */                      /* 90 */
 256         { SIRF_END, 0 },                                                        /* 91 */
 257 };
 258
 259 static void
 260 ao_sirf_parse_41(void) __reentrant
 261 {
 262         ao_sirf_parse(&ao_sirf_data, geodetic_nav_data_packet);
 263 }
 264
 265 static const struct sirf_packet_parse measured_tracker_data_packet[] = {
 266         { SIRF_U16, offsetof (struct sirf_measured_tracker_data, gps_week) },   /* 1 week */
 267         { SIRF_U32, offsetof (struct sirf_measured_tracker_data, gps_tow) },    /* 3 time of week */
 268         { SIRF_U8, offsetof (struct sirf_measured_tracker_data, channels) },    /* 7 channels */
 269         { SIRF_END, 0 },
 270 };
 271
 272 static const struct sirf_packet_parse measured_sat_data_packet[] = {
 273         { SIRF_U8, offsetof (struct sirf_measured_sat_data, svid) },            /* 0 SV id */
 274         { SIRF_DISCARD, 4 },                                                    /* 1 azimuth, 2 elevation, 3 state */
 275         { SIRF_U8, offsetof (struct sirf_measured_sat_data, c_n_1) },           /* C/N0 1 */
 276         { SIRF_DISCARD, 9 },                                                    /* C/N0 2-10 */
 277         { SIRF_END, 0 },
 278 };
 279
 280 static void
 281 ao_sirf_parse_4(void) __reentrant
 282 {
 283         uint8_t i;
 284         ao_sirf_parse(&ao_sirf_tracker_data, measured_tracker_data_packet);
 285         for (i = 0; i < 12; i++)
 286                 ao_sirf_parse(&ao_sirf_tracker_data.sats[i], measured_sat_data_packet);
 287 }
 288
 289 static void
 290 ao_gps_setup(void) __reentrant
 291 {
 292         uint8_t i, k;
 293         ao_sirf_set_speed(AO_SERIAL_SPEED_4800);
 294         for (i = 0; i < 64; i++)
 295                 ao_sirf_putchar(0x00);
 296         for (k = 0; k < 3; k++)
 297                 for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
 298                         ao_sirf_putchar(ao_gps_set_nmea[i]);
 299         ao_sirf_set_speed(AO_SERIAL_SPEED_57600);
 300         for (i = 0; i < 64; i++)
 301                 ao_sirf_putchar(0x00);
 302 }
 303
 304 static const char ao_gps_set_message_rate[] = {
 305         0xa0, 0xa2, 0x00, 0x08,
 306         166,
 307         0,
 308 };
 309
 310 void
 311 ao_sirf_set_message_rate(uint8_t msg, uint8_t rate) __reentrant
 312 {
 313         uint16_t        cksum = 0x00a6;
 314         uint8_t         i;
 315
 316         for (i = 0; i < sizeof (ao_gps_set_message_rate); i++)
 317                 ao_sirf_putchar(ao_gps_set_message_rate[i]);
 318         ao_sirf_putchar(msg);
 319         ao_sirf_putchar(rate);
 320         cksum = 0xa6 + msg + rate;
 321         for (i = 0; i < 4; i++)
 322                 ao_sirf_putchar(0);
 323         ao_sirf_putchar((cksum >> 8) & 0x7f);
 324         ao_sirf_putchar(cksum & 0xff);
 325         ao_sirf_putchar(0xb0);
 326         ao_sirf_putchar(0xb3);
 327 }
 328
 329 static const uint8_t sirf_disable[] = {
 330         2,
 331         9,
 332         10,
 333         27,
 334         50,
 335         52,
 336 };
 337
 338 void
 339 ao_gps(void) __reentrant
 340 {
 341         uint8_t i, k;
 342         uint16_t cksum;
 343
 344         ao_gps_setup();
 345         for (k = 0; k < 5; k++)
 346         {
 347                 for (i = 0; i < sizeof (ao_gps_config); i++)
 348                         ao_sirf_putchar(ao_gps_config[i]);
 349                 for (i = 0; i < sizeof (sirf_disable); i++)
 350                         ao_sirf_set_message_rate(sirf_disable[i], 0);
 351                 ao_sirf_set_message_rate(41, 1);
 352                 ao_sirf_set_message_rate(4, 1);
 353         }
 354         for (;;) {
 355                 /* Locate the begining of the next record */
 356                 while (ao_sirf_byte() != (uint8_t) 0xa0)
 357                         ;
 358                 if (ao_sirf_byte() != (uint8_t) 0xa2)
 359                         continue;
 360
 361                 /* Length */
 362                 ao_sirf_len = ao_sirf_byte() << 8;
 363                 ao_sirf_len |= ao_sirf_byte();
 364                 if (ao_sirf_len > 1023)
 365                         continue;
 366
 367                 ao_sirf_cksum = 0;
 368
 369                 /* message ID */
 370                 i = data_byte ();                                                       /* 0 */
 371
 372                 switch (i) {
 373                 case 41:
 374                         if (ao_sirf_len < 90)
 375                                 break;
 376                         ao_sirf_parse_41();
 377                         break;
 378                 case 4:
 379                         if (ao_sirf_len < 187)
 380                                 break;
 381                         ao_sirf_parse_4();
 382                         break;
 383                 }
 384                 if (ao_sirf_len != 0)
 385                         continue;
 386
 387                 /* verify checksum and end sequence */
 388                 ao_sirf_cksum &= 0x7fff;
 389                 cksum = ao_sirf_byte() << 8;
 390                 cksum |= ao_sirf_byte();
 391                 if (ao_sirf_cksum != cksum)
 392                         continue;
 393                 if (ao_sirf_byte() != (uint8_t) 0xb0)
 394                         continue;
 395                 if (ao_sirf_byte() != (uint8_t) 0xb3)
 396                         continue;
 397
 398                 switch (i) {
 399                 case 41:
 400                         ao_mutex_get(&ao_gps_mutex);
 401                         ao_gps_tick = ao_time();
 402                         ao_gps_data.hour = ao_sirf_data.utc_hour;
 403                         ao_gps_data.minute = ao_sirf_data.utc_minute;
 404                         ao_gps_data.second = ao_sirf_data.utc_second / 1000;
 405                         ao_gps_data.flags = ((ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK) | AO_GPS_RUNNING;
 406                         if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF)
 407                                 ao_gps_data.flags |= AO_GPS_VALID;
 408                         ao_gps_data.latitude = ao_sirf_data.lat;
 409                         ao_gps_data.longitude = ao_sirf_data.lon;
 410                         ao_gps_data.altitude = ao_sirf_data.alt_msl / 100;
 411                         ao_gps_data.ground_speed = ao_sirf_data.ground_speed;
 412                         ao_gps_data.course = ao_sirf_data.course / 200;
 413                         ao_gps_data.hdop = ao_sirf_data.hdop;
 414                         ao_gps_data.climb_rate = ao_sirf_data.climb_rate;
 415                         ao_gps_data.flags |= AO_GPS_COURSE_VALID;
 416 #if 0
 417                         if (ao_sirf_data.h_error > 6553500)
 418                                 ao_gps_data.h_error = 65535;
 419                         else
 420                                 ao_gps_data.h_error = ao_sirf_data.h_error / 100;
 421                         if (ao_sirf_data.v_error > 6553500)
 422                                 ao_gps_data.v_error = 65535;
 423                         else
 424                                 ao_gps_data.v_error = ao_sirf_data.v_error / 100;
 425 #endif
 426                         ao_gps_new |= AO_GPS_NEW_DATA;
 427                         ao_mutex_put(&ao_gps_mutex);
 428                         ao_wakeup(&ao_gps_new);
 429                         break;
 430                 case 4:
 431                         ao_mutex_get(&ao_gps_mutex);
 432                         ao_gps_tracking_data.channels = ao_sirf_tracker_data.channels;
 433                         for (i = 0; i < 12; i++) {
 434                                 ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid;
 435                                 ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1;
 436                         }
 437                         ao_gps_new |= AO_GPS_NEW_TRACKING;
 438                         ao_mutex_put(&ao_gps_mutex);
 439                         ao_wakeup(&ao_gps_new);
 440                         break;
 441                 }
 442         }
 443 }
 444
 445 __xdata struct ao_task ao_gps_task;
 446
 447 void
 448 ao_gps_init(void)
 449 {
 450         ao_add_task(&ao_gps_task, ao_gps, "gps");
 451 }