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
  22 static int
  23 timedata_add(struct cc_timedata *data, double time, double value)
  24 {
  25         struct cc_timedataelt   *newdata;
  26         int                     newsize;
  27         if (data->size == data->num) {
  28                 if (data->size == 0)
  29                         newdata = malloc((newsize = 256) * sizeof (struct cc_timedataelt));
  30                 else
  31                         newdata = realloc (data->data, (newsize = data->size * 2)
  32                                            * sizeof (struct cc_timedataelt));
  33                 if (!newdata)
  34                         return 0;
  35                 data->size = newsize;
  36                 data->data = newdata;
  37         }
  38         if (data->num && data->data[data->num-1].time > time)
  39                 time += 65536;
  40         data->data[data->num].time = time;
  41         data->data[data->num].value = value;
  42         data->num++;
  43         return 1;
  44 }
  45
  46 static void
  47 timedata_free(struct cc_timedata *data)
  48 {
  49         if (data->data)
  50                 free(data->data);
  51 }
  52
  53 static int
  54 gpsdata_add(struct cc_gpsdata *data, struct cc_gpselt *elt)
  55 {
  56         struct cc_gpselt        *newdata;
  57         int                     newsize;
  58         if (data->size == data->num) {
  59                 if (data->size == 0)
  60                         newdata = malloc((newsize = 256) * sizeof (struct cc_gpselt));
  61                 else
  62                         newdata = realloc (data->data, (newsize = data->size * 2)
  63                                            * sizeof (struct cc_gpselt));
  64                 if (!newdata)
  65                         return 0;
  66                 data->size = newsize;
  67                 data->data = newdata;
  68         }
  69         data->data[data->num] = *elt;
  70         data->num++;
  71         return 1;
  72 }
  73
  74 static void
  75 gpsdata_free(struct cc_gpsdata *data)
  76 {
  77         if (data->data)
  78                 free(data->data);
  79 }
  80
  81 #define AO_LOG_FLIGHT           'F'
  82 #define AO_LOG_SENSOR           'A'
  83 #define AO_LOG_TEMP_VOLT        'T'
  84 #define AO_LOG_DEPLOY           'D'
  85 #define AO_LOG_STATE            'S'
  86 #define AO_LOG_GPS_TIME         'G'
  87 #define AO_LOG_GPS_LAT          'N'
  88 #define AO_LOG_GPS_LON          'W'
  89 #define AO_LOG_GPS_ALT          'H'
  90 #define AO_LOG_GPS_SAT          'V'
  91
  92 #define AO_LOG_POS_NONE         (~0UL)
  93
  94 static int
  95 read_eeprom(const char *line, struct cc_flightraw *f, double *ground_pres, int *ground_pres_count)
  96 {
  97         char    type;
  98         int     tick;
  99         int     a, b;
 100         struct cc_gpselt        gps;
 101         int     serial;
 102
 103         if (sscanf(line, "serial-number %u", &serial) == 1) {
 104                 f->serial = serial;
 105                 return 1;
 106         }
 107         if (sscanf(line, "%c %x %x %x", &type, &tick, &a, &b) != 4)
 108                 return 0;
 109         switch (type) {
 110         case AO_LOG_FLIGHT:
 111                 f->ground_accel = a;
 112                 f->ground_pres = 0;
 113                 f->flight = b;
 114                 *ground_pres = 0;
 115                 *ground_pres_count = 0;
 116                 break;
 117         case AO_LOG_SENSOR:
 118                 timedata_add(&f->accel, tick, a);
 119                 timedata_add(&f->pres, tick, b);
 120                 if (*ground_pres_count < 20) {
 121                         *ground_pres += b;
 122                         (*ground_pres_count)++;
 123                         if (*ground_pres_count >= 20)
 124                                 f->ground_pres = *ground_pres / *ground_pres_count;
 125                 }
 126                 break;
 127         case AO_LOG_TEMP_VOLT:
 128                 timedata_add(&f->temp, tick, a);
 129                 timedata_add(&f->volt, tick, b);
 130                 break;
 131         case AO_LOG_DEPLOY:
 132                 timedata_add(&f->drogue, tick, a);
 133                 timedata_add(&f->main, tick, b);
 134                 break;
 135         case AO_LOG_STATE:
 136                 timedata_add(&f->state, tick, a);
 137                 break;
 138         case AO_LOG_GPS_TIME:
 139                 gps.time = tick;
 140                 break;
 141         case AO_LOG_GPS_LAT:
 142                 gps.lat = ((int32_t) (a + (b << 16))) / 10000000.0;
 143                 break;
 144         case AO_LOG_GPS_LON:
 145                 gps.lon = ((int32_t) (a + (b << 16))) / 10000000.0;
 146                 break;
 147         case AO_LOG_GPS_ALT:
 148                 gps.alt = ((int32_t) (a + (b << 16)));
 149                 gpsdata_add(&f->gps, &gps);
 150                 break;
 151         case AO_LOG_GPS_SAT:
 152                 break;
 153         default:
 154                 return 0;
 155         }
 156         return 1;
 157 }
 158
 159 static int
 160 read_telem(const char *line, struct cc_flightraw *f)
 161 {
 162         struct cc_telem         telem;
 163         struct cc_gpselt        gps;
 164         if (!cc_telem_parse(line, &telem))
 165                 return 0;
 166         f->ground_accel = telem.ground_accel;
 167         f->ground_pres = telem.ground_pres;
 168         f->flight = 0;
 169         timedata_add(&f->accel, telem.tick, telem.flight_accel);
 170         timedata_add(&f->pres, telem.tick, telem.flight_pres);
 171         timedata_add(&f->temp, telem.tick, telem.temp);
 172         timedata_add(&f->volt, telem.tick, telem.batt);
 173         timedata_add(&f->drogue, telem.tick, telem.drogue);
 174         timedata_add(&f->main, telem.tick, telem.main);
 175         if (telem.gps.gps_locked) {
 176                 gps.time = telem.tick;
 177                 gps.lat = telem.gps.lat;
 178                 gps.lon = telem.gps.lon;
 179                 gps.alt = telem.gps.alt;
 180                 gpsdata_add(&f->gps, &gps);
 181         }
 182         return 1;
 183 }
 184
 185 struct cc_flightraw *
 186 cc_log_read(FILE *file)
 187 {
 188         struct cc_flightraw     *f;
 189         char                    line[8192];
 190         double                  ground_pres;
 191         int                     ground_pres_count;
 192
 193         f = calloc(1, sizeof (struct cc_flightraw));
 194         if (!f)
 195                 return NULL;
 196         while (fgets(line, sizeof (line), file)) {
 197                 if (read_eeprom(line, f, &ground_pres, &ground_pres_count))
 198                         continue;
 199                 if (read_telem(line, f))
 200                         continue;
 201                 fprintf (stderr, "invalid line: %s", line);
 202         }
 203         return f;
 204 }
 205
 206 void
 207 cc_flightraw_free(struct cc_flightraw *raw)
 208 {
 209         timedata_free(&raw->accel);
 210         timedata_free(&raw->pres);
 211         timedata_free(&raw->temp);
 212         timedata_free(&raw->volt);
 213         timedata_free(&raw->main);
 214         timedata_free(&raw->drogue);
 215         timedata_free(&raw->state);
 216         gpsdata_free(&raw->gps);
 217         free(raw);
 218 }