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                         for (int i = 0; i < sense.length; i++)
 166                                 System.out.printf("sense[%d]: %d\n", i, sense[i]);
 167                         break;
 168                 }
 169         }
 170 }
 171
 172 class AltosIMUQuery extends AltosIMU {
 173
 174         public AltosIMUQuery (AltosLink link) throws InterruptedException, TimeoutException {
 175                 link.printf("I\n");
 176                 for (;;) {
 177                         String line = link.get_reply_no_dialog(5000);
 178                         if (line == null) {
 179                                 throw new TimeoutException();
 180                         }
 181                         if (!line.startsWith("Accel:"))
 182                                 continue;
 183                         String[] items = line.split("\\s+");
 184                         if (items.length >= 8) {
 185                                 accel_x = Integer.parseInt(items[1]);
 186                                 accel_y = Integer.parseInt(items[2]);
 187                                 accel_z = Integer.parseInt(items[3]);
 188                                 gyro_x = Integer.parseInt(items[5]);
 189                                 gyro_y = Integer.parseInt(items[6]);
 190                                 gyro_z = Integer.parseInt(items[7]);
 191                         }
 192                         break;
 193                 }
 194         }
 195 }
 196
 197 class AltosMs5607Query extends AltosMs5607 {
 198         public AltosMs5607Query (AltosLink link) throws InterruptedException, TimeoutException {
 199                 link.printf("v\nB\n");
 200                 for (;;) {
 201                         String line = link.get_reply_no_dialog(5000);
 202                         if (line == null) {
 203                                 throw new TimeoutException();
 204                         }
 205                         String[] items = line.split("\\s+");
 206                         if (line.startsWith("Pressure:")) {
 207                                 if (items.length >= 2)
 208                                         raw_pres = Integer.parseInt(items[1]);
 209                         } else if (line.startsWith("Temperature:")) {
 210                                 if (items.length >= 2)
 211                                         raw_temp = Integer.parseInt(items[1]);
 212                         } else if (line.startsWith("ms5607 reserved:")) {
 213                                 if (items.length >= 3)
 214                                         reserved = Integer.parseInt(items[2]);
 215                         } else if (line.startsWith("ms5607 sens:")) {
 216                                 if (items.length >= 3)
 217                                         sens = Integer.parseInt(items[2]);
 218                         } else if (line.startsWith("ms5607 off:")) {
 219                                 if (items.length >= 3)
 220                                         off = Integer.parseInt(items[2]);
 221                         } else if (line.startsWith("ms5607 tcs:")) {
 222                                 if (items.length >= 3)
 223                                         tcs = Integer.parseInt(items[2]);
 224                         } else if (line.startsWith("ms5607 tco:")) {
 225                                 if (items.length >= 3)
 226                                         tco = Integer.parseInt(items[2]);
 227                         } else if (line.startsWith("ms5607 tref:")) {
 228                                 if (items.length >= 3)
 229                                         tref = Integer.parseInt(items[2]);
 230                         } else if (line.startsWith("ms5607 tempsens:")) {
 231                                 if (items.length >= 3)
 232                                         tempsens = Integer.parseInt(items[2]);
 233                         } else if (line.startsWith("ms5607 crc:")) {
 234                                 if (items.length >= 3)
 235                                         crc = Integer.parseInt(items[2]);
 236                         } else if (line.startsWith("Altitude"))
 237                                 break;
 238                 }
 239                 convert();
 240         }
 241 }
 242
 243 class AltosGPSQuery extends AltosGPS {
 244         public AltosGPSQuery (AltosLink link, AltosConfigData config_data)
 245                 throws TimeoutException, InterruptedException {
 246                 boolean says_done = config_data.compare_version("1.0") >= 0;
 247                 link.printf("g\n");
 248                 for (;;) {
 249                         String line = link.get_reply_no_dialog(5000);
 250                         if (line == null)
 251                                 throw new TimeoutException();
 252                         String[] bits = line.split("\\s+");
 253                         if (bits.length == 0)
 254                                 continue;
 255                         if (line.startsWith("Date:")) {
 256                                 if (bits.length < 2)
 257                                         continue;
 258                                 String[] d = bits[1].split(":");
 259                                 if (d.length < 3)
 260                                         continue;
 261                                 year = Integer.parseInt(d[0]) + 2000;
 262                                 month = Integer.parseInt(d[1]);
 263                                 day = Integer.parseInt(d[2]);
 264                                 continue;
 265                         }
 266                         if (line.startsWith("Time:")) {
 267                                 if (bits.length < 2)
 268                                         continue;
 269                                 String[] d = bits[1].split("/");
 270                                 if (d.length < 3)
 271                                         continue;
 272                                 hour = Integer.parseInt(d[0]);
 273                                 minute = Integer.parseInt(d[1]);
 274                                 second = Integer.parseInt(d[2]);
 275                                 continue;
 276                         }
 277                         if (line.startsWith("Lat/Lon:")) {
 278                                 if (bits.length < 3)
 279                                         continue;
 280                                 lat = Integer.parseInt(bits[1]) * 1.0e-7;
 281                                 lon = Integer.parseInt(bits[2]) * 1.0e-7;
 282                                 continue;
 283                         }
 284                         if (line.startsWith("Alt:")) {
 285                                 if (bits.length < 2)
 286                                         continue;
 287                                 alt = Integer.parseInt(bits[1]);
 288                                 continue;
 289                         }
 290                         if (line.startsWith("Flags:")) {
 291                                 if (bits.length < 2)
 292                                         continue;
 293                                 int status = Integer.decode(bits[1]);
 294                                 connected = (status & AltosLib.AO_GPS_RUNNING) != 0;
 295                                 locked = (status & AltosLib.AO_GPS_VALID) != 0;
 296                                 if (!says_done)
 297                                         break;
 298                                 continue;
 299                         }
 300                         if (line.startsWith("Sats:")) {
 301                                 if (bits.length < 2)
 302                                         continue;
 303                                 nsat = Integer.parseInt(bits[1]);
 304                                 cc_gps_sat = new AltosGPSSat[nsat];
 305                                 for (int i = 0; i < nsat; i++) {
 306                                         int     svid = Integer.parseInt(bits[2+i*2]);
 307                                         int     cc_n0 = Integer.parseInt(bits[3+i*2]);
 308                                         cc_gps_sat[i] = new AltosGPSSat(svid, cc_n0);
 309                                 }
 310                         }
 311                         if (line.startsWith("done"))
 312                                 break;
 313                         if (line.startsWith("Syntax error"))
 314                                 break;
 315                 }
 316         }
 317 }
 318
 319 public class AltosIdleMonitor extends Thread {
 320         AltosLink               link;
 321         AltosIdleMonitorListener        listener;
 322         AltosState              state;
 323         boolean                 remote;
 324         double                  frequency;
 325         AltosState              previous_state;
 326         AltosConfigData         config_data;
 327         AltosGPS                gps;
 328
 329         int AltosRSSI() throws TimeoutException, InterruptedException {
 330                 link.printf("s\n");
 331                 String line = link.get_reply_no_dialog(5000);
 332                 if (line == null)
 333                         throw new TimeoutException();
 334                 String[] items = line.split("\\s+");
 335                 if (items.length < 2)
 336                         return 0;
 337                 if (!items[0].equals("RSSI:"))
 338                         return 0;
 339                 int rssi = Integer.parseInt(items[1]);
 340                 return rssi;
 341         }
 342
 343         void update_state() throws InterruptedException, TimeoutException {
 344                 AltosRecord     record;
 345                 int             rssi;
 346
 347                 try {
 348                         if (remote) {
 349                                 link.set_radio_frequency(frequency);
 350                                 link.start_remote();
 351                         } else
 352                                 link.flush_input();
 353                         config_data = new AltosConfigData(link);
 354                         if (config_data.product.startsWith("TeleMetrum")) {
 355                                 AltosRecordTM record_tm = new AltosRecordTM();
 356                                 AltosSensorTM sensor = new AltosSensorTM(link);
 357                                 record_tm.accel = sensor.accel;
 358                                 record_tm.pres = sensor.pres;
 359                                 record_tm.batt = sensor.batt;
 360                                 record_tm.temp = sensor.temp;
 361                                 record_tm.drogue = sensor.drogue;
 362                                 record_tm.main = sensor.main;
 363                                 record_tm.ground_accel = record_tm.accel;
 364                                 record_tm.ground_pres = record_tm.pres;
 365                                 record_tm.accel_plus_g = config_data.accel_cal_plus;
 366                                 record_tm.accel_minus_g = config_data.accel_cal_minus;
 367                                 record_tm.tick = sensor.tick;
 368                                 record = record_tm;
 369                         } else if (config_data.product.startsWith("MegaMetrum")) {
 370                                 AltosRecordMM record_mm = new AltosRecordMM();
 371                                 AltosSensorMM sensor = new AltosSensorMM(link);
 372                                 AltosMs5607 ms5607 = new AltosMs5607Query(link);
 373                                 AltosIMU imu = new AltosIMUQuery(link);
 374
 375                                 record_mm.accel_plus_g = config_data.accel_cal_plus;
 376                                 record_mm.accel_minus_g = config_data.accel_cal_minus;
 377
 378                                 record_mm.ground_accel = sensor.accel;
 379                                 record_mm.accel = sensor.accel;
 380                                 record_mm.ground_pres = ms5607.pa;
 381                                 record_mm.pres = ms5607.pa;
 382                                 record_mm.temp = ms5607.cc;
 383
 384                                 record_mm.v_batt = sensor.v_batt;
 385                                 record_mm.v_pyro = sensor.v_pyro;
 386                                 record_mm.sense = sensor.sense;
 387
 388                                 record_mm.imu = imu;
 389
 390                                 record = record_mm;
 391                         } else
 392                                 record = new AltosRecord();
 393
 394                         gps = new AltosGPSQuery(link, config_data);
 395                 } finally {
 396                         if (remote) {
 397                                 link.stop_remote();
 398                                 rssi = AltosRSSI();
 399                         } else
 400                                 rssi = 0;
 401                 }
 402
 403                 record.version = 0;
 404                 record.callsign = config_data.callsign;
 405                 record.serial = config_data.serial;
 406                 record.flight = config_data.log_available() > 0 ? 255 : 0;
 407                 record.rssi = rssi;
 408                 record.status = 0;
 409                 record.state = AltosLib.ao_flight_idle;
 410
 411                 record.gps = gps;
 412                 state = new AltosState (record, state);
 413         }
 414
 415         public void set_frequency(double in_frequency) {
 416                 frequency = in_frequency;
 417         }
 418
 419         public void post_state() {
 420                 listener.update(state);
 421         }
 422
 423         public void run() {
 424                 try {
 425                         for (;;) {
 426                                 try {
 427                                         update_state();
 428                                         post_state();
 429                                 } catch (TimeoutException te) {
 430                                 }
 431                                 Thread.sleep(1000);
 432                         }
 433                 } catch (InterruptedException ie) {
 434                         link.close();
 435                 }
 436         }
 437
 438         public AltosIdleMonitor(AltosIdleMonitorListener in_listener, AltosLink in_link, boolean in_remote)
 439                 throws FileNotFoundException, InterruptedException, TimeoutException {
 440                 listener = in_listener;
 441                 link = in_link;
 442                 remote = in_remote;
 443                 state = null;
 444         }
 445 }