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; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #include "aoview.h"
  20 #include <math.h>
  21
  22 static inline double sqr(double a) { return a * a; };
  23
  24 static void
  25 aoview_great_circle (double start_lat, double start_lon,
  26                      double end_lat, double end_lon,
  27                      double *dist, double *bearing)
  28 {
  29         const double rad = M_PI / 180;
  30         const double earth_radius = 6371.2 * 1000;      /* in meters */
  31         double lat1 = rad * start_lat;
  32         double lon1 = rad * -start_lon;
  33         double lat2 = rad * end_lat;
  34         double lon2 = rad * -end_lon;
  35
  36         double d_lat = lat2 - lat1;
  37         double d_lon = lon2 - lon1;
  38
  39         /* From http://en.wikipedia.org/wiki/Great-circle_distance */
  40         double vdn = sqrt(sqr(cos(lat2) * sin(d_lon)) +
  41                           sqr(cos(lat1) * sin(lat2) -
  42                               sin(lat1) * cos(lat2) * cos(d_lon)));
  43         double vdd = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(d_lon);
  44         double d = atan2(vdn,vdd);
  45         double course;
  46
  47         if (cos(lat1) < 1e-20) {
  48                 if (lat1 > 0)
  49                         course = M_PI;
  50                 else
  51                         course = -M_PI;
  52         } else {
  53                 if (d < 1e-10)
  54                         course = 0;
  55                 else
  56                         course = acos((sin(lat2)-sin(lat1)*cos(d)) /
  57                                       (sin(d)*cos(lat1)));
  58                 if (sin(lon2-lon1) > 0)
  59                         course = 2 * M_PI-course;
  60         }
  61         *dist = d * earth_radius;
  62         *bearing = course * 180/M_PI;
  63 }
  64
  65 static void
  66 aoview_state_add_deg(int column, char *label, double deg, char pos, char neg)
  67 {
  68         double  int_part;
  69         double  min;
  70         char    sign = pos;
  71
  72         if (deg < 0) {
  73                 deg = -deg;
  74                 sign = neg;
  75         }
  76         int_part = floor (deg);
  77         min = (deg - int_part) * 60.0;
  78         aoview_table_add_row(column, label, "%d°%lf'%c",
  79                              (int) int_part, min, sign);
  80
  81 }
  82
  83 static char *ascent_states[] = {
  84         "boost",
  85         "fast",
  86         "coast",
  87         0,
  88 };
  89
  90 static double
  91 aoview_time(void)
  92 {
  93         struct timespec now;
  94
  95         clock_gettime(CLOCK_MONOTONIC, &now);
  96         return (double) now.tv_sec + (double) now.tv_nsec / 1.0e9;
  97 }
  98
  99 /*
 100  * Fill out the derived data fields
 101  */
 102 static void
 103 aoview_state_derive(struct cc_telem *data, struct aostate *state)
 104 {
 105         int     i;
 106         double  new_height;
 107         double  height_change;
 108         double  time_change;
 109         double  accel_counts_per_mss;
 110         int     tick_count;
 111
 112         state->report_time = aoview_time();
 113
 114         state->prev_data = state->data;
 115         state->prev_npad = state->npad;
 116         state->data = *data;
 117         tick_count = data->tick;
 118         if (tick_count < state->prev_data.tick)
 119                 tick_count += 65536;
 120         time_change = (tick_count - state->prev_data.tick) / 100.0;
 121
 122         state->ground_altitude = aoview_pres_to_altitude(data->ground_pres);
 123         new_height = aoview_pres_to_altitude(data->flight_pres) - state->ground_altitude;
 124         height_change = new_height - state->height;
 125         state->height = new_height;
 126         if (time_change)
 127                 state->baro_speed = (state->baro_speed * 3 + (height_change / time_change)) / 4.0;
 128         accel_counts_per_mss = ((data->accel_minus_g - data->accel_plus_g) / 2.0) / 9.80665;
 129         state->acceleration = (data->ground_accel - data->flight_accel) / accel_counts_per_mss;
 130         state->speed = data->flight_vel / (accel_counts_per_mss * 100.0);
 131         state->temperature = cc_thermometer_to_temperature(data->temp);
 132         state->drogue_sense = cc_ignitor_to_voltage(data->drogue);
 133         state->main_sense = cc_ignitor_to_voltage(data->main);
 134         state->battery = cc_battery_to_voltage(data->batt);
 135         if (!strcmp(data->state, "pad")) {
 136                 if (data->gps.gps_locked && data->gps.nsat >= 4) {
 137                         state->npad++;
 138                         state->pad_lat_total += data->gps.lat;
 139                         state->pad_lon_total += data->gps.lon;
 140                         state->pad_alt_total += data->gps.alt;
 141                         if (state->npad > 1) {
 142                                 state->pad_lat = (state->pad_lat * 31 + data->gps.lat) / 32.0;
 143                                 state->pad_lon = (state->pad_lon * 31 + data->gps.lon) / 32.0;
 144                                 state->pad_alt = (state->pad_alt * 31 + data->gps.alt) / 32.0;
 145                         } else {
 146                                 state->pad_lat = data->gps.lat;
 147                                 state->pad_lon = data->gps.lon;
 148                                 state->pad_alt = data->gps.alt;
 149                         }
 150                 }
 151         }
 152         state->ascent = FALSE;
 153         for (i = 0; ascent_states[i]; i++)
 154                 if (!strcmp(data->state, ascent_states[i]))
 155                         state->ascent = TRUE;
 156
 157         /* Only look at accelerometer data on the way up */
 158         if (state->ascent && state->acceleration > state->max_acceleration)
 159                 state->max_acceleration = state->acceleration;
 160         if (state->ascent && state->speed > state->max_speed)
 161                 state->max_speed = state->speed;
 162
 163         if (state->height > state->max_height)
 164                 state->max_height = state->height;
 165         state->gps.gps_locked = data->gps.gps_locked;
 166         state->gps.gps_connected = data->gps.gps_connected;
 167         if (data->gps.gps_locked) {
 168                 state->gps = data->gps;
 169                 state->gps_valid = 1;
 170                 if (state->npad)
 171                         aoview_great_circle(state->pad_lat, state->pad_lon, state->gps.lat, state->gps.lon,
 172                                             &state->distance, &state->bearing);
 173         }
 174         if (data->gps_tracking.channels)
 175                 state->gps_tracking = data->gps_tracking;
 176         if (state->npad) {
 177                 state->gps_height = state->gps.alt - state->pad_alt;
 178         } else {
 179                 state->gps_height = 0;
 180         }
 181 }
 182
 183 void
 184 aoview_speak_state(struct aostate *state)
 185 {
 186         if (strcmp(state->data.state, state->prev_data.state)) {
 187                 aoview_voice_speak("%s\n", state->data.state);
 188                 if (!strcmp(state->data.state, "drogue"))
 189                         aoview_voice_speak("apogee %d meters\n",
 190                                            (int) state->max_height);
 191                 if (!strcmp(state->prev_data.state, "boost"))
 192                         aoview_voice_speak("max speed %d meters per second\n",
 193                                            (int) state->max_speed);
 194         }
 195         if (state->prev_npad < MIN_PAD_SAMPLES && state->npad >= MIN_PAD_SAMPLES)
 196                 aoview_voice_speak("g p s ready\n");
 197 }
 198
 199 void
 200 aoview_speak_height(struct aostate *state)
 201 {
 202         aoview_voice_speak("%d meters\n", state->height);
 203 }
 204
 205 struct aostate aostate;
 206
 207 static guint aostate_timeout;
 208
 209 #define COMPASS_LIMIT(n)        ((n * 22.5) + 22.5/2)
 210
 211 static char *compass_points[] = {
 212         "north",
 213         "north north east",
 214         "north east",
 215         "east north east",
 216         "east",
 217         "east south east",
 218         "south east",
 219         "south south east",
 220         "south",
 221         "south south west",
 222         "south west",
 223         "west south west",
 224         "west",
 225         "west north west",
 226         "north west",
 227         "north north west",
 228 };
 229
 230 static char *
 231 aoview_compass_point(double bearing)
 232 {
 233         int     i;
 234         while (bearing < 0)
 235                 bearing += 360.0;
 236         while (bearing >= 360.0)
 237                 bearing -= 360.0;
 238
 239         i = floor ((bearing - 22.5/2) / 22.5 + 0.5);
 240         if (i < 0) i = 0;
 241         if (i >= sizeof (compass_points) / sizeof (compass_points[0]))
 242                 i = 0;
 243         return compass_points[i];
 244 }
 245
 246 static gboolean
 247 aoview_state_timeout(gpointer data)
 248 {
 249         double  now = aoview_time();
 250
 251         if (strlen(aostate.data.state) > 0 && strcmp(aostate.data.state, "pad") != 0)
 252                 aoview_speak_height(&aostate);
 253         if (now - aostate.report_time >= 20 || !strcmp(aostate.data.state, "landed")) {
 254                 if (!aostate.ascent) {
 255                         if (fabs(aostate.baro_speed) < 20 && aostate.height < 100)
 256                                 aoview_voice_speak("rocket landed safely\n");
 257                         else
 258                                 aoview_voice_speak("rocket may have crashed\n");
 259                         if (aostate.gps_valid) {
 260                                 aoview_voice_speak("rocket reported %s of pad distance %d meters\n",
 261                                                    aoview_compass_point(aostate.bearing),
 262                                                    (int) aostate.distance);
 263                         }
 264                 }
 265                 aostate_timeout = 0;
 266                 return FALSE;
 267         }
 268         return TRUE;
 269 }
 270
 271 void
 272 aoview_state_reset(void)
 273 {
 274         memset(&aostate, '\0', sizeof (aostate));
 275 }
 276
 277 void
 278 aoview_state_notify(struct cc_telem *data)
 279 {
 280         struct aostate *state = &aostate;
 281         aoview_state_derive(data, state);
 282         aoview_table_start();
 283
 284         if (state->npad >= MIN_PAD_SAMPLES)
 285                 aoview_table_add_row(0, "Ground state", "ready");
 286         else
 287                 aoview_table_add_row(0, "Ground state", "waiting for gps (%d)",
 288                                      MIN_PAD_SAMPLES - state->npad);
 289         aoview_table_add_row(0, "Rocket state", "%s", state->data.state);
 290         aoview_table_add_row(0, "Callsign", "%s", state->data.callsign);
 291         aoview_table_add_row(0, "Rocket serial", "%d", state->data.serial);
 292         aoview_table_add_row(0, "Rocket flight", "%d", state->data.flight);
 293
 294         aoview_table_add_row(0, "RSSI", "%6ddBm", state->data.rssi);
 295         aoview_table_add_row(0, "Height", "%6dm", state->height);
 296         aoview_table_add_row(0, "Max height", "%6dm", state->max_height);
 297         aoview_table_add_row(0, "Acceleration", "%7.1fm/s²", state->acceleration);
 298         aoview_table_add_row(0, "Max acceleration", "%7.1fm/s²", state->max_acceleration);
 299         aoview_table_add_row(0, "Speed", "%7.1fm/s", state->ascent ? state->speed : state->baro_speed);
 300         aoview_table_add_row(0, "Max Speed", "%7.1fm/s", state->max_speed);
 301         aoview_table_add_row(0, "Temperature", "%6.2f°C", state->temperature);
 302         aoview_table_add_row(0, "Battery", "%5.2fV", state->battery);
 303         aoview_table_add_row(0, "Drogue", "%5.2fV", state->drogue_sense);
 304         aoview_table_add_row(0, "Main", "%5.2fV", state->main_sense);
 305         aoview_table_add_row(0, "Pad altitude", "%dm", state->ground_altitude);
 306         aoview_table_add_row(1, "Satellites", "%d", state->gps.nsat);
 307         if (state->gps.gps_locked) {
 308                 aoview_table_add_row(1, "GPS", "locked");
 309         } else if (state->gps.gps_connected) {
 310                 aoview_table_add_row(1, "GPS", "unlocked");
 311         } else {
 312                 aoview_table_add_row(1, "GPS", "not available");
 313         }
 314         if (state->gps_valid) {
 315                 aoview_state_add_deg(1, "Latitude", state->gps.lat, 'N', 'S');
 316                 aoview_state_add_deg(1, "Longitude", state->gps.lon, 'E', 'W');
 317                 aoview_table_add_row(1, "GPS altitude", "%d", state->gps.alt);
 318                 aoview_table_add_row(1, "GPS height", "%d", state->gps_height);
 319                 aoview_table_add_row(1, "GPS date", "%04d-%02d-%02d",
 320                                      state->gps.gps_time.year,
 321                                      state->gps.gps_time.month,
 322                                      state->gps.gps_time.day);
 323                 aoview_table_add_row(1, "GPS time", "%02d:%02d:%02d",
 324                                      state->gps.gps_time.hour,
 325                                      state->gps.gps_time.minute,
 326                                      state->gps.gps_time.second);
 327         }
 328         if (state->gps.gps_extended) {
 329                 aoview_table_add_row(1, "GPS ground speed", "%7.1fm/s %d°",
 330                                      state->gps.ground_speed,
 331                                      state->gps.course);
 332                 aoview_table_add_row(1, "GPS climb rate", "%7.1fm/s",
 333                                      state->gps.climb_rate);
 334                 aoview_table_add_row(1, "GPS precision", "%4.1f(hdop) %3dm(h) %3dm(v)",
 335                                      state->gps.hdop, state->gps.h_error, state->gps.v_error);
 336         }
 337         if (state->npad) {
 338                 aoview_table_add_row(1, "Distance from pad", "%5.0fm", state->distance);
 339                 aoview_table_add_row(1, "Direction from pad", "%4.0f°", state->bearing);
 340                 aoview_state_add_deg(1, "Pad latitude", state->pad_lat, 'N', 'S');
 341                 aoview_state_add_deg(1, "Pad longitude", state->pad_lon, 'E', 'W');
 342                 aoview_table_add_row(1, "Pad GPS alt", "%gm", state->pad_alt);
 343         }
 344         if (state->gps.gps_connected) {
 345                 int     nsat_vis = 0;
 346                 int     c;
 347
 348                 aoview_table_add_row(2, "Satellites Visible", "%d", state->gps_tracking.channels);
 349                 for (c = 0; c < state->gps_tracking.channels; c++) {
 350                         aoview_table_add_row(2, "Satellite id,C/N0",
 351                                              "%3d,%2d",
 352                                              state->gps_tracking.sats[c].svid,
 353                                              state->gps_tracking.sats[c].c_n0);
 354                 }
 355         }
 356         aoview_table_finish();
 357         aoview_label_show(state);
 358         aoview_speak_state(state);
 359         if (!aostate_timeout && strcmp(state->data.state, "pad") != 0)
 360                 aostate_timeout = g_timeout_add_seconds(10, aoview_state_timeout, NULL);
 361 }
 362
 363 void
 364 aoview_state_new(void)
 365 {
 366 }
 367
 368 void
 369 aoview_state_init(GladeXML *xml)
 370 {
 371         aoview_state_new();
 372 }