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