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
 140 #define AO_LOG_POS_NONE         (~0UL)
 141
 142 #define GPS_TIME        1
 143 #define GPS_LAT         2
 144 #define GPS_LON         4
 145 #define GPS_ALT         8
 146
 147 static int
 148 read_eeprom(const char *line, struct cc_flightraw *f, double *ground_pres, int *ground_pres_count)
 149 {
 150         char    type;
 151         int     tick;
 152         int     a, b;
 153         static struct cc_gpselt gps;
 154         static int              gps_valid;
 155         struct cc_gpssat        sat;
 156         int     serial;
 157
 158         if (sscanf(line, "serial-number %u", &serial) == 1) {
 159                 f->serial = serial;
 160                 return 1;
 161         }
 162         if (sscanf(line, "%c %x %x %x", &type, &tick, &a, &b) != 4)
 163                 return 0;
 164         switch (type) {
 165         case AO_LOG_FLIGHT:
 166                 f->ground_accel = a;
 167                 f->ground_pres = 0;
 168                 f->flight = b;
 169                 *ground_pres = 0;
 170                 *ground_pres_count = 0;
 171                 break;
 172         case AO_LOG_SENSOR:
 173                 timedata_add(&f->accel, tick, a);
 174                 timedata_add(&f->pres, tick, b);
 175                 if (*ground_pres_count < 20) {
 176                         *ground_pres += b;
 177                         (*ground_pres_count)++;
 178                         if (*ground_pres_count >= 20)
 179                                 f->ground_pres = *ground_pres / *ground_pres_count;
 180                 }
 181                 break;
 182         case AO_LOG_TEMP_VOLT:
 183                 timedata_add(&f->temp, tick, a);
 184                 timedata_add(&f->volt, tick, b);
 185                 break;
 186         case AO_LOG_DEPLOY:
 187                 timedata_add(&f->drogue, tick, a);
 188                 timedata_add(&f->main, tick, b);
 189                 break;
 190         case AO_LOG_STATE:
 191                 timedata_add(&f->state, tick, a);
 192                 break;
 193         case AO_LOG_GPS_TIME:
 194                 /* the flight computer writes TIME first, so reset
 195                  * any stale data before adding this record
 196                  */
 197                 gps.time = tick;
 198                 gps_valid = GPS_TIME;
 199                 break;
 200         case AO_LOG_GPS_LAT:
 201                 gps.lat = ((int32_t) (a + (b << 16))) / 10000000.0;
 202                 gps_valid |= GPS_LAT;
 203                 break;
 204         case AO_LOG_GPS_LON:
 205                 gps.lon = ((int32_t) (a + (b << 16))) / 10000000.0;
 206                 gps_valid |= GPS_LON;
 207                 break;
 208         case AO_LOG_GPS_ALT:
 209                 gps.alt = (int16_t) a;
 210                 gps_valid |= GPS_ALT;
 211                 break;
 212         case AO_LOG_GPS_SAT:
 213                 sat.time = tick;
 214                 sat.svid = a;
 215                 sat.state = (b & 0xff);
 216                 sat.c_n = (b >> 8) & 0xff;
 217                 gpssat_add(&f->gps, &sat);
 218                 break;
 219         default:
 220                 return 0;
 221         }
 222         if (gps_valid == 0xf) {
 223                 gps_valid = 0;
 224                 gpsdata_add(&f->gps, &gps);
 225         }
 226         return 1;
 227 }
 228
 229 static const char *state_names[] = {
 230         "startup",
 231         "idle",
 232         "pad",
 233         "boost",
 234         "fast",
 235         "coast",
 236         "drogue",
 237         "main",
 238         "landed",
 239         "invalid"
 240 };
 241
 242 static enum ao_flight_state
 243 state_name_to_state(char *state_name)
 244 {
 245         enum ao_flight_state    state;
 246         for (state = ao_flight_startup; state < ao_flight_invalid; state++)
 247                 if (!strcmp(state_names[state], state_name))
 248                         return state;
 249         return ao_flight_invalid;
 250 }
 251
 252 static int
 253 read_telem(const char *line, struct cc_flightraw *f)
 254 {
 255         struct cc_telem         telem;
 256         struct cc_gpselt        gps;
 257         if (!cc_telem_parse(line, &telem))
 258                 return 0;
 259         f->ground_accel = telem.ground_accel;
 260         f->ground_pres = telem.ground_pres;
 261         f->flight = 0;
 262         timedata_add(&f->accel, telem.tick, telem.flight_accel);
 263         timedata_add(&f->pres, telem.tick, telem.flight_pres);
 264         timedata_add(&f->temp, telem.tick, telem.temp);
 265         timedata_add(&f->volt, telem.tick, telem.batt);
 266         timedata_add(&f->drogue, telem.tick, telem.drogue);
 267         timedata_add(&f->main, telem.tick, telem.main);
 268         timedata_add(&f->state, telem.tick, state_name_to_state(telem.state));
 269         if (telem.gps.gps_locked) {
 270                 gps.time = telem.tick;
 271                 gps.lat = telem.gps.lat;
 272                 gps.lon = telem.gps.lon;
 273                 gps.alt = telem.gps.alt;
 274                 gpsdata_add(&f->gps, &gps);
 275         }
 276         return 1;
 277 }
 278
 279 struct cc_flightraw *
 280 cc_log_read(FILE *file)
 281 {
 282         struct cc_flightraw     *f;
 283         char                    line[8192];
 284         double                  ground_pres;
 285         int                     ground_pres_count;
 286
 287         f = calloc(1, sizeof (struct cc_flightraw));
 288         if (!f)
 289                 return NULL;
 290         while (fgets(line, sizeof (line), file)) {
 291                 if (read_eeprom(line, f, &ground_pres, &ground_pres_count))
 292                         continue;
 293                 if (read_telem(line, f))
 294                         continue;
 295                 fprintf (stderr, "invalid line: %s", line);
 296         }
 297         return f;
 298 }
 299
 300 void
 301 cc_flightraw_free(struct cc_flightraw *raw)
 302 {
 303         timedata_free(&raw->accel);
 304         timedata_free(&raw->pres);
 305         timedata_free(&raw->temp);
 306         timedata_free(&raw->volt);
 307         timedata_free(&raw->main);
 308         timedata_free(&raw->drogue);
 309         timedata_free(&raw->state);
 310         gpsdata_free(&raw->gps);
 311         free(raw);
 312 }