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