git.gag.com Git - fw/altos/blob - src/test/ao_micropeak_test.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 #define _GNU_SOURCE
  19
  20 #include <stdint.h>
  21 #include <stdio.h>
  22 #include <stdlib.h>
  23 #include <string.h>
  24 #include <getopt.h>
  25 #include <math.h>
  26
  27 FILE *emulator_in;
  28 char *emulator_app;
  29 char *emulator_name;
  30 char *emulator_info;
  31 uint8_t ao_flight_debug;
  32
  33 #define AO_FLIGHT_TEST
  34
  35 typedef int32_t alt_t;
  36
  37 #define AO_MS_TO_TICKS(ms)      ((ms) / 10)
  38
  39 #define AO_LED_REPORT 0
  40
  41 static void ao_led_on(uint8_t led) {
  42 }
  43
  44 static void ao_led_off(uint8_t led) {
  45 }
  46
  47 static void ao_delay_until(uint16_t target) {
  48 }
  49
  50 static uint16_t ao_time(void) {
  51         return 0;
  52 }
  53
  54 #include "ao_microflight.c"
  55 #include "ao_microkalman.c"
  56 #include "ao_convert_pa.c"
  57
  58 uint16_t        now;
  59 uint8_t         running;
  60
  61 void ao_log_micro_data() {
  62         running = 1;
  63 }
  64
  65 void
  66 ao_micro_report(void)
  67 {
  68         if (running) {
  69                 alt_t   ground = ao_pa_to_altitude(pa_ground);
  70                 printf ("%6.2f %10d %10d %10d\n", now / 100.0,
  71                         ao_pa_to_altitude(pa) - ground,
  72                         ao_pa_to_altitude(ao_pa) - ground,
  73                         ao_pa_to_altitude(pa_min) - ground);
  74         }
  75 }
  76
  77 void
  78 ao_micro_finish(void)
  79 {
  80         ao_micro_report();
  81 }
  82
  83 void
  84 ao_pa_get(void)
  85 {
  86         char    line[4096];
  87         char    *toks[128];
  88         char    *saveptr;
  89         int     t, ntok;
  90         static int      time_id;
  91         static int      pa_id;
  92         double          time;
  93         double          pressure;
  94         static double   last_time;
  95         static int      been_here;
  96         static int      start_samples;
  97
  98         if (been_here && start_samples < 100) {
  99                 start_samples++;
 100                 return;
 101         }
 102         ao_micro_report();
 103         for (;;) {
 104                 if (!fgets(line, sizeof (line), emulator_in))
 105                         exit(0);
 106                 for (t = 0; t < 128; t++) {
 107                         toks[t] = strtok_r(t ? NULL : line, ", ", &saveptr);
 108                         if (!toks[t])
 109                                 break;
 110                 }
 111                 ntok = t;
 112                 if (toks[0][0] == '#') {
 113                         if (strcmp(toks[0],"#version") == 0) {
 114                                 for (t = 1; t < ntok; t++) {
 115                                         if (!strcmp(toks[t], "time"))
 116                                                 time_id = t;
 117                                         if (!strcmp(toks[t],"pressure"))
 118                                                 pa_id = t;
 119                                 }
 120                         }
 121                         continue;
 122                 }
 123                 time = strtod(toks[time_id],NULL);
 124                 pressure = strtod(toks[pa_id],NULL);
 125                 if (been_here && time - last_time < 0.1)
 126                         continue;
 127                 been_here = 1;
 128                 last_time = time;
 129                 now = floor (time * 100 + 0.5);
 130                 pa = pressure;
 131                 break;
 132         }
 133 }
 134
 135 void
 136 ao_dump_state(void)
 137 {
 138 }
 139
 140 static const struct option options[] = {
 141         { .name = "summary", .has_arg = 0, .val = 's' },
 142         { .name = "debug", .has_arg = 0, .val = 'd' },
 143         { .name = "info", .has_arg = 1, .val = 'i' },
 144         { 0, 0, 0, 0},
 145 };
 146
 147 void run_flight_fixed(char *name, FILE *f, int summary, char *info)
 148 {
 149         emulator_name = name;
 150         emulator_in = f;
 151         emulator_info = info;
 152         ao_microflight();
 153         ao_micro_finish();
 154 }
 155
 156 int
 157 main (int argc, char **argv)
 158 {
 159         int     summary = 0;
 160         int     c;
 161         int     i;
 162         char    *info = NULL;
 163
 164         emulator_app="baro";
 165         while ((c = getopt_long(argc, argv, "sdi:", options, NULL)) != -1) {
 166                 switch (c) {
 167                 case 's':
 168                         summary = 1;
 169                         break;
 170                 case 'd':
 171                         ao_flight_debug = 1;
 172                         break;
 173                 case 'i':
 174                         info = optarg;
 175                         break;
 176                 }
 177         }
 178
 179         if (optind == argc)
 180                 run_flight_fixed("<stdin>", stdin, summary, info);
 181         else
 182                 for (i = optind; i < argc; i++) {
 183                         FILE    *f = fopen(argv[i], "r");
 184                         if (!f) {
 185                                 perror(argv[i]);
 186                                 continue;
 187                         }
 188                         run_flight_fixed(argv[i], f, summary, info);
 189                         fclose(f);
 190                 }
 191         exit(0);
 192 }