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