git.gag.com Git - fw/altos/blob - ao-tools/lib/cc-logfile.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; 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 #include "cc.h"
  20 #include <stdio.h>
  21 #include <stdlib.h>
  22 #include <string.h>
  23
  24 static int
  25 timedata_add(struct cc_timedata *data, double time, double value)
  26 {
  27         struct cc_timedataelt   *newdata;
  28         int                     newsize;
  29         if (data->size == data->num) {
  30                 if (data->size == 0)
  31                         newdata = malloc((newsize = 256) * sizeof (struct cc_timedataelt));
  32                 else
  33                         newdata = realloc (data->data, (newsize = data->size * 2)
  34                                            * sizeof (struct cc_timedataelt));
  35                 if (!newdata)
  36                         return 0;
  37                 data->size = newsize;
  38                 data->data = newdata;
  39         }
  40         time += data->time_offset;
  41         if (data->num && data->data[data->num-1].time > time) {
  42                 data->time_offset += 65536;
  43                 time += 65536;
  44         }
  45         data->data[data->num].time = time;
  46         data->data[data->num].value = value;
  47         data->num++;
  48         return 1;
  49 }
  50
  51 static void
  52 timedata_free(struct cc_timedata *data)
  53 {
  54         if (data->data)
  55                 free(data->data);
  56 }
  57
  58 static int
  59 gpsdata_add(struct cc_gpsdata *data, struct cc_gpselt *elt)
  60 {
  61         struct cc_gpselt        *newdata;
  62         int                     newsize;
  63         if (data->size == data->num) {
  64                 if (data->size == 0)
  65                         newdata = malloc((newsize = 256) * sizeof (struct cc_gpselt));
  66                 else
  67                         newdata = realloc (data->data, (newsize = data->size * 2)
  68                                            * sizeof (struct cc_gpselt));
  69                 if (!newdata)
  70                         return 0;
  71                 data->size = newsize;
  72                 data->data = newdata;
  73         }
  74         elt->time += data->time_offset;
  75         if (data->num && data->data[data->num-1].time > elt->time) {
  76                 data->time_offset += 65536;
  77                 elt->time += 65536;
  78         }
  79         data->data[data->num] = *elt;
  80         data->num++;
  81         return 1;
  82 }
  83
  84 static int
  85 gpssat_add(struct cc_gpsdata *data, struct cc_gpssat *sat)
  86 {
  87         int     i, j;
  88         int     reuse = 0;
  89         int     newsizesats;
  90         struct cc_gpssats       *newsats;
  91
  92         for (i = data->numsats; --i >= 0;) {
  93                 if (data->sats[i].sat[0].time == sat->time) {
  94                         reuse = 1;
  95                         break;
  96                 }
  97                 if (data->sats[i].sat[0].time < sat->time)
  98                         break;
  99         }
 100         if (!reuse) {
 101                 if (data->numsats == data->sizesats) {
 102                         if (data->sizesats == 0)
 103                                 newsats = malloc((newsizesats = 256) * sizeof (struct cc_gpssats));
 104                         else
 105                                 newsats = realloc (data->sats, (newsizesats = data->sizesats * 2)
 106                                                    * sizeof (struct cc_gpssats));
 107                         if (!newsats)
 108                                 return 0;
 109                         data->sats = newsats;
 110                         data->sizesats = newsizesats;
 111                 }
 112                 i = data->numsats++;
 113                 data->sats[i].nsat = 0;
 114         }
 115         j = data->sats[i].nsat;
 116         if (j < 12) {
 117                 data->sats[i].sat[j] = *sat;
 118                 data->sats[i].nsat = j + 1;
 119         }
 120         return 1;
 121 }
 122
 123 static void
 124 gpsdata_free(struct cc_gpsdata *data)
 125 {
 126         if (data->data)
 127                 free(data->data);
 128 }
 129
 130 #define AO_LOG_FLIGHT           'F'
 131 #define AO_LOG_SENSOR           'A'
 132 #define AO_LOG_TEMP_VOLT        'T'
 133 #define AO_LOG_DEPLOY           'D'
 134 #define AO_LOG_STATE            'S'
 135 #define AO_LOG_GPS_TIME         'G'
 136 #define AO_LOG_GPS_LAT          'N'
 137 #define AO_LOG_GPS_LON          'W'
 138 #define AO_LOG_GPS_ALT          'H'
 139 #define AO_LOG_GPS_SAT          'V'
 140 #define AO_LOG_GPS_DATE         'Y'
 141
 142 #define AO_LOG_POS_NONE         (~0UL)
 143
 144 #define GPS_TIME        1
 145 #define GPS_LAT         2
 146 #define GPS_LON         4
 147 #define GPS_ALT         8
 148
 149 static int
 150 read_eeprom(const char *line, struct cc_flightraw *f, double *ground_pres, int *ground_pres_count)
 151 {
 152         char    type;
 153         int     tick;
 154         int     a, b;
 155         static struct cc_gpselt gps;
 156         static int              gps_valid;
 157         struct cc_gpssat        sat;
 158         int     serial;
 159
 160         if (sscanf(line, "serial-number %u", &serial) == 1) {
 161                 f->serial = serial;
 162                 return 1;
 163         }
 164         if (sscanf(line, "%c %x %x %x", &type, &tick, &a, &b) != 4)
 165                 return 0;
 166         switch (type) {
 167         case AO_LOG_FLIGHT:
 168                 f->ground_accel = a;
 169                 f->ground_pres = 0;
 170                 f->flight = b;
 171                 *ground_pres = 0;
 172                 *ground_pres_count = 0;
 173                 break;
 174         case AO_LOG_SENSOR:
 175                 timedata_add(&f->accel, tick, a);
 176                 timedata_add(&f->pres, tick, b);
 177                 if (*ground_pres_count < 20) {
 178                         *ground_pres += b;
 179                         (*ground_pres_count)++;
 180                         if (*ground_pres_count >= 20)
 181                                 f->ground_pres = *ground_pres / *ground_pres_count;
 182                 }
 183                 break;
 184         case AO_LOG_TEMP_VOLT:
 185                 timedata_add(&f->temp, tick, a);
 186                 timedata_add(&f->volt, tick, b);
 187                 break;
 188         case AO_LOG_DEPLOY:
 189                 timedata_add(&f->drogue, tick, a);
 190                 timedata_add(&f->main, tick, b);
 191                 break;
 192         case AO_LOG_STATE:
 193                 timedata_add(&f->state, tick, a);
 194                 break;
 195         case AO_LOG_GPS_TIME:
 196                 /* the flight computer writes TIME first, so reset
 197                  * any stale data before adding this record
 198                  */
 199                 gps.time = tick;
 200                 gps.hour = (a & 0xff);
 201                 gps.minute = (a >> 8) & 0xff;
 202                 gps.second = (b & 0xff);
 203                 gps.flags = (b >> 8) & 0xff;
 204                 gps_valid = GPS_TIME;
 205                 break;
 206         case AO_LOG_GPS_LAT:
 207                 gps.lat = ((int32_t) (a + (b << 16))) / 10000000.0;
 208                 gps_valid |= GPS_LAT;
 209                 break;
 210         case AO_LOG_GPS_LON:
 211                 gps.lon = ((int32_t) (a + (b << 16))) / 10000000.0;
 212                 gps_valid |= GPS_LON;
 213                 break;
 214         case AO_LOG_GPS_ALT:
 215                 gps.alt = (int16_t) a;
 216                 gps_valid |= GPS_ALT;
 217                 break;
 218         case AO_LOG_GPS_SAT:
 219                 sat.time = tick;
 220                 sat.svid = a;
 221                 sat.c_n = (b >> 8) & 0xff;
 222                 gpssat_add(&f->gps, &sat);
 223                 break;
 224         case AO_LOG_GPS_DATE:
 225                 f->year = 2000 + (a & 0xff);
 226                 f->month = (a >> 8) & 0xff;
 227                 f->day = (b & 0xff);
 228                 break;
 229         default:
 230                 return 0;
 231         }
 232         if (gps_valid == 0xf) {
 233                 gps_valid = 0;
 234                 gpsdata_add(&f->gps, &gps);
 235         }
 236         return 1;
 237 }
 238
 239 static const char *state_names[] = {
 240         "startup",
 241         "idle",
 242         "pad",
 243         "boost",
 244         "fast",
 245         "coast",
 246         "drogue",
 247         "main",
 248         "landed",
 249         "invalid"
 250 };
 251
 252 static enum ao_flight_state
 253 state_name_to_state(char *state_name)
 254 {
 255         enum ao_flight_state    state;
 256         for (state = ao_flight_startup; state < ao_flight_invalid; state++)
 257                 if (!strcmp(state_names[state], state_name))
 258                         return state;
 259         return ao_flight_invalid;
 260 }
 261
 262 static int
 263 read_telem(const char *line, struct cc_flightraw *f)
 264 {
 265         struct cc_telem         telem;
 266         struct cc_gpselt        gps;
 267         struct cc_gpssat        sat;
 268         int                     s;
 269
 270         if (!cc_telem_parse(line, &telem))
 271                 return 0;
 272         f->ground_accel = telem.ground_accel;
 273         f->ground_pres = telem.ground_pres;
 274         f->flight = 0;
 275         timedata_add(&f->accel, telem.tick, telem.flight_accel);
 276         timedata_add(&f->pres, telem.tick, telem.flight_pres);
 277         timedata_add(&f->temp, telem.tick, telem.temp);
 278         timedata_add(&f->volt, telem.tick, telem.batt);
 279         timedata_add(&f->drogue, telem.tick, telem.drogue);
 280         timedata_add(&f->main, telem.tick, telem.main);
 281         timedata_add(&f->state, telem.tick, state_name_to_state(telem.state));
 282         if (telem.gps.gps_locked) {
 283                 f->year = telem.gps.gps_time.year;
 284                 f->month = telem.gps.gps_time.month;
 285                 f->day = telem.gps.gps_time.day;
 286                 gps.time = telem.tick;
 287                 gps.lat = telem.gps.lat;
 288                 gps.lon = telem.gps.lon;
 289                 gps.alt = telem.gps.alt;
 290                 gps.hour = telem.gps.gps_time.hour;
 291                 gps.minute = telem.gps.gps_time.minute;
 292                 gps.second = telem.gps.gps_time.second;
 293                 gpsdata_add(&f->gps, &gps);
 294         }
 295         for (s = 0; s < telem.gps_tracking.channels; s++) {
 296                 sat.time = telem.tick;
 297                 sat.svid = telem.gps_tracking.sats[s].svid;
 298                 sat.c_n = telem.gps_tracking.sats[s].c_n0;
 299                 gpssat_add(&f->gps, &sat);
 300         }
 301         return 1;
 302 }
 303
 304 struct cc_flightraw *
 305 cc_log_read(FILE *file)
 306 {
 307         struct cc_flightraw     *f;
 308         char                    line[8192];
 309         double                  ground_pres;
 310         int                     ground_pres_count;
 311
 312         f = calloc(1, sizeof (struct cc_flightraw));
 313         if (!f)
 314                 return NULL;
 315         while (fgets(line, sizeof (line), file)) {
 316                 if (read_eeprom(line, f, &ground_pres, &ground_pres_count))
 317                         continue;
 318                 if (read_telem(line, f))
 319                         continue;
 320                 fprintf (stderr, "invalid line: %s", line);
 321         }
 322         return f;
 323 }
 324
 325 void
 326 cc_flightraw_free(struct cc_flightraw *raw)
 327 {
 328         timedata_free(&raw->accel);
 329         timedata_free(&raw->pres);
 330         timedata_free(&raw->temp);
 331         timedata_free(&raw->volt);
 332         timedata_free(&raw->main);
 333         timedata_free(&raw->drogue);
 334         timedata_free(&raw->state);
 335         gpsdata_free(&raw->gps);
 336         free(raw);
 337 }