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 void
  84 gpsdata_free(struct cc_gpsdata *data)
  85 {
  86         if (data->data)
  87                 free(data->data);
  88 }
  89
  90 #define AO_LOG_FLIGHT           'F'
  91 #define AO_LOG_SENSOR           'A'
  92 #define AO_LOG_TEMP_VOLT        'T'
  93 #define AO_LOG_DEPLOY           'D'
  94 #define AO_LOG_STATE            'S'
  95 #define AO_LOG_GPS_TIME         'G'
  96 #define AO_LOG_GPS_LAT          'N'
  97 #define AO_LOG_GPS_LON          'W'
  98 #define AO_LOG_GPS_ALT          'H'
  99 #define AO_LOG_GPS_SAT          'V'
 100
 101 #define AO_LOG_POS_NONE         (~0UL)
 102
 103 static int
 104 read_eeprom(const char *line, struct cc_flightraw *f, double *ground_pres, int *ground_pres_count)
 105 {
 106         char    type;
 107         int     tick;
 108         int     a, b;
 109         struct cc_gpselt        gps;
 110         int     serial;
 111
 112         if (sscanf(line, "serial-number %u", &serial) == 1) {
 113                 f->serial = serial;
 114                 return 1;
 115         }
 116         if (sscanf(line, "%c %x %x %x", &type, &tick, &a, &b) != 4)
 117                 return 0;
 118         switch (type) {
 119         case AO_LOG_FLIGHT:
 120                 f->ground_accel = a;
 121                 f->ground_pres = 0;
 122                 f->flight = b;
 123                 *ground_pres = 0;
 124                 *ground_pres_count = 0;
 125                 break;
 126         case AO_LOG_SENSOR:
 127                 timedata_add(&f->accel, tick, a);
 128                 timedata_add(&f->pres, tick, b);
 129                 if (*ground_pres_count < 20) {
 130                         *ground_pres += b;
 131                         (*ground_pres_count)++;
 132                         if (*ground_pres_count >= 20)
 133                                 f->ground_pres = *ground_pres / *ground_pres_count;
 134                 }
 135                 break;
 136         case AO_LOG_TEMP_VOLT:
 137                 timedata_add(&f->temp, tick, a);
 138                 timedata_add(&f->volt, tick, b);
 139                 break;
 140         case AO_LOG_DEPLOY:
 141                 timedata_add(&f->drogue, tick, a);
 142                 timedata_add(&f->main, tick, b);
 143                 break;
 144         case AO_LOG_STATE:
 145                 timedata_add(&f->state, tick, a);
 146                 break;
 147         case AO_LOG_GPS_TIME:
 148                 gps.time = tick;
 149                 break;
 150         case AO_LOG_GPS_LAT:
 151                 gps.lat = ((int32_t) (a + (b << 16))) / 10000000.0;
 152                 break;
 153         case AO_LOG_GPS_LON:
 154                 gps.lon = ((int32_t) (a + (b << 16))) / 10000000.0;
 155                 break;
 156         case AO_LOG_GPS_ALT:
 157                 gps.alt = ((int32_t) (a + (b << 16)));
 158                 gpsdata_add(&f->gps, &gps);
 159                 break;
 160         case AO_LOG_GPS_SAT:
 161                 break;
 162         default:
 163                 return 0;
 164         }
 165         return 1;
 166 }
 167
 168 static const char *state_names[] = {
 169         "startup",
 170         "idle",
 171         "pad",
 172         "boost",
 173         "fast",
 174         "coast",
 175         "drogue",
 176         "main",
 177         "landed",
 178         "invalid"
 179 };
 180
 181 static enum ao_flight_state
 182 state_name_to_state(char *state_name)
 183 {
 184         enum ao_flight_state    state;
 185         for (state = ao_flight_startup; state < ao_flight_invalid; state++)
 186                 if (!strcmp(state_names[state], state_name))
 187                         return state;
 188         return ao_flight_invalid;
 189 }
 190
 191 static int
 192 read_telem(const char *line, struct cc_flightraw *f)
 193 {
 194         struct cc_telem         telem;
 195         struct cc_gpselt        gps;
 196         if (!cc_telem_parse(line, &telem))
 197                 return 0;
 198         f->ground_accel = telem.ground_accel;
 199         f->ground_pres = telem.ground_pres;
 200         f->flight = 0;
 201         timedata_add(&f->accel, telem.tick, telem.flight_accel);
 202         timedata_add(&f->pres, telem.tick, telem.flight_pres);
 203         timedata_add(&f->temp, telem.tick, telem.temp);
 204         timedata_add(&f->volt, telem.tick, telem.batt);
 205         timedata_add(&f->drogue, telem.tick, telem.drogue);
 206         timedata_add(&f->main, telem.tick, telem.main);
 207         timedata_add(&f->state, telem.tick, state_name_to_state(telem.state));
 208         if (telem.gps.gps_locked) {
 209                 gps.time = telem.tick;
 210                 gps.lat = telem.gps.lat;
 211                 gps.lon = telem.gps.lon;
 212                 gps.alt = telem.gps.alt;
 213                 gpsdata_add(&f->gps, &gps);
 214         }
 215         return 1;
 216 }
 217
 218 struct cc_flightraw *
 219 cc_log_read(FILE *file)
 220 {
 221         struct cc_flightraw     *f;
 222         char                    line[8192];
 223         double                  ground_pres;
 224         int                     ground_pres_count;
 225
 226         f = calloc(1, sizeof (struct cc_flightraw));
 227         if (!f)
 228                 return NULL;
 229         while (fgets(line, sizeof (line), file)) {
 230                 if (read_eeprom(line, f, &ground_pres, &ground_pres_count))
 231                         continue;
 232                 if (read_telem(line, f))
 233                         continue;
 234                 fprintf (stderr, "invalid line: %s", line);
 235         }
 236         return f;
 237 }
 238
 239 void
 240 cc_flightraw_free(struct cc_flightraw *raw)
 241 {
 242         timedata_free(&raw->accel);
 243         timedata_free(&raw->pres);
 244         timedata_free(&raw->temp);
 245         timedata_free(&raw->volt);
 246         timedata_free(&raw->main);
 247         timedata_free(&raw->drogue);
 248         timedata_free(&raw->state);
 249         gpsdata_free(&raw->gps);
 250         free(raw);
 251 }