git.gag.com Git - fw/altos/blob - altoslib/AltosIdleMonitor.java

   1 /*
   2  * Copyright © 2010 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 package org.altusmetrum.AltosLib;
  19
  20 import java.io.*;
  21 import java.util.*;
  22 import java.text.*;
  23 import java.util.prefs.*;
  24 import java.util.concurrent.*;
  25
  26 class AltosSensorTM {
  27         int     tick;
  28         int     accel;
  29         int     pres;
  30         int     temp;
  31         int     batt;
  32         int     drogue;
  33         int     main;
  34
  35         public AltosSensorTM(AltosLink link) throws InterruptedException, TimeoutException {
  36                 link.printf("a\n");
  37                 for (;;) {
  38                         String line = link.get_reply_no_dialog(5000);
  39                         if (line == null) {
  40                                 throw new TimeoutException();
  41                         }
  42                         if (!line.startsWith("tick:"))
  43                                 continue;
  44                         String[] items = line.split("\\s+");
  45                         for (int i = 0; i < items.length;) {
  46                                 if (items[i].equals("tick:")) {
  47                                         tick = Integer.parseInt(items[i+1]);
  48                                         i += 2;
  49                                         continue;
  50                                 }
  51                                 if (items[i].equals("accel:")) {
  52                                         accel = Integer.parseInt(items[i+1]);
  53                                         i += 2;
  54                                         continue;
  55                                 }
  56                                 if (items[i].equals("pres:")) {
  57                                         pres = Integer.parseInt(items[i+1]);
  58                                         i += 2;
  59                                         continue;
  60                                 }
  61                                 if (items[i].equals("temp:")) {
  62                                         temp = Integer.parseInt(items[i+1]);
  63                                         i += 2;
  64                                         continue;
  65                                 }
  66                                 if (items[i].equals("batt:")) {
  67                                         batt = Integer.parseInt(items[i+1]);
  68                                         i += 2;
  69                                         continue;
  70                                 }
  71                                 if (items[i].equals("drogue:")) {
  72                                         drogue = Integer.parseInt(items[i+1]);
  73                                         i += 2;
  74                                         continue;
  75                                 }
  76                                 if (items[i].equals("main:")) {
  77                                         main = Integer.parseInt(items[i+1]);
  78                                         i += 2;
  79                                         continue;
  80                                 }
  81                                 i++;
  82                         }
  83                         break;
  84                 }
  85         }
  86 }
  87
  88 class AltosSensorMM {
  89         int             tick;
  90         int             sense[];
  91         int             v_batt;
  92         int             v_pyro;
  93         int             accel;
  94         int             accel_ref;
  95
  96         public AltosSensorMM(AltosLink link) throws InterruptedException, TimeoutException {
  97                 link.printf("a\n");
  98                 for (;;) {
  99                         String line = link.get_reply_no_dialog(5000);
 100                         if (line == null) {
 101                                 throw new TimeoutException();
 102                         }
 103                         if (!line.startsWith("tick:"))
 104                                 continue;
 105                         String[] items = line.split("\\s+");
 106                         sense = new int[6];
 107                         for (int i = 0; i < items.length;) {
 108                                 if (items[i].equals("tick:")) {
 109                                         tick = Integer.parseInt(items[i+1]);
 110                                         i += 2;
 111                                         continue;
 112                                 }
 113                                 if (items[i].equals("0:")) {
 114                                         sense[0] = Integer.parseInt(items[i+1]);
 115                                         i += 2;
 116                                         continue;
 117                                 }
 118                                 if (items[i].equals("1:")) {
 119                                         sense[1] = Integer.parseInt(items[i+1]);
 120                                         i += 2;
 121                                         continue;
 122                                 }
 123                                 if (items[i].equals("2:")) {
 124                                         sense[2] = Integer.parseInt(items[i+1]);
 125                                         i += 2;
 126                                         continue;
 127                                 }
 128                                 if (items[i].equals("3:")) {
 129                                         sense[3] = Integer.parseInt(items[i+1]);
 130                                         i += 2;
 131                                         continue;
 132                                 }
 133                                 if (items[i].equals("4:")) {
 134                                         sense[4] = Integer.parseInt(items[i+1]);
 135                                         i += 2;
 136                                         continue;
 137                                 }
 138                                 if (items[i].equals("5:")) {
 139                                         sense[5] = Integer.parseInt(items[i+1]);
 140                                         i += 2;
 141                                         continue;
 142                                 }
 143                                 if (items[i].equals("6:")) {
 144                                         v_batt = Integer.parseInt(items[i+1]);
 145                                         i += 2;
 146                                         continue;
 147                                 }
 148                                 if (items[i].equals("7:")) {
 149                                         v_pyro = Integer.parseInt(items[i+1]);
 150                                         i += 2;
 151                                         continue;
 152                                 }
 153                                 if (items[i].equals("8:")) {
 154                                         accel = Integer.parseInt(items[i+1]);
 155                                         i += 2;
 156                                         continue;
 157                                 }
 158                                 if (items[i].equals("9:")) {
 159                                         accel_ref = Integer.parseInt(items[i+1]);
 160                                         i += 2;
 161                                         continue;
 162                                 }
 163                                 i++;
 164                         }
 165                         break;
 166                 }
 167         }
 168 }
 169
 170 class AltosIMUQuery extends AltosIMU {
 171
 172         public AltosIMUQuery (AltosLink link) throws InterruptedException, TimeoutException {
 173                 link.printf("I\n");
 174                 for (;;) {
 175                         String line = link.get_reply_no_dialog(5000);
 176                         if (line == null) {
 177                                 throw new TimeoutException();
 178                         }
 179                         if (!line.startsWith("Accel:"))
 180                                 continue;
 181                         String[] items = line.split("\\s+");
 182                         if (items.length >= 8) {
 183                                 accel_x = Integer.parseInt(items[1]);
 184                                 accel_y = Integer.parseInt(items[2]);
 185                                 accel_z = Integer.parseInt(items[3]);
 186                                 gyro_x = Integer.parseInt(items[5]);
 187                                 gyro_y = Integer.parseInt(items[6]);
 188                                 gyro_z = Integer.parseInt(items[7]);
 189                         }
 190                         break;
 191                 }
 192         }
 193 }
 194
 195 class AltosMs5607Query extends AltosMs5607 {
 196         public AltosMs5607Query (AltosLink link) throws InterruptedException, TimeoutException {
 197                 link.printf("v\nB\n");
 198                 for (;;) {
 199                         String line = link.get_reply_no_dialog(5000);
 200                         if (line == null) {
 201                                 throw new TimeoutException();
 202                         }
 203                         String[] items = line.split("\\s+");
 204                         if (line.startsWith("Pressure:")) {
 205                                 if (items.length >= 2)
 206                                         raw_pres = Integer.parseInt(items[1]);
 207                         } else if (line.startsWith("Temperature:")) {
 208                                 if (items.length >= 2)
 209                                         raw_temp = Integer.parseInt(items[1]);
 210                         } else if (line.startsWith("ms5607 reserved:")) {
 211                                 if (items.length >= 3)
 212                                         reserved = Integer.parseInt(items[2]);
 213                         } else if (line.startsWith("ms5607 sens:")) {
 214                                 if (items.length >= 3)
 215                                         sens = Integer.parseInt(items[2]);
 216                         } else if (line.startsWith("ms5607 off:")) {
 217                                 if (items.length >= 3)
 218                                         off = Integer.parseInt(items[2]);
 219                         } else if (line.startsWith("ms5607 tcs:")) {
 220                                 if (items.length >= 3)
 221                                         tcs = Integer.parseInt(items[2]);
 222                         } else if (line.startsWith("ms5607 tco:")) {
 223                                 if (items.length >= 3)
 224                                         tco = Integer.parseInt(items[2]);
 225                         } else if (line.startsWith("ms5607 tref:")) {
 226                                 if (items.length >= 3)
 227                                         tref = Integer.parseInt(items[2]);
 228                         } else if (line.startsWith("ms5607 tempsens:")) {
 229                                 if (items.length >= 3)
 230                                         tempsens = Integer.parseInt(items[2]);
 231                         } else if (line.startsWith("ms5607 crc:")) {
 232                                 if (items.length >= 3)
 233                                         crc = Integer.parseInt(items[2]);
 234                         } else if (line.startsWith("Altitude"))
 235                                 break;
 236                 }
 237                 convert();
 238         }
 239 }
 240
 241 class AltosGPSQuery extends AltosGPS {
 242         public AltosGPSQuery (AltosLink link, AltosConfigData config_data)
 243                 throws TimeoutException, InterruptedException {
 244                 boolean says_done = config_data.compare_version("1.0") >= 0;
 245                 link.printf("g\n");
 246                 for (;;) {
 247                         String line = link.get_reply_no_dialog(5000);
 248                         if (line == null)
 249                                 throw new TimeoutException();
 250                         String[] bits = line.split("\\s+");
 251                         if (bits.length == 0)
 252                                 continue;
 253                         if (line.startsWith("Date:")) {
 254                                 if (bits.length < 2)
 255                                         continue;
 256                                 String[] d = bits[1].split(":");
 257                                 if (d.length < 3)
 258                                         continue;
 259                                 year = Integer.parseInt(d[0]) + 2000;
 260                                 month = Integer.parseInt(d[1]);
 261                                 day = Integer.parseInt(d[2]);
 262                                 continue;
 263                         }
 264                         if (line.startsWith("Time:")) {
 265                                 if (bits.length < 2)
 266                                         continue;
 267                                 String[] d = bits[1].split("/");
 268                                 if (d.length < 3)
 269                                         continue;
 270                                 hour = Integer.parseInt(d[0]);
 271                                 minute = Integer.parseInt(d[1]);
 272                                 second = Integer.parseInt(d[2]);
 273                                 continue;
 274                         }
 275                         if (line.startsWith("Lat/Lon:")) {
 276                                 if (bits.length < 3)
 277                                         continue;
 278                                 lat = Integer.parseInt(bits[1]) * 1.0e-7;
 279                                 lon = Integer.parseInt(bits[2]) * 1.0e-7;
 280                                 continue;
 281                         }
 282                         if (line.startsWith("Alt:")) {
 283                                 if (bits.length < 2)
 284                                         continue;
 285                                 alt = Integer.parseInt(bits[1]);
 286                                 continue;
 287                         }
 288                         if (line.startsWith("Flags:")) {
 289                                 if (bits.length < 2)
 290                                         continue;
 291                                 int status = Integer.decode(bits[1]);
 292                                 connected = (status & AltosLib.AO_GPS_RUNNING) != 0;
 293                                 locked = (status & AltosLib.AO_GPS_VALID) != 0;
 294                                 if (!says_done)
 295                                         break;
 296                                 continue;
 297                         }
 298                         if (line.startsWith("Sats:")) {
 299                                 if (bits.length < 2)
 300                                         continue;
 301                                 nsat = Integer.parseInt(bits[1]);
 302                                 cc_gps_sat = new AltosGPSSat[nsat];
 303                                 for (int i = 0; i < nsat; i++) {
 304                                         int     svid = Integer.parseInt(bits[2+i*2]);
 305                                         int     cc_n0 = Integer.parseInt(bits[3+i*2]);
 306                                         cc_gps_sat[i] = new AltosGPSSat(svid, cc_n0);
 307                                 }
 308                         }
 309                         if (line.startsWith("done"))
 310                                 break;
 311                         if (line.startsWith("Syntax error"))
 312                                 break;
 313                 }
 314         }
 315 }
 316
 317 public class AltosIdleMonitor extends Thread {
 318         AltosLink               link;
 319         AltosIdleMonitorListener        listener;
 320         AltosState              state;
 321         boolean                 remote;
 322         double                  frequency;
 323         AltosState              previous_state;
 324         AltosConfigData         config_data;
 325         AltosGPS                gps;
 326
 327         int AltosRSSI() throws TimeoutException, InterruptedException {
 328                 link.printf("s\n");
 329                 String line = link.get_reply_no_dialog(5000);
 330                 if (line == null)
 331                         throw new TimeoutException();
 332                 String[] items = line.split("\\s+");
 333                 if (items.length < 2)
 334                         return 0;
 335                 if (!items[0].equals("RSSI:"))
 336                         return 0;
 337                 int rssi = Integer.parseInt(items[1]);
 338                 return rssi;
 339         }
 340
 341         void update_state() throws InterruptedException, TimeoutException {
 342                 AltosRecord     record;
 343                 int             rssi;
 344
 345                 try {
 346                         if (remote) {
 347                                 link.set_radio_frequency(frequency);
 348                                 link.start_remote();
 349                         } else
 350                                 link.flush_input();
 351                         config_data = new AltosConfigData(link);
 352                         if (config_data.product.startsWith("TeleMetrum")) {
 353                                 AltosRecordTM record_tm = new AltosRecordTM();
 354                                 AltosSensorTM sensor = new AltosSensorTM(link);
 355                                 record_tm.accel = sensor.accel;
 356                                 record_tm.pres = sensor.pres;
 357                                 record_tm.batt = sensor.batt;
 358                                 record_tm.temp = sensor.temp;
 359                                 record_tm.drogue = sensor.drogue;
 360                                 record_tm.main = sensor.main;
 361                                 record_tm.ground_accel = record_tm.accel;
 362                                 record_tm.ground_pres = record_tm.pres;
 363                                 record_tm.accel_plus_g = config_data.accel_cal_plus;
 364                                 record_tm.accel_minus_g = config_data.accel_cal_minus;
 365                                 record_tm.tick = sensor.tick;
 366                                 record = record_tm;
 367                         } else if (config_data.product.startsWith("MegaMetrum")) {
 368                                 AltosRecordMM record_mm = new AltosRecordMM();
 369                                 AltosSensorMM sensor = new AltosSensorMM(link);
 370                                 AltosMs5607 ms5607 = new AltosMs5607Query(link);
 371                                 AltosIMU imu = new AltosIMUQuery(link);
 372
 373                                 record_mm.accel_plus_g = config_data.accel_cal_plus;
 374                                 record_mm.accel_minus_g = config_data.accel_cal_minus;
 375
 376                                 record_mm.ground_accel = sensor.accel;
 377                                 record_mm.accel = sensor.accel;
 378                                 record_mm.ground_pres = ms5607.pa;
 379                                 record_mm.pres = ms5607.pa;
 380                                 record_mm.temp = ms5607.cc;
 381
 382                                 record_mm.v_batt = sensor.v_batt;
 383                                 record_mm.v_pyro = sensor.v_pyro;
 384                                 record_mm.sense = sensor.sense;
 385
 386                                 record_mm.imu = imu;
 387
 388                                 record = record_mm;
 389                         } else
 390                                 record = new AltosRecord();
 391
 392                         gps = new AltosGPSQuery(link, config_data);
 393                 } finally {
 394                         if (remote) {
 395                                 link.stop_remote();
 396                                 rssi = AltosRSSI();
 397                         } else
 398                                 rssi = 0;
 399                 }
 400
 401                 record.version = 0;
 402                 record.callsign = config_data.callsign;
 403                 record.serial = config_data.serial;
 404                 record.flight = config_data.log_available() > 0 ? 255 : 0;
 405                 record.rssi = rssi;
 406                 record.status = 0;
 407                 record.state = AltosLib.ao_flight_idle;
 408
 409                 record.gps = gps;
 410                 state = new AltosState (record, state);
 411         }
 412
 413         public void set_frequency(double in_frequency) {
 414                 frequency = in_frequency;
 415         }
 416
 417         public void post_state() {
 418                 listener.update(state);
 419         }
 420
 421         public void run() {
 422                 try {
 423                         for (;;) {
 424                                 try {
 425                                         update_state();
 426                                         post_state();
 427                                 } catch (TimeoutException te) {
 428                                 }
 429                                 Thread.sleep(1000);
 430                         }
 431                 } catch (InterruptedException ie) {
 432                         link.close();
 433                 }
 434         }
 435
 436         public AltosIdleMonitor(AltosIdleMonitorListener in_listener, AltosLink in_link, boolean in_remote)
 437                 throws FileNotFoundException, InterruptedException, TimeoutException {
 438                 listener = in_listener;
 439                 link = in_link;
 440                 remote = in_remote;
 441                 state = null;
 442         }
 443 }