git.gag.com Git - fw/altos/blob - ao-tools/ao-fakeflight/ao-fake-log.c

   1 /*
   2  * Copyright © 2014 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 "ao-fakeflight.h"
  19
  20 static uint8_t
  21 ao_log_csum(uint8_t *b)
  22 {
  23         uint8_t sum = 0x5a;
  24         uint8_t i;
  25
  26         for (i = 0; i < sizeof (struct ao_log_metrum); i++)
  27                 sum += *b++;
  28         return -sum;
  29 }
  30
  31 void
  32 ao_log_metrum(FILE *file, struct ao_log_metrum *log)
  33 {
  34         uint8_t *d;
  35
  36         /* set checksum */
  37         log->csum = 0;
  38         log->csum = ao_log_csum((uint8_t *) log);
  39         int i;
  40
  41         fprintf (file, "%c %04x", log->type, log->tick);
  42         d = (void *) &log->u;
  43         for (i = 0; i < sizeof (log->u); i++)
  44                 fprintf(file, " %02x", d[i]);
  45         fprintf (file, "\n");
  46 }
  47
  48 void
  49 ao_log_flight(FILE *file, struct flight *f, struct rocket *r)
  50 {
  51         static struct ao_log_metrum     log;
  52
  53         log.type = AO_LOG_FLIGHT;
  54         log.tick = (f->time * 100);
  55         log.u.flight.ground_accel = ao_accel_unconvert(GRAVITY);
  56         log.u.flight.ground_pres = cc_altitude_to_pressure(f->altitude);
  57         log.u.flight.flight = 1;
  58         ao_log_metrum(file, &log);
  59 }
  60
  61 void
  62 ao_log_sensor(FILE *file, struct flight *f, struct rocket *r)
  63 {
  64         static struct ao_log_metrum     log;
  65         struct ao_ms5607_sample         sample_baro;
  66         int16_t                         sample_accel;
  67
  68         ao_ms5607_unconvert(f->altitude, &sample_baro);
  69         sample_accel = ao_accel_unconvert(f->accel);
  70
  71         log.type = AO_LOG_SENSOR;
  72         log.tick = (f->time * 100);
  73         log.u.sensor.pres = sample_baro.pres;
  74         log.u.sensor.temp = sample_baro.temp;
  75         log.u.sensor.accel = sample_accel;
  76         ao_log_metrum(file, &log);
  77 }
  78
  79 void
  80 ao_log_gps(FILE *file, struct flight *f, struct rocket *r)
  81 {
  82         static struct ao_log_metrum     log;
  83         static double                   gps_time;
  84         static int                      been_here;
  85         int32_t                         altitude;
  86         int32_t                         seconds;
  87
  88         if (been_here && (f->time - gps_time) < 1)
  89                 return;
  90         been_here = 1;
  91         gps_time = f->time;
  92
  93         altitude = f->altitude;
  94         seconds = f->time;
  95
  96         log.type = AO_LOG_GPS_POS;
  97         log.tick = (f->time * 100);
  98         log.u.gps.latitude = 45 * 1e7;
  99         log.u.gps.longitude = -121 * 1e7;
 100         log.u.gps.altitude_low = altitude;
 101         log.u.gps.altitude_high = altitude >> 16;
 102         ao_log_metrum(file, &log);
 103
 104         log.type = AO_LOG_GPS_TIME;
 105         log.tick = (f->time * 100);
 106         log.u.gps_time.hour = seconds / 3600;
 107         log.u.gps_time.minute = (seconds / 60) % 60;
 108         log.u.gps_time.second = seconds % 60;
 109         log.u.gps_time.flags = AO_GPS_VALID | AO_GPS_RUNNING | AO_GPS_DATE_VALID | AO_GPS_COURSE_VALID | (12 << AO_GPS_NUM_SAT_SHIFT);
 110         log.u.gps_time.year = 114;
 111         log.u.gps_time.month = 7;
 112         log.u.gps_time.day = 1;
 113         log.u.gps_time.pdop = 1;
 114
 115         ao_log_metrum(file, &log);
 116 }
 117
 118 void
 119 ao_log_state(FILE *file, struct flight *f, struct rocket *r)
 120 {
 121         static struct ao_log_metrum     log;
 122         static int                      log_state = ao_flight_startup;
 123         int                             flight_state;
 124
 125         switch (f->state) {
 126         case state_ascent:
 127                 if (f->speed == 0 && f->altitude == 0)
 128                         flight_state = ao_flight_pad;
 129                 else if (f->accel > 0)
 130                         flight_state = ao_flight_boost;
 131                 else if (f->speed > 300)
 132                         flight_state = ao_flight_fast;
 133                 else
 134                         flight_state = ao_flight_coast;
 135                 break;
 136         case state_drogue:
 137                 flight_state = ao_flight_drogue;
 138                 break;
 139         case state_main:
 140                 flight_state = ao_flight_main;
 141                 break;
 142         case state_landed:
 143                 flight_state = ao_flight_landed;
 144                 break;
 145         }
 146         if (flight_state != log_state) {
 147                 log.type = AO_LOG_STATE;
 148                 log.tick = (f->time * 100);
 149                 log.u.state.state = flight_state;
 150                 log.u.state.reason = 0;
 151                 ao_log_metrum(file, &log);
 152                 log_state = flight_state;
 153         }
 154 }
 155
 156 void
 157 ao_log(FILE *file, struct flight *f, struct rocket *r)
 158 {
 159         static int      been_here = 0;
 160
 161         if (!been_here) {
 162                 been_here = 1;
 163                 ao_log_flight(file, f, r);
 164         }
 165         ao_log_sensor(file, f, r);
 166         ao_log_state(file, f, r);
 167         ao_log_gps(file, f, r);
 168 }