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(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 (state->npad) {
 172                 state->gps_height = state->gps.alt - state->pad_alt;
 173         } else {
 174                 state->gps_height = 0;
 175         }
 176 }
 177
 178 void
 179 aoview_speak_state(struct aostate *state)
 180 {
 181         if (strcmp(state->data.state, state->prev_data.state)) {
 182                 aoview_voice_speak("%s\n", state->data.state);
 183                 if (!strcmp(state->data.state, "drogue"))
 184                         aoview_voice_speak("apogee %d meters\n",
 185                                            (int) state->max_height);
 186                 if (!strcmp(state->prev_data.state, "boost"))
 187                         aoview_voice_speak("max speed %d meters per second\n",
 188                                            (int) state->max_speed);
 189         }
 190         if (state->prev_npad < MIN_PAD_SAMPLES && state->npad >= MIN_PAD_SAMPLES)
 191                 aoview_voice_speak("g p s ready\n");
 192 }
 193
 194 void
 195 aoview_speak_height(struct aostate *state)
 196 {
 197         aoview_voice_speak("%d meters\n", state->height);
 198 }
 199
 200 struct aostate aostate;
 201
 202 static guint aostate_timeout;
 203
 204 #define COMPASS_LIMIT(n)        ((n * 22.5) + 22.5/2)
 205
 206 static char *compass_points[] = {
 207         "north",
 208         "north north east",
 209         "north east",
 210         "east north east",
 211         "east",
 212         "east south east",
 213         "south east",
 214         "south south east",
 215         "south",
 216         "south south west",
 217         "south west",
 218         "west south west",
 219         "west",
 220         "west north west",
 221         "north west",
 222         "north north west",
 223 };
 224
 225 static char *
 226 aoview_compass_point(double bearing)
 227 {
 228         int     i;
 229         while (bearing < 0)
 230                 bearing += 360.0;
 231         while (bearing >= 360.0)
 232                 bearing -= 360.0;
 233
 234         i = floor ((bearing - 22.5/2) / 22.5 + 0.5);
 235         if (i < 0) i = 0;
 236         if (i >= sizeof (compass_points) / sizeof (compass_points[0]))
 237                 i = 0;
 238         return compass_points[i];
 239 }
 240
 241 static gboolean
 242 aoview_state_timeout(gpointer data)
 243 {
 244         double  now = aoview_time();
 245
 246         if (strlen(aostate.data.state) > 0 && strcmp(aostate.data.state, "pad") != 0)
 247                 aoview_speak_height(&aostate);
 248         if (now - aostate.report_time >= 20 || !strcmp(aostate.data.state, "landed")) {
 249                 if (!aostate.ascent) {
 250                         if (fabs(aostate.baro_speed) < 20 && aostate.height < 100)
 251                                 aoview_voice_speak("rocket landed safely\n");
 252                         else
 253                                 aoview_voice_speak("rocket may have crashed\n");
 254                         if (aostate.gps_valid) {
 255                                 aoview_voice_speak("rocket reported %s of pad distance %d meters\n",
 256                                                    aoview_compass_point(aostate.bearing),
 257                                                    (int) aostate.distance);
 258                         }
 259                 }
 260                 aostate_timeout = 0;
 261                 return FALSE;
 262         }
 263         return TRUE;
 264 }
 265
 266 void
 267 aoview_state_reset(void)
 268 {
 269         memset(&aostate, '\0', sizeof (aostate));
 270 }
 271
 272 void
 273 aoview_state_notify(struct aodata *data)
 274 {
 275         struct aostate *state = &aostate;
 276         aoview_state_derive(data, state);
 277         aoview_table_start();
 278
 279         if (state->npad >= MIN_PAD_SAMPLES)
 280                 aoview_table_add_row(0, "Ground state", "ready");
 281         else
 282                 aoview_table_add_row(0, "Ground state", "waiting for gps (%d)",
 283                                      MIN_PAD_SAMPLES - state->npad);
 284         aoview_table_add_row(0, "Rocket state", "%s", state->data.state);
 285         aoview_table_add_row(0, "Callsign", "%s", state->data.callsign);
 286         aoview_table_add_row(0, "Rocket serial", "%d", state->data.serial);
 287
 288         aoview_table_add_row(0, "RSSI", "%6ddBm", state->data.rssi);
 289         aoview_table_add_row(0, "Height", "%6dm", state->height);
 290         aoview_table_add_row(0, "Max height", "%6dm", state->max_height);
 291         aoview_table_add_row(0, "Acceleration", "%7.1fm/s²", state->acceleration);
 292         aoview_table_add_row(0, "Max acceleration", "%7.1fm/s²", state->max_acceleration);
 293         aoview_table_add_row(0, "Speed", "%7.1fm/s", state->ascent ? state->speed : state->baro_speed);
 294         aoview_table_add_row(0, "Max Speed", "%7.1fm/s", state->max_speed);
 295         aoview_table_add_row(0, "Temperature", "%6.2f°C", state->temperature);
 296         aoview_table_add_row(0, "Battery", "%5.2fV", state->battery);
 297         aoview_table_add_row(0, "Drogue", "%5.2fV", state->drogue_sense);
 298         aoview_table_add_row(0, "Main", "%5.2fV", state->main_sense);
 299         aoview_table_add_row(0, "Pad altitude", "%dm", state->ground_altitude);
 300         aoview_table_add_row(1, "Satellites", "%d", state->gps.nsat);
 301         if (state->gps.gps_locked) {
 302                 aoview_table_add_row(1, "GPS", "locked");
 303         } else if (state->gps.gps_connected) {
 304                 aoview_table_add_row(1, "GPS", "unlocked");
 305         } else {
 306                 aoview_table_add_row(1, "GPS", "not available");
 307         }
 308         if (state->gps_valid) {
 309                 aoview_state_add_deg(1, "Latitude", state->gps.lat, 'N', 'S');
 310                 aoview_state_add_deg(1, "Longitude", state->gps.lon, 'E', 'W');
 311                 aoview_table_add_row(1, "GPS height", "%d", state->gps_height);
 312                 aoview_table_add_row(1, "GPS time", "%02d:%02d:%02d",
 313                                      state->gps.gps_time.hour,
 314                                      state->gps.gps_time.minute,
 315                                      state->gps.gps_time.second);
 316         }
 317         if (state->gps.gps_extended) {
 318                 aoview_table_add_row(1, "GPS ground speed", "%7.1fm/s %d°",
 319                                      state->gps.ground_speed,
 320                                      state->gps.course);
 321                 aoview_table_add_row(1, "GPS climb rate", "%7.1fm/s",
 322                                      state->gps.climb_rate);
 323                 aoview_table_add_row(1, "GPS precision", "%4.1f(hdop) %3dm(h) %3dm(v)",
 324                                      state->gps.hdop, state->gps.h_error, state->gps.v_error);
 325         }
 326         if (state->npad) {
 327                 aoview_table_add_row(1, "Distance from pad", "%5.0fm", state->distance);
 328                 aoview_table_add_row(1, "Direction from pad", "%4.0f°", state->bearing);
 329                 aoview_state_add_deg(1, "Pad latitude", state->pad_lat, 'N', 'S');
 330                 aoview_state_add_deg(1, "Pad longitude", state->pad_lon, 'E', 'W');
 331                 aoview_table_add_row(1, "Pad GPS alt", "%gm", state->pad_alt);
 332         }
 333         aoview_table_finish();
 334         aoview_label_show(state);
 335         aoview_speak_state(state);
 336         if (!aostate_timeout && strcmp(state->data.state, "pad") != 0)
 337                 aostate_timeout = g_timeout_add_seconds(10, aoview_state_timeout, NULL);
 338 }
 339
 340 void
 341 aoview_state_new(void)
 342 {
 343 }
 344
 345 void
 346 aoview_state_init(GladeXML *xml)
 347 {
 348         aoview_state_new();
 349 }