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