git.gag.com Git - fw/altos/blob - ao-view/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(double 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(int column, 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(column, 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 static double
  90 aoview_time(void)
  91 {
  92         struct timespec now;
  93
  94         clock_gettime(CLOCK_MONOTONIC, &now);
  95         return (double) now.tv_sec + (double) now.tv_nsec / 1.0e9;
  96 }
  97
  98 /*
  99  * Fill out the derived data fields
 100  */
 101 static void
 102 aoview_state_derive(struct aodata *data, struct aostate *state)
 103 {
 104         int     i;
 105         double  new_height;
 106         double  height_change;
 107         double  time_change;
 108         int     tick_count;
 109
 110         state->report_time = aoview_time();
 111
 112         state->prev_data = state->data;
 113         state->prev_npad = state->npad;
 114         state->data = *data;
 115         tick_count = data->tick;
 116         if (tick_count < state->prev_data.tick)
 117                 tick_count += 65536;
 118         time_change = (tick_count - state->prev_data.tick) / 100.0;
 119
 120         state->ground_altitude = aoview_pres_to_altitude(data->ground_pres);
 121         new_height = aoview_pres_to_altitude(data->flight_pres) - state->ground_altitude;
 122         height_change = new_height - state->height;
 123         state->height = new_height;
 124         if (time_change)
 125                 state->baro_speed = (state->baro_speed * 3 + (height_change / time_change)) / 4.0;
 126         state->acceleration = (data->ground_accel - data->flight_accel) / 27.0;
 127         state->speed = data->flight_vel / 2700.0;
 128         state->temperature = ((data->temp / 32767.0 * 3.3) - 0.5) / 0.01;
 129         state->drogue_sense = data->drogue / 32767.0 * 15.0;
 130         state->main_sense = data->main / 32767.0 * 15.0;
 131         state->battery = data->batt / 32767.0 * 5.0;
 132         if (!strcmp(data->state, "pad")) {
 133                 if (data->gps.gps_locked && data->gps.nsat >= 4) {
 134                         state->npad++;
 135                         state->pad_lat_total += data->gps.lat;
 136                         state->pad_lon_total += data->gps.lon;
 137                         state->pad_alt_total += data->gps.alt;
 138                         if (state->npad > 1) {
 139                                 state->pad_lat = (state->pad_lat * 31 + data->gps.lat) / 32.0;
 140                                 state->pad_lon = (state->pad_lon * 31 + data->gps.lon) / 32.0;
 141                                 state->pad_alt = (state->pad_alt * 31 + data->gps.alt) / 32.0;
 142                         } else {
 143                                 state->pad_lat = data->gps.lat;
 144                                 state->pad_lon = data->gps.lon;
 145                                 state->pad_alt = data->gps.alt;
 146                         }
 147                 }
 148         }
 149         state->ascent = FALSE;
 150         for (i = 0; ascent_states[i]; i++)
 151                 if (!strcmp(data->state, ascent_states[i]))
 152                         state->ascent = TRUE;
 153
 154         /* Only look at accelerometer data on the way up */
 155         if (state->ascent && state->acceleration > state->max_acceleration)
 156                 state->max_acceleration = state->acceleration;
 157         if (state->ascent && state->speed > state->max_speed)
 158                 state->max_speed = state->speed;
 159
 160         if (state->height > state->max_height)
 161                 state->max_height = state->height;
 162         state->gps.gps_locked = data->gps.gps_locked;
 163         state->gps.gps_connected = data->gps.gps_connected;
 164         if (data->gps.gps_locked) {
 165                 state->gps = data->gps;
 166                 state->gps_valid = 1;
 167                 if (state->npad)
 168                         aoview_great_circle(state->pad_lat, state->pad_lon, state->gps.lat, state->gps.lon,
 169                                             &state->distance, &state->bearing);
 170         }
 171         if (data->gps_tracking.channels)
 172                 state->gps_tracking = data->gps_tracking;
 173         if (state->npad) {
 174                 state->gps_height = state->gps.alt - state->pad_alt;
 175         } else {
 176                 state->gps_height = 0;
 177         }
 178 }
 179
 180 void
 181 aoview_speak_state(struct aostate *state)
 182 {
 183         if (strcmp(state->data.state, state->prev_data.state)) {
 184                 aoview_voice_speak("%s\n", state->data.state);
 185                 if (!strcmp(state->data.state, "drogue"))
 186                         aoview_voice_speak("apogee %d meters\n",
 187                                            (int) state->max_height);
 188                 if (!strcmp(state->prev_data.state, "boost"))
 189                         aoview_voice_speak("max speed %d meters per second\n",
 190                                            (int) state->max_speed);
 191         }
 192         if (state->prev_npad < MIN_PAD_SAMPLES && state->npad >= MIN_PAD_SAMPLES)
 193                 aoview_voice_speak("g p s ready\n");
 194 }
 195
 196 void
 197 aoview_speak_height(struct aostate *state)
 198 {
 199         aoview_voice_speak("%d meters\n", state->height);
 200 }
 201
 202 struct aostate aostate;
 203
 204 static guint aostate_timeout;
 205
 206 #define COMPASS_LIMIT(n)        ((n * 22.5) + 22.5/2)
 207
 208 static char *compass_points[] = {
 209         "north",
 210         "north north east",
 211         "north east",
 212         "east north east",
 213         "east",
 214         "east south east",
 215         "south east",
 216         "south south east",
 217         "south",
 218         "south south west",
 219         "south west",
 220         "west south west",
 221         "west",
 222         "west north west",
 223         "north west",
 224         "north north west",
 225 };
 226
 227 static char *
 228 aoview_compass_point(double bearing)
 229 {
 230         int     i;
 231         while (bearing < 0)
 232                 bearing += 360.0;
 233         while (bearing >= 360.0)
 234                 bearing -= 360.0;
 235
 236         i = floor ((bearing - 22.5/2) / 22.5 + 0.5);
 237         if (i < 0) i = 0;
 238         if (i >= sizeof (compass_points) / sizeof (compass_points[0]))
 239                 i = 0;
 240         return compass_points[i];
 241 }
 242
 243 static gboolean
 244 aoview_state_timeout(gpointer data)
 245 {
 246         double  now = aoview_time();
 247
 248         if (strlen(aostate.data.state) > 0 && strcmp(aostate.data.state, "pad") != 0)
 249                 aoview_speak_height(&aostate);
 250         if (now - aostate.report_time >= 20 || !strcmp(aostate.data.state, "landed")) {
 251                 if (!aostate.ascent) {
 252                         if (fabs(aostate.baro_speed) < 20 && aostate.height < 100)
 253                                 aoview_voice_speak("rocket landed safely\n");
 254                         else
 255                                 aoview_voice_speak("rocket may have crashed\n");
 256                         if (aostate.gps_valid) {
 257                                 aoview_voice_speak("rocket reported %s of pad distance %d meters\n",
 258                                                    aoview_compass_point(aostate.bearing),
 259                                                    (int) aostate.distance);
 260                         }
 261                 }
 262                 aostate_timeout = 0;
 263                 return FALSE;
 264         }
 265         return TRUE;
 266 }
 267
 268 void
 269 aoview_state_reset(void)
 270 {
 271         memset(&aostate, '\0', sizeof (aostate));
 272 }
 273
 274 void
 275 aoview_state_notify(struct aodata *data)
 276 {
 277         struct aostate *state = &aostate;
 278         aoview_state_derive(data, state);
 279         aoview_table_start();
 280
 281         if (state->npad >= MIN_PAD_SAMPLES)
 282                 aoview_table_add_row(0, "Ground state", "ready");
 283         else
 284                 aoview_table_add_row(0, "Ground state", "waiting for gps (%d)",
 285                                      MIN_PAD_SAMPLES - state->npad);
 286         aoview_table_add_row(0, "Rocket state", "%s", state->data.state);
 287         aoview_table_add_row(0, "Callsign", "%s", state->data.callsign);
 288         aoview_table_add_row(0, "Rocket serial", "%d", state->data.serial);
 289
 290         aoview_table_add_row(0, "RSSI", "%6ddBm", state->data.rssi);
 291         aoview_table_add_row(0, "Height", "%6dm", state->height);
 292         aoview_table_add_row(0, "Max height", "%6dm", state->max_height);
 293         aoview_table_add_row(0, "Acceleration", "%7.1fm/s²", state->acceleration);
 294         aoview_table_add_row(0, "Max acceleration", "%7.1fm/s²", state->max_acceleration);
 295         aoview_table_add_row(0, "Speed", "%7.1fm/s", state->ascent ? state->speed : state->baro_speed);
 296         aoview_table_add_row(0, "Max Speed", "%7.1fm/s", state->max_speed);
 297         aoview_table_add_row(0, "Temperature", "%6.2f°C", state->temperature);
 298         aoview_table_add_row(0, "Battery", "%5.2fV", state->battery);
 299         aoview_table_add_row(0, "Drogue", "%5.2fV", state->drogue_sense);
 300         aoview_table_add_row(0, "Main", "%5.2fV", state->main_sense);
 301         aoview_table_add_row(0, "Pad altitude", "%dm", state->ground_altitude);
 302         aoview_table_add_row(1, "Satellites", "%d", state->gps.nsat);
 303         if (state->gps.gps_locked) {
 304                 aoview_table_add_row(1, "GPS", "locked");
 305         } else if (state->gps.gps_connected) {
 306                 aoview_table_add_row(1, "GPS", "unlocked");
 307         } else {
 308                 aoview_table_add_row(1, "GPS", "not available");
 309         }
 310         if (state->gps_valid) {
 311                 aoview_state_add_deg(1, "Latitude", state->gps.lat, 'N', 'S');
 312                 aoview_state_add_deg(1, "Longitude", state->gps.lon, 'E', 'W');
 313                 aoview_table_add_row(1, "GPS altitude", "%d", state->gps.alt);
 314                 aoview_table_add_row(1, "GPS height", "%d", state->gps_height);
 315                 aoview_table_add_row(1, "GPS time", "%02d:%02d:%02d",
 316                                      state->gps.gps_time.hour,
 317                                      state->gps.gps_time.minute,
 318                                      state->gps.gps_time.second);
 319         }
 320         if (state->gps.gps_extended) {
 321                 aoview_table_add_row(1, "GPS ground speed", "%7.1fm/s %d°",
 322                                      state->gps.ground_speed,
 323                                      state->gps.course);
 324                 aoview_table_add_row(1, "GPS climb rate", "%7.1fm/s",
 325                                      state->gps.climb_rate);
 326                 aoview_table_add_row(1, "GPS precision", "%4.1f(hdop) %3dm(h) %3dm(v)",
 327                                      state->gps.hdop, state->gps.h_error, state->gps.v_error);
 328         }
 329         if (state->npad) {
 330                 aoview_table_add_row(1, "Distance from pad", "%5.0fm", state->distance);
 331                 aoview_table_add_row(1, "Direction from pad", "%4.0f°", state->bearing);
 332                 aoview_state_add_deg(1, "Pad latitude", state->pad_lat, 'N', 'S');
 333                 aoview_state_add_deg(1, "Pad longitude", state->pad_lon, 'E', 'W');
 334                 aoview_table_add_row(1, "Pad GPS alt", "%gm", state->pad_alt);
 335         }
 336         if (state->gps.gps_connected) {
 337                 int     nsat_vis = 0;
 338                 int     nsat_locked = 0;
 339                 int     c;
 340
 341                 for (c = 0; c < state->gps_tracking.channels; c++) {
 342                         if ((state->gps_tracking.sats[c].state & 0xff) == 0xbf)
 343                                 nsat_locked++;
 344                 }
 345                 aoview_table_add_row(2, "Satellites Visible", "%d", state->gps_tracking.channels);
 346                 aoview_table_add_row(2, "Satellites Locked", "%d", nsat_locked);
 347                 for (c = 0; c < state->gps_tracking.channels; c++) {
 348                         aoview_table_add_row(2, "Satellite id,state,C/N0",
 349                                              "%3d,%02x,%2d%s",
 350                                              state->gps_tracking.sats[c].svid,
 351                                              state->gps_tracking.sats[c].state,
 352                                              state->gps_tracking.sats[c].c_n0,
 353                                              (state->gps_tracking.sats[c].state & 0xff) == 0xbf ?
 354                                              " LOCKED" : "");
 355                 }
 356         }
 357         aoview_table_finish();
 358         aoview_label_show(state);
 359         aoview_speak_state(state);
 360         if (!aostate_timeout && strcmp(state->data.state, "pad") != 0)
 361                 aostate_timeout = g_timeout_add_seconds(10, aoview_state_timeout, NULL);
 362 }
 363
 364 void
 365 aoview_state_new(void)
 366 {
 367 }
 368
 369 void
 370 aoview_state_init(GladeXML *xml)
 371 {
 372         aoview_state_new();
 373 }