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