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 public class AltosIdleMonitor extends Thread {
 237         AltosLink               link;
 238         AltosIdleMonitorListener        listener;
 239         AltosState              state;
 240         boolean                 remote;
 241         double                  frequency;
 242         AltosState              previous_state;
 243         AltosConfigData         config_data;
 244         AltosGPS                gps;
 245
 246         int AltosRSSI() throws TimeoutException, InterruptedException {
 247                 link.printf("s\n");
 248                 String line = link.get_reply_no_dialog(5000);
 249                 if (line == null)
 250                         throw new TimeoutException();
 251                 String[] items = line.split("\\s+");
 252                 if (items.length < 2)
 253                         return 0;
 254                 if (!items[0].equals("RSSI:"))
 255                         return 0;
 256                 int rssi = Integer.parseInt(items[1]);
 257                 return rssi;
 258         }
 259
 260         void update_state() throws InterruptedException, TimeoutException {
 261                 AltosRecord     record = null;
 262
 263                 try {
 264                         if (remote) {
 265                                 link.set_radio_frequency(frequency);
 266                                 link.start_remote();
 267                         } else
 268                                 link.flush_input();
 269                         config_data = new AltosConfigData(link);
 270                         if (config_data.product.startsWith("TeleMetrum")) {
 271                                 record = new AltosSensorTM(link, config_data);
 272                         } else if (config_data.product.startsWith("MegaMetrum")) {
 273                                 AltosRecordMM record_mm = new AltosRecordMM();
 274                                 AltosSensorMM sensor = new AltosSensorMM(link);
 275                                 AltosMs5607 ms5607 = new AltosMs5607Query(link);
 276                                 AltosIMU imu = new AltosIMUQuery(link);
 277
 278                                 record_mm.accel_plus_g = config_data.accel_cal_plus;
 279                                 record_mm.accel_minus_g = config_data.accel_cal_minus;
 280
 281                                 record_mm.ground_accel = sensor.accel;
 282                                 record_mm.accel = sensor.accel;
 283                                 record_mm.ground_pres = ms5607.pa;
 284                                 record_mm.pres = ms5607.pa;
 285                                 record_mm.temp = ms5607.cc;
 286
 287                                 record_mm.v_batt = sensor.v_batt;
 288                                 record_mm.v_pyro = sensor.v_pyro;
 289                                 record_mm.sense = sensor.sense;
 290
 291                                 record_mm.imu = imu;
 292
 293                                 record = record_mm;
 294                         } else
 295                                 record = new AltosRecord();
 296
 297                         gps = new AltosGPSQuery(link, config_data);
 298
 299                         record.version = 0;
 300                         record.callsign = config_data.callsign;
 301                         record.serial = config_data.serial;
 302                         record.flight = config_data.log_available() > 0 ? 255 : 0;
 303                         record.status = 0;
 304                         record.state = AltosLib.ao_flight_idle;
 305                         record.gps = gps;
 306                         record.new_gps = true;
 307                         state = new AltosState (record, state);
 308                 } finally {
 309                         if (remote) {
 310                                 link.stop_remote();
 311                                 if (record != null)
 312                                         record.rssi = AltosRSSI();
 313                         } else {
 314                                 if (record != null)
 315                                         record.rssi = 0;
 316                         }
 317                 }
 318
 319         }
 320
 321         public void set_frequency(double in_frequency) {
 322                 frequency = in_frequency;
 323         }
 324
 325         public void post_state() {
 326                 listener.update(state);
 327         }
 328
 329         public void run() {
 330                 try {
 331                         for (;;) {
 332                                 try {
 333                                         update_state();
 334                                         post_state();
 335                                 } catch (TimeoutException te) {
 336                                 }
 337                                 Thread.sleep(1000);
 338                         }
 339                 } catch (InterruptedException ie) {
 340                         link.close();
 341                 }
 342         }
 343
 344         public AltosIdleMonitor(AltosIdleMonitorListener in_listener, AltosLink in_link, boolean in_remote)
 345                 throws FileNotFoundException, InterruptedException, TimeoutException {
 346                 listener = in_listener;
 347                 link = in_link;
 348                 remote = in_remote;
 349                 state = null;
 350         }
 351 }