git.gag.com Git - fw/altos/blob - aoview/aoview_state.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 "aoview.h"
  19 #include <math.h>
  20
  21 static inline double sqr(a) { return a * a; };
  22
  23 static void
  24 aoview_great_circle (double start_lat, double start_lon,
  25                      double end_lat, double end_lon,
  26                      double *dist, double *bearing)
  27 {
  28         const double rad = M_PI / 180;
  29         const double earth_radius = 6371.2 * 1000;      /* in meters */
  30         double lat1 = rad * start_lat;
  31         double lon1 = rad * -start_lon;
  32         double lat2 = rad * end_lat;
  33         double lon2 = rad * -end_lon;
  34
  35         double d_lat = lat2 - lat1;
  36         double d_lon = lon2 - lon1;
  37
  38         /* From http://en.wikipedia.org/wiki/Great-circle_distance */
  39         double vdn = sqrt(sqr(cos(lat2) * sin(d_lon)) +
  40                           sqr(cos(lat1) * sin(lat2) -
  41                               sin(lat1) * cos(lat2) * cos(d_lon)));
  42         double vdd = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(d_lon);
  43         double d = atan2(vdn,vdd);
  44         double course;
  45
  46         if (cos(lat1) < 1e-20) {
  47                 if (lat1 > 0)
  48                         course = M_PI;
  49                 else
  50                         course = -M_PI;
  51         } else {
  52                 if (d < 1e-10)
  53                         course = 0;
  54                 else
  55                         course = acos((sin(lat2)-sin(lat1)*cos(d)) /
  56                                       (sin(d)*cos(lat1)));
  57                 if (sin(lon2-lon1) > 0)
  58                         course = 2 * M_PI-course;
  59         }
  60         *dist = d * earth_radius;
  61         *bearing = course * 180/M_PI;
  62 }
  63
  64 static void
  65 aoview_state_add_deg(char *label, double deg, char pos, char neg)
  66 {
  67         double  int_part;
  68         double  min;
  69         char    sign = pos;
  70
  71         if (deg < 0) {
  72                 deg = -deg;
  73                 sign = neg;
  74         }
  75         int_part = floor (deg);
  76         min = (deg - int_part) * 60.0;
  77         aoview_table_add_row(label, "%d°%lf'%c",
  78                              (int) int_part, min, sign);
  79
  80 }
  81
  82 static char *ascent_states[] = {
  83         "boost",
  84         "fast",
  85         "coast",
  86         0,
  87 };
  88
  89 /*
  90  * Fill out the derived data fields
  91  */
  92 static void
  93 aoview_state_derive(struct aostate *state)
  94 {
  95         int     i;
  96
  97         state->ground_altitude = aoview_pres_to_altitude(state->ground_pres);
  98         state->height = aoview_pres_to_altitude(state->flight_pres) - state->ground_altitude;
  99         state->acceleration = (state->ground_accel - state->flight_accel) / 27.0;
 100         state->speed = state->flight_vel / 2700.0;
 101         state->temperature = ((state->temp / 32767.0 * 3.3) - 0.5) / 0.01;
 102         state->drogue_sense = state->drogue / 32767.0 * 15.0;
 103         state->main_sense = state->main / 32767.0 * 15.0;
 104         state->battery = state->batt / 32767.0 * 5.0;
 105         if (!strcmp(state->state, "pad")) {
 106                 if (state->locked) {
 107                         state->npad++;
 108                         state->pad_lat_total += state->lat;
 109                         state->pad_lon_total += state->lon;
 110                         state->pad_alt_total += state->alt;
 111                         state->pad_lat = state->pad_lat_total / state->npad;
 112                         state->pad_lon = state->pad_lon_total / state->npad;
 113                         state->pad_alt = state->pad_alt_total / state->npad;
 114                 }
 115         }
 116         state->ascent = FALSE;
 117         for (i = 0; ascent_states[i]; i++)
 118                 if (!strcmp(state->state, ascent_states[i]))
 119                         state->ascent = TRUE;
 120
 121         /* Only look at accelerometer data on the way up */
 122         if (state->ascent && state->acceleration > state->max_acceleration)
 123                 state->max_acceleration = state->acceleration;
 124         if (state->ascent && state->speed > state->max_speed)
 125                 state->max_speed = state->speed;
 126
 127         if (state->height > state->max_height)
 128                 state->max_height = state->height;
 129         aoview_great_circle(state->pad_lat, state->pad_lon, state->lat, state->lon,
 130                             &state->distance, &state->bearing);
 131 }
 132
 133 void
 134 aoview_state_speak(struct aostate *state)
 135 {
 136         static char     last_state[32];
 137         int             i;
 138         gboolean        report = FALSE;
 139         int             this_tick;
 140         static int      last_tick;
 141         static int      last_altitude;
 142         int             this_altitude;
 143
 144         if (strcmp(state->state, last_state)) {
 145                 aoview_voice_speak("%s\n", state->state);
 146                 if (!strcmp(state->state, "drogue"))
 147                         aoview_voice_speak("apogee %d meters\n",
 148                                            (int) state->max_height);
 149                 report = TRUE;
 150                 strcpy(last_state, state->state);
 151         }
 152         this_altitude = aoview_pres_to_altitude(state->flight_pres) - aoview_pres_to_altitude(state->ground_pres);
 153         this_tick = state->tick;
 154         while (this_tick < last_tick)
 155                 this_tick += 65536;
 156         if (strcmp(state->state, "pad") != 0) {
 157                 if (this_altitude / 1000 != last_altitude / 1000)
 158                         report = TRUE;
 159                 if (this_tick - last_tick >= 10 * 100)
 160                         report = TRUE;
 161         }
 162         if (report) {
 163                 aoview_voice_speak("%d meters\n",
 164                                    this_altitude);
 165                 if (state->ascent)
 166                         aoview_voice_speak("%d meters per second\n",
 167                                            state->flight_vel / 2700);
 168                 last_tick = state->tick;
 169                 last_altitude = this_altitude;
 170                 printf ("report at tick %d height %d\n",
 171                         state->tick, this_altitude);
 172         }
 173 }
 174
 175 void
 176 aoview_state_notify(struct aostate *state)
 177 {
 178         aoview_state_derive(state);
 179         aoview_table_start();
 180
 181         if (state->npad >= MIN_PAD_SAMPLES)
 182                 aoview_table_add_row("Ground state", "ready");
 183         else
 184                 aoview_table_add_row("Ground state", "waiting for gps (%d)",
 185                                      MIN_PAD_SAMPLES - state->npad);
 186         aoview_table_add_row("Rocket state", "%s", state->state);
 187         aoview_table_add_row("Callsign", "%s", state->callsign);
 188         aoview_table_add_row("Rocket serial", "%d", state->serial);
 189
 190         aoview_table_add_row("RSSI", "%ddBm", state->rssi);
 191         aoview_table_add_row("Height", "%dm", state->height);
 192         aoview_table_add_row("Max height", "%dm", state->max_height);
 193         aoview_table_add_row("Acceleration", "%gm/s²", state->acceleration);
 194         aoview_table_add_row("Max acceleration", "%gm/s²", state->max_acceleration);
 195         aoview_table_add_row("Speed", "%gm/s", state->speed);
 196         aoview_table_add_row("Max Speed", "%gm/s", state->max_speed);
 197         aoview_table_add_row("Temperature", "%g°C", state->temperature);
 198         aoview_table_add_row("Battery", "%gV", state->battery);
 199         aoview_table_add_row("Drogue", "%gV", state->drogue_sense);
 200         aoview_table_add_row("Main", "%gV", state->main_sense);
 201         aoview_table_add_row("Pad altitude", "%dm", state->ground_altitude);
 202         aoview_table_add_row("Satellites", "%d", state->nsat);
 203         if (state->locked) {
 204                 aoview_state_add_deg("Latitude", state->lat, 'N', 'S');
 205                 aoview_state_add_deg("Longitude", state->lon, 'E', 'W');
 206                 aoview_table_add_row("GPS alt", "%d", state->alt);
 207                 aoview_table_add_row("GPS time", "%02d:%02d:%02d",
 208                                      state->gps_time.hour,
 209                                      state->gps_time.minute,
 210                                      state->gps_time.second);
 211                 aoview_table_add_row("GPS ground speed", "%fm/s %d°",
 212                                      state->ground_speed,
 213                                      state->course);
 214                 aoview_table_add_row("GPS climb rate", "%fm/s",
 215                                      state->climb_rate);
 216                 aoview_table_add_row("GPS precision", "%f(hdop) %dm(h) %dm(v)\n",
 217                                      state->hdop, state->h_error, state->v_error);
 218                 aoview_table_add_row("Distance from pad", "%gm", state->distance);
 219                 aoview_table_add_row("Direction from pad", "%g°", state->bearing);
 220         } else {
 221                 aoview_table_add_row("GPS", "unlocked");
 222         }
 223         if (state->npad) {
 224                 aoview_state_add_deg("Pad latitude", state->pad_lat, 'N', 'S');
 225                 aoview_state_add_deg("Pad longitude", state->pad_lon, 'E', 'W');
 226                 aoview_table_add_row("Pad GPS alt", "%gm", state->pad_alt);
 227         }
 228         aoview_table_finish();
 229         aoview_label_show(state);
 230         aoview_state_speak(state);
 231 }
 232
 233 void
 234 aoview_state_new(void)
 235 {
 236 }
 237
 238 void
 239 aoview_state_init(GladeXML *xml)
 240 {
 241         aoview_state_new();
 242         aoview_voice_speak("initializing rocket flight monitoring system\n");
 243 }