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