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