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

   1 /*
   2  * Copyright © 2024 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, either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #ifndef AO_GPS_TEST
  20 #include "ao.h"
  21 #endif
  22
  23 #if HAS_GPS_MOSAIC
  24
  25 #include "ao_gps_mosaic.h"
  26 #include "ao_crc_ccitt.h"
  27 #include <stdarg.h>
  28 #include <stdio.h>
  29 #include <math.h>
  30
  31 #define AO_MOSAIC_DEBUG 0
  32
  33 #if AO_MOSAIC_DEBUG
  34
  35 #define DBG_PROTO       1
  36 #define DBG_CHAR        2
  37 #define DBG_INIT        4
  38
  39 static uint8_t mosaic_dbg_enable = 0;
  40
  41 static void mosaic_dbg(int level, char *format, ...) {
  42         va_list a;
  43
  44         if (level & mosaic_dbg_enable) {
  45                 va_start(a, format);
  46                 vprintf(format, a);
  47                 va_end(a);
  48                 flush();
  49         }
  50 }
  51
  52 #else
  53 #define mosaic_dbg(fmt, ...)
  54 #endif
  55
  56 static inline uint8_t mosaic_byte(void) {
  57         uint8_t c = (uint8_t) ao_mosaic_getchar();
  58 #if AO_MOSAIC_DEBUG
  59         if (' ' <= c && c <= '~')
  60                 mosaic_dbg(DBG_CHAR, "%c", c);
  61         else
  62                 mosaic_dbg(DBG_CHAR, " %02x", c);
  63 #endif
  64         return c;
  65 }
  66
  67 static inline void mosaic_putc(char c) {
  68 #if AO_MOSAIC_DEBUG
  69         if (' ' <= c && c <= '~')
  70                 mosaic_dbg(DBG_CHAR, "%c", c);
  71         else
  72                 mosaic_dbg(DBG_CHAR, " (%02x)", c);
  73 #endif
  74         ao_mosaic_putchar(c);
  75 }
  76
  77 static void mosaic_puts(const char *s) {
  78         char c;
  79         while ((c = *s++) != '\0')
  80                 mosaic_putc(c);
  81 }
  82
  83 extern uint8_t ao_gps_new;
  84 extern uint8_t ao_gps_mutex;
  85 extern AO_TICK_TYPE ao_gps_tick;
  86 extern AO_TICK_TYPE ao_gps_utc_tick;
  87 extern struct ao_telemetry_location     ao_gps_data;
  88 extern struct ao_telemetry_satellite    ao_gps_tracking_data;
  89
  90 static void
  91 mosaic_wait_idle(void)
  92 {
  93         uint8_t b;
  94         for (;;) {
  95                 while(mosaic_byte() != '$')
  96                         ;
  97                 if (mosaic_byte() != 'R')
  98                         continue;
  99                 for (;;) {
 100                         b = mosaic_byte();
 101                         if (b == '\n')
 102                                 return;
 103                 }
 104         }
 105 }
 106
 107 static void
 108 mosaic_command(const char *cmd)
 109 {
 110         mosaic_puts(cmd);
 111         mosaic_wait_idle();
 112 }
 113
 114 static void
 115 mosaic_setup(void)
 116 {
 117         ao_mosaic_set_speed(AO_SERIAL_SPEED_115200);
 118         /* Send messages always */
 119         mosaic_command("smrf, OnlyRef \r");
 120         /* Set receiver dynamics mode */
 121         mosaic_command("srd, High, Unlimited \r");
 122         /* Report position once per second */
 123         /* Report time once per second */
 124         /* Report sat info once per second */
 125         mosaic_command("sso, Stream1, COM3, ReceiverTime+MeasEpoch+PVTGeodetic+DOP, sec1 \r");
 126 }
 127
 128 #if AO_MOSAIC_DEBUG && !defined(AO_GPS_TEST)
 129 static void mosaic_option(void)
 130 {
 131         uint8_t r = (uint8_t) ao_cmd_hex();
 132         if (ao_cmd_status != ao_cmd_success) {
 133                 ao_cmd_status = ao_cmd_success;
 134                 ao_gps_show();
 135         } else {
 136                 mosaic_dbg_enable = r;
 137                 printf ("gps debug set to %d\n", mosaic_dbg_enable);
 138         }
 139 }
 140 #else
 141 #define mosaic_option ao_gps_show
 142 #endif
 143
 144 const struct ao_cmds ao_mosaic_cmds[] = {
 145         { mosaic_option,        "g\0Display Mosaic GPS" },
 146         { 0, NULL },
 147 };
 148
 149 static struct sbf sbf;
 150
 151 static void
 152 mosaic_read(void *buf, size_t len)
 153 {
 154         uint8_t *b = buf;
 155         while (len--)
 156                 *b++ = mosaic_byte();
 157 }
 158
 159 static uint16_t
 160 mosaic_read_crc(void *buf, size_t len, uint16_t crc)
 161 {
 162         uint8_t *b = buf;
 163
 164         while (len--) {
 165                 uint8_t byte = mosaic_byte();
 166                 crc = ao_crc16_ccitt(crc, byte);
 167                 *b++ = byte;
 168         }
 169         return crc;
 170 }
 171
 172 static struct ao_telemetry_location new_location;
 173 static struct ao_telemetry_satellite new_satellite;
 174
 175 static
 176 int32_t
 177 clip_value(float value, float low, float high)
 178 {
 179         if (value < low)
 180                 value = low;
 181         else if (value > high)
 182                 value = high;
 183         return (int32_t) roundf(value);
 184 }
 185
 186 void
 187 ao_gps_mosaic(void)
 188 {
 189         AO_TICK_TYPE            packet_start_tick;
 190         AO_TICK_TYPE            solution_tick = 0;
 191
 192 #ifndef AO_GPS_TEST
 193         ao_cmd_register(&ao_mosaic_cmds[0]);
 194 #endif
 195         mosaic_setup();
 196
 197         for (;;) {
 198                 uint16_t        crc_computed;
 199
 200                 while(mosaic_byte() != '$')
 201                         ;
 202                 if (mosaic_byte() != '@')
 203                         continue;
 204                 packet_start_tick = ao_tick_count;
 205
 206                 mosaic_read(&sbf.header.crc, sizeof(sbf.header.crc));
 207                 crc_computed = mosaic_read_crc(&sbf.header.h, sizeof(sbf.header.h), 0);
 208                 if (sbf.header.h.length > sizeof(sbf) + 2) {
 209                         mosaic_dbg(DBG_PROTO, "too long %d > %ld\n",
 210                                    sbf.header.h.length, sizeof(sbf) + 2);
 211                         continue;
 212                 }
 213                 crc_computed = mosaic_read_crc(sbf.data, sbf.header.h.length - 8, crc_computed);
 214                 if (crc_computed != sbf.header.crc) {
 215                         mosaic_dbg(DBG_PROTO, "crc error (computed 0x%04x)\n", crc_computed);
 216                         continue;
 217                 }
 218
 219                 bool gps_ready = false;
 220
 221                 switch (SBF_BLOCK_NUMBER(sbf.header.h.id)) {
 222                 case SBF_RECEIVER_TIME:
 223                         solution_tick = packet_start_tick;
 224                         memset(&new_location, 0, sizeof(new_location));
 225                         memset(&new_satellite, 0, sizeof(new_satellite));
 226                         new_location.year = (uint8_t) sbf.receiver_time.utcyear;
 227                         new_location.month = (uint8_t) sbf.receiver_time.utcmonth;
 228                         new_location.day = (uint8_t) sbf.receiver_time.utcday;
 229                         new_location.hour = (uint8_t) sbf.receiver_time.utchour;
 230                         new_location.minute = (uint8_t) sbf.receiver_time.utcmin;
 231                         new_location.second = (uint8_t) sbf.receiver_time.utcsec;
 232
 233                         if (sbf.receiver_time.utcyear != -128 &&
 234                             sbf.receiver_time.utcmonth != -128 &&
 235                             sbf.receiver_time.utcday != -128 &&
 236                             sbf.receiver_time.utchour != -128 &&
 237                             sbf.receiver_time.utcmin != -128 &&
 238                             sbf.receiver_time.utcsec != -128)
 239                         {
 240                                 new_location.flags |= AO_GPS_DATE_VALID;
 241                         }
 242
 243                         mosaic_dbg(DBG_PROTO, "ReceiverTime year %d month %d day %d hour %d min %d sec %d\n",
 244                                    sbf.receiver_time.utcyear,
 245                                    sbf.receiver_time.utcmonth,
 246                                    sbf.receiver_time.utcday,
 247                                    sbf.receiver_time.utchour,
 248                                    sbf.receiver_time.utcmin,
 249                                    sbf.receiver_time.utcsec);
 250                         break;
 251                 case SBF_MEAS_EPOCH:
 252                         mosaic_dbg(DBG_PROTO, "MeasEpoch sb1len %d sb2len %d\n",
 253                                    sbf.meas_epoch.sb1length,
 254                                    sbf.meas_epoch.sb2length);
 255                         {
 256                                 uint8_t i1, i2;
 257                                 uint8_t nsat = 0, nsol = 0;
 258                                 struct sbf_meas_epoch_channel_type1 *type1;
 259                                 struct sbf_meas_epoch_channel_type2 *type2;
 260
 261                                 type1 = (void *) (&sbf.meas_epoch + 1);
 262                                 for (i1 = 0; i1 < sbf.meas_epoch.n1; i1++) {
 263                                         uint8_t signal_type = type1->type & 0x1f;
 264                                         uint8_t cn0;
 265
 266                                         if (signal_type == 1 || signal_type == 2)
 267                                                 cn0 = type1->cn0 / 4;
 268                                         else
 269                                                 cn0 = type1->cn0 / 4 + 10;
 270                                         if (nsat < AO_TELEMETRY_SATELLITE_MAX_SAT) {
 271                                                 new_satellite.sats[nsat].svid = type1->svid;
 272                                                 new_satellite.sats[nsat].c_n_1 = cn0;
 273                                                 new_satellite.channels = nsat + 1;
 274                                         }
 275                                         nsat++;
 276                                         if (type1->locktime != 65535)
 277                                                 nsol++;
 278                                         mosaic_dbg(DBG_PROTO, "  Type1 type 0x%x channel %d svid %d cn0 %d locktime %u\n",
 279                                                    type1->type, type1->rxchannel, type1->svid, cn0, type1->locktime);
 280                                         type2 = (void *) ((uint8_t *) type1 + sbf.meas_epoch.sb1length);
 281                                         for (i2 = 0; i2 < type1->n2; i2++) {
 282                                                 mosaic_dbg(DBG_PROTO, "    Type2 type %d cn0 %d\n",
 283                                                            type2->type, type2->cn0);
 284                                                 type2 = (void *) ((uint8_t *) type2 + sbf.meas_epoch.sb1length);
 285                                         }
 286                                         type1 = (void *) type2;
 287                                 }
 288                                 if (nsol > 15)
 289                                         nsol = 15;
 290                                 new_location.flags |= nsol;
 291                         }
 292                         break;
 293                 case SBF_PVT_GEODETIC:
 294                         mosaic_dbg(DBG_PROTO, "PVTGeodetic mode 0x%02x error %d lat %f lon %f height %f\n",
 295                                    sbf.geodetic.mode,
 296                                    sbf.geodetic.error,
 297                                    sbf.geodetic.latitude * 180.0/M_PI,
 298                                    sbf.geodetic.longitude * 180.0/M_PI,
 299                                    sbf.geodetic.height);
 300
 301                         /* Need to use double here to preserve all of the available precision */
 302                         new_location.latitude = (int32_t) round(sbf.geodetic.latitude * (1e7 * 180.0/M_PI));
 303                         new_location.longitude = (int32_t) round(sbf.geodetic.longitude * (1e7 * 180.0f/M_PI));
 304                         gps_alt_t altitude = (gps_alt_t) round(sbf.geodetic.height);
 305                         AO_TELEMETRY_LOCATION_SET_ALTITUDE(&new_location, altitude);
 306                         if (sbf.geodetic.latitude != -2e10 &&
 307                             sbf.geodetic.longitude != -2e10 &&
 308                             sbf.geodetic.height != -2e10)
 309                         {
 310                                 new_location.flags |= AO_GPS_VALID;
 311                         }
 312                         if (sbf.geodetic.vn != -2e10 &&
 313                             sbf.geodetic.ve != -2e10 &&
 314                             sbf.geodetic.vu != -2e10)
 315                         {
 316                                 new_location.flags |= AO_GPS_COURSE_VALID;
 317                         }
 318                         float ground_speed = hypotf((float) sbf.geodetic.vn, (float) sbf.geodetic.ve);
 319                         float course = atan2f((float) sbf.geodetic.ve, (float) sbf.geodetic.vn);
 320                         new_location.ground_speed = (uint16_t) clip_value(ground_speed, 0, 65535.0f);
 321                         new_location.climb_rate = (int16_t) clip_value(sbf.geodetic.vu * 100, -32768.0f, 32767.0f);
 322                         new_location.course = (uint8_t) clip_value(course * (90.0f/(float)M_PI), 0.0f, 255.0f);
 323                         break;
 324                 case SBF_DOP:
 325                         mosaic_dbg(DBG_PROTO, "DOP pdop%d tdop %d hdop %d vdop %d\n",
 326                                    sbf.dop.pdop,
 327                                    sbf.dop.tdop,
 328                                    sbf.dop.hdop,
 329                                    sbf.dop.vdop);
 330                         new_location.pdop = (uint8_t) (sbf.dop.pdop / 10);
 331                         new_location.hdop = (uint8_t) (sbf.dop.hdop / 10);
 332                         new_location.vdop = (uint8_t) (sbf.dop.vdop / 10);
 333                         gps_ready = true;
 334                         break;
 335                 default:
 336                         mosaic_dbg(DBG_PROTO, "block %d revision %d length %d avail %ld\n",
 337                                    SBF_BLOCK_NUMBER(sbf.header.h.id),
 338                                    SBF_BLOCK_REVISION(sbf.header.h.id),
 339                                    sbf.header.h.length,
 340                                    sizeof(sbf));
 341                         break;
 342                 }
 343
 344                 if (!gps_ready)
 345                         continue;
 346
 347                 new_location.flags |= AO_GPS_RUNNING;
 348
 349                 ao_mutex_get(&ao_gps_mutex);
 350                 ao_gps_data = new_location;
 351                 ao_gps_tracking_data = new_satellite;
 352                 ao_gps_tick = solution_tick;
 353
 354                 ao_mutex_put(&ao_gps_mutex);
 355                 ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
 356                 ao_wakeup(&ao_gps_new);
 357         }
 358 }
 359
 360 #endif