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

   1 /*
   2  * Copyright © 2011 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; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 package org.altusmetrum.altoslib_13;
  20
  21 import java.io.*;
  22 import java.util.concurrent.*;
  23 import java.util.*;
  24
  25 public abstract class AltosLink implements Runnable {
  26
  27         public final static int ERROR = -1;
  28         public final static int TIMEOUT = -2;
  29
  30         public abstract int getchar() throws InterruptedException;
  31         public abstract void print(String data) throws InterruptedException;
  32         public abstract void putchar(byte c);
  33         public abstract void close() throws InterruptedException;
  34
  35         public static boolean debug = false;
  36         public static void set_debug(boolean in_debug) { debug = in_debug; }
  37
  38         public boolean has_error;
  39
  40         LinkedList<String> pending_output = new LinkedList<String>();
  41
  42         public LinkedList<LinkedBlockingQueue<AltosLine>> monitors = new LinkedList<LinkedBlockingQueue<AltosLine>> ();;
  43         public LinkedBlockingQueue<AltosLine> reply_queue = new LinkedBlockingQueue<AltosLine>();
  44         public LinkedBlockingQueue<byte[]> binary_queue = new LinkedBlockingQueue<byte[]>();
  45
  46         private String match_string = null;
  47
  48         public synchronized void add_monitor(LinkedBlockingQueue<AltosLine> q) {
  49                 set_monitor(true);
  50                 monitors.add(q);
  51         }
  52
  53         public synchronized void remove_monitor(LinkedBlockingQueue<AltosLine> q) {
  54                 monitors.remove(q);
  55                 if (monitors.isEmpty())
  56                         set_monitor(false);
  57         }
  58
  59         public void printf(String format, Object ... arguments) {
  60                 String  line = String.format(format, arguments);
  61                 if (debug) {
  62                         synchronized (pending_output) {
  63                                 pending_output.add(line);
  64                         }
  65                 }
  66                 try {
  67                         print(line);
  68                 } catch (InterruptedException ie) {
  69
  70                 }
  71         }
  72
  73         public String get_reply_no_dialog(int timeout) throws InterruptedException, TimeoutException {
  74                 flush_output();
  75                 AltosLine line = reply_queue.poll(timeout, TimeUnit.MILLISECONDS);
  76                 if (line != null)
  77                         return line.line;
  78                 return null;
  79         }
  80
  81         public String get_reply() throws InterruptedException {
  82                 return get_reply(5000);
  83         }
  84
  85
  86         public abstract boolean can_cancel_reply();
  87         public abstract boolean show_reply_timeout();
  88         public abstract void hide_reply_timeout();
  89
  90         public boolean  reply_abort;
  91         public int      in_reply;
  92         boolean cancel_enable = true;
  93
  94         public void set_cancel_enable(boolean e) {
  95                 cancel_enable = e;
  96         }
  97
  98         boolean         reply_timeout_shown = false;
  99
 100         private boolean check_reply_timeout() {
 101                 if (!cancel_enable)
 102                         return false;
 103                 if (!reply_timeout_shown)
 104                         reply_timeout_shown = show_reply_timeout();
 105                 return reply_abort;
 106         }
 107
 108         private void cleanup_reply_timeout() {
 109                 if (reply_timeout_shown) {
 110                         reply_timeout_shown = false;
 111                         hide_reply_timeout();
 112                 }
 113         }
 114
 115         private int     len_read = 0;
 116
 117         private boolean match_bytes(byte[] bytes, int byte_count, String match) {
 118                 if (byte_count < match.length())
 119                         return false;
 120                 String  line = new String(bytes, 0, byte_count, AltosLib.unicode_set);
 121                 if (line == null)
 122                         return false;
 123                 return line.indexOf(match) >= 0;
 124         }
 125
 126         public void run () {
 127                 int c;
 128                 byte[] line_bytes = null;
 129                 int line_count = 0;
 130
 131                 try {
 132                         for (;;) {
 133                                 c = getchar();
 134                                 if (Thread.interrupted()) {
 135                                         break;
 136                                 }
 137                                 if (c == ERROR) {
 138                                         if (debug)
 139                                                 System.out.printf("ERROR\n");
 140                                         has_error = true;
 141                                         add_telem (new AltosLine());
 142                                         add_reply (new AltosLine());
 143                                         break;
 144                                 }
 145                                 if (c == TIMEOUT) {
 146                                         if (debug)
 147                                                 System.out.printf("TIMEOUT\n");
 148                                         continue;
 149                                 }
 150                                 if (c == '\r' && len_read == 0)
 151                                         continue;
 152                                 synchronized(this) {
 153                                         if (c == '\n' && len_read == 0) {
 154                                                 if (line_count != 0) {
 155                                                         add_bytes(line_bytes, line_count);
 156                                                         line_count = 0;
 157                                                 }
 158                                         } else {
 159                                                 if (line_bytes == null) {
 160                                                         line_bytes = new byte[256];
 161                                                 } else if (line_count == line_bytes.length) {
 162                                                         byte[] new_line_bytes = new byte[line_count * 2];
 163                                                         System.arraycopy(line_bytes, 0, new_line_bytes, 0, line_count);
 164                                                         line_bytes = new_line_bytes;
 165                                                 }
 166                                                 line_bytes[line_count] = (byte) c;
 167                                                 line_count++;
 168                                                 if (len_read !=0 && line_count == len_read) {
 169                                                         add_binary(line_bytes, line_count);
 170                                                         line_count = 0;
 171                                                         len_read = 0;
 172                                                 }
 173                                                 if (match_string != null && match_bytes(line_bytes, line_count, match_string)) {
 174                                                         match_string = null;
 175                                                         add_bytes(line_bytes, line_count);
 176                                                         line_count = 0;
 177                                                 }
 178                                         }
 179                                 }
 180                         }
 181                 } catch (InterruptedException e) {
 182                 }
 183         }
 184
 185         public void set_match(String match) {
 186                 match_string = match;
 187         }
 188
 189         public String get_reply(int timeout) throws InterruptedException {
 190                 boolean can_cancel = can_cancel_reply();
 191                 String  reply = null;
 192
 193 //              if (!can_cancel && remote)
 194 //                      System.out.printf("Uh-oh, reading remote serial device from swing thread\n");
 195
 196                 if (remote && can_cancel) {
 197                         timeout = 500;
 198                         switch (telemetry_rate) {
 199                         case AltosLib.ao_telemetry_rate_38400:
 200                         default:
 201                                 timeout = 500;
 202                                 break;
 203                         case AltosLib.ao_telemetry_rate_9600:
 204                                 timeout = 2000;
 205                                 break;
 206                         case AltosLib.ao_telemetry_rate_2400:
 207                                 timeout = 8000;
 208                                 break;
 209                         }
 210                 }
 211                 try {
 212                         ++in_reply;
 213
 214                         flush_output();
 215
 216                         reply_abort = false;
 217                         reply_timeout_shown = false;
 218                         for (;;) {
 219                                 AltosLine line = reply_queue.poll(timeout, TimeUnit.MILLISECONDS);
 220                                 if (line != null) {
 221                                         cleanup_reply_timeout();
 222                                         reply = line.line;
 223                                         break;
 224                                 }
 225                                 if (!remote || !can_cancel || check_reply_timeout()) {
 226                                         reply = null;
 227                                         break;
 228                                 }
 229                         }
 230                 } finally {
 231                         --in_reply;
 232                 }
 233                 return reply;
 234         }
 235
 236         public byte[] get_binary_reply(int timeout, int len) throws InterruptedException {
 237                 boolean can_cancel = can_cancel_reply();
 238                 byte[] bytes = null;
 239
 240                 synchronized(this) {
 241                         len_read = len;
 242                 }
 243                 try {
 244                         ++in_reply;
 245
 246                         flush_output();
 247
 248                         reply_abort = false;
 249                         reply_timeout_shown = false;
 250                         for (;;) {
 251                                 bytes = binary_queue.poll(timeout, TimeUnit.MILLISECONDS);
 252                                 if (bytes != null) {
 253                                         cleanup_reply_timeout();
 254                                         break;
 255                                 }
 256                                 if (!remote || !can_cancel || check_reply_timeout()) {
 257                                         bytes = null;
 258                                         break;
 259                                 }
 260                         }
 261
 262                 } finally {
 263                         --in_reply;
 264                 }
 265                 return bytes;
 266         }
 267
 268         public void add_telem(AltosLine line) throws InterruptedException {
 269                 for (int e = 0; e < monitors.size(); e++) {
 270                         LinkedBlockingQueue<AltosLine> q = monitors.get(e);
 271                         q.put(line);
 272                 }
 273         }
 274
 275         public void add_reply(AltosLine line) throws InterruptedException {
 276                 reply_queue.put (line);
 277         }
 278
 279         public void abort_reply() {
 280                 try {
 281                         add_telem (new AltosLine());
 282                         add_reply (new AltosLine());
 283                 } catch (InterruptedException ie) {
 284                 }
 285         }
 286
 287         public void add_string(String line) throws InterruptedException {
 288                 if (line.startsWith("TELEM") || line.startsWith("VERSION") || line.startsWith("CRC")) {
 289                         add_telem(new AltosLine(line));
 290                 } else {
 291                         add_reply(new AltosLine(line));
 292                 }
 293         }
 294
 295         public void add_bytes(byte[] bytes, int len) throws InterruptedException {
 296                 String  line;
 297                 line = new String(bytes, 0, len, AltosLib.unicode_set);
 298                 if (debug)
 299                         System.out.printf("\t\t\t\t\t%s\n", line);
 300                 add_string(line);
 301         }
 302
 303         public void add_binary(byte[] bytes, int len) throws InterruptedException {
 304                 byte[] dup = new byte[len];
 305
 306                 if (debug)
 307                         System.out.printf ("\t\t\t\t\t%d:", len);
 308                 for(int i = 0; i < len; i++) {
 309                         dup[i] = bytes[i];
 310                         if (debug)
 311                                 System.out.printf(" %02x", dup[i]);
 312                 }
 313                 if (debug)
 314                         System.out.printf("\n");
 315
 316                 binary_queue.put(dup);
 317         }
 318
 319         public synchronized void flush_output() {
 320                 if (pending_output == null)
 321                         return;
 322                 synchronized (pending_output) {
 323                         for (String s : pending_output)
 324                                 System.out.print(s);
 325                         pending_output.clear();
 326                 }
 327         }
 328
 329         public void flush_input(int timeout) throws InterruptedException {
 330                 flush_output();
 331                 boolean got_some;
 332
 333                 do {
 334                         Thread.sleep(timeout);
 335                         got_some = !reply_queue.isEmpty();
 336                         reply_queue.clear();
 337                 } while (got_some);
 338         }
 339
 340
 341         public void flush_input() throws InterruptedException {
 342                 if (remote) {
 343                         int timeout = 500;
 344                         switch (telemetry_rate) {
 345                         case AltosLib.ao_telemetry_rate_38400:
 346                         default:
 347                                 timeout = 500;
 348                                 break;
 349                         case AltosLib.ao_telemetry_rate_9600:
 350                                 timeout = 1000;
 351                                 break;
 352                         case AltosLib.ao_telemetry_rate_2400:
 353                                 timeout = 2000;
 354                                 break;
 355                         }
 356                         flush_input(timeout);
 357                 } else
 358                         flush_input(100);
 359         }
 360
 361
 362         /*
 363          * Various command-level operations on
 364          * the link
 365          */
 366         public boolean monitor_mode = false;
 367         public int telemetry = AltosLib.ao_telemetry_standard;
 368         public int telemetry_rate = -1;
 369         public double frequency;
 370         public String callsign;
 371         private AltosConfigData config_data_local;
 372         private AltosConfigData config_data_remote;
 373
 374         private Object config_data_lock = new Object();
 375
 376         private int telemetry_len() {
 377                 return AltosLib.telemetry_len(telemetry);
 378         }
 379
 380         private void set_radio_freq(int frequency) {
 381                 if (monitor_mode)
 382                         printf("m 0\nc F %d\nm %x\n",
 383                                frequency, telemetry_len());
 384                 else
 385                         printf("c F %d\n", frequency);
 386                 flush_output();
 387         }
 388
 389         public void set_radio_frequency(double frequency,
 390                                         boolean has_frequency,
 391                                         boolean has_setting,
 392                                         int cal) {
 393                 if (debug)
 394                         System.out.printf("set_radio_frequency %7.3f (freq %b) (set %b) %d\n", frequency, has_frequency, has_setting, cal);
 395                 if (frequency == 0)
 396                         return;
 397                 if (has_frequency)
 398                         set_radio_freq((int) Math.floor (frequency * 1000 + 0.5));
 399                 else if (has_setting)
 400                         set_radio_setting(AltosConvert.radio_frequency_to_setting(frequency, cal));
 401                 else
 402                         set_channel(AltosConvert.radio_frequency_to_channel(frequency));
 403         }
 404
 405         public void set_radio_frequency(double in_frequency) throws InterruptedException, TimeoutException {
 406                 frequency = in_frequency;
 407                 AltosConfigData config_data = config_data();
 408                 set_radio_frequency(frequency,
 409                                     config_data.radio_frequency > 0,
 410                                     config_data.radio_setting > 0,
 411                                     config_data.radio_calibration);
 412         }
 413
 414         public void set_telemetry(int in_telemetry) {
 415                 telemetry = in_telemetry;
 416                 if (monitor_mode)
 417                         printf("m 0\nm %x\n", telemetry_len());
 418                 flush_output();
 419         }
 420
 421         public void set_telemetry_rate(int in_telemetry_rate) {
 422                 telemetry_rate = in_telemetry_rate;
 423                 if (monitor_mode)
 424                         printf("m 0\nc T %d\nm %x\n", telemetry_rate, telemetry_len());
 425                 else
 426                         printf("c T %d\n", telemetry_rate);
 427                 flush_output();
 428         }
 429
 430         public synchronized void set_monitor(boolean monitor) {
 431                 monitor_mode = monitor;
 432                 if (monitor)
 433                         printf("m %x\n", telemetry_len());
 434                 else
 435                         printf("m 0\n");
 436                 flush_output();
 437         }
 438
 439         public synchronized boolean get_monitor() {
 440                 return monitor_mode;
 441         }
 442
 443         private void set_channel(int channel) {
 444                 if (monitor_mode)
 445                         printf("m 0\nc r %d\nm %x\n",
 446                                channel, telemetry_len());
 447                 else
 448                         printf("c r %d\n", channel);
 449                 flush_output();
 450         }
 451
 452         private void set_radio_setting(int setting) {
 453                 if (monitor_mode)
 454                         printf("m 0\nc R %d\nm %x\n",
 455                                setting, telemetry_len());
 456                 else
 457                         printf("c R %d\n", setting);
 458                 flush_output();
 459         }
 460
 461         public AltosConfigData config_data() throws InterruptedException, TimeoutException {
 462                 synchronized(config_data_lock) {
 463                         AltosConfigData config_data;
 464
 465                         if (remote) {
 466                                 if (config_data_remote == null) {
 467                                         printf("m 0\n");
 468                                         config_data_remote = new AltosConfigData(this);
 469                                         if (monitor_mode)
 470                                                 set_monitor(true);
 471                                 }
 472                                 config_data = config_data_remote;
 473                         } else {
 474                                 if (config_data_local == null) {
 475                                         printf("m 0\n");
 476                                         config_data_local = new AltosConfigData(this);
 477                                         if (monitor_mode)
 478                                                 set_monitor(true);
 479                                 }
 480                                 config_data = config_data_local;
 481                         }
 482                         return config_data;
 483                 }
 484         }
 485
 486         public void set_callsign(String callsign) {
 487                 this.callsign = callsign;
 488                 if (callsign != null) {
 489                         printf ("c c %s\n", callsign);
 490                         flush_output();
 491                 }
 492         }
 493
 494         public boolean is_loader() throws InterruptedException {
 495                 boolean ret = false;
 496                 printf("v\n");
 497                 for (;;) {
 498                         String line = get_reply();
 499
 500                         if (line == null)
 501                                 return false;
 502                         if (line.startsWith("software-version"))
 503                                 break;
 504                         if (line.startsWith("altos-loader"))
 505                                 ret = true;
 506                 }
 507                 return ret;
 508         }
 509
 510         public void to_loader() throws InterruptedException {
 511                 printf("X\n");
 512                 flush_output();
 513                 close();
 514                 Thread.sleep(1000);
 515         }
 516
 517         public boolean remote;
 518         public int serial;
 519         public String name;
 520
 521         public void start_remote() throws TimeoutException, InterruptedException {
 522                 if (frequency == 0.0)
 523                         frequency = AltosPreferences.frequency(serial);
 524                 if (debug)
 525                         System.out.printf("start remote %7.3f\n", frequency);
 526                 set_radio_frequency(frequency);
 527                 if (telemetry_rate < 0)
 528                         telemetry_rate = AltosPreferences.telemetry_rate(serial);
 529                 set_telemetry_rate(telemetry_rate);
 530                 if (callsign == null || callsign.equals(""))
 531                         callsign = AltosPreferences.callsign();
 532                 set_callsign(callsign);
 533                 printf("p\nE 0\n");
 534                 flush_input();
 535                 remote = true;
 536         }
 537
 538         public void stop_remote() throws InterruptedException {
 539                 if (debug)
 540                         System.out.printf("stop remote\n");
 541                 try {
 542                         flush_input();
 543                 } finally {
 544                         printf ("~\n");
 545                         flush_output();
 546                 }
 547                 remote = false;
 548         }
 549
 550         public int rssi() throws TimeoutException, InterruptedException {
 551                 if (remote)
 552                         return 0;
 553                 printf("s\n");
 554                 String line = get_reply_no_dialog(5000);
 555                 if (line == null)
 556                         throw new TimeoutException();
 557                 String[] items = line.split("\\s+");
 558                 if (items.length < 2)
 559                         return 0;
 560                 if (!items[0].equals("RSSI:"))
 561                         return 0;
 562                 int rssi = Integer.parseInt(items[1]);
 563                 return rssi;
 564         }
 565
 566         public String[] adc() throws TimeoutException, InterruptedException {
 567                 printf("a\n");
 568                 for (;;) {
 569                         String line = get_reply_no_dialog(5000);
 570                         if (line == null) {
 571                                 throw new TimeoutException();
 572                         }
 573                         if (!line.startsWith("tick:"))
 574                                 continue;
 575                         String[] items = line.split("\\s+");
 576                         return items;
 577                 }
 578         }
 579
 580         public boolean has_monitor_battery() {
 581                 try {
 582                         return config_data().has_monitor_battery();
 583                 } catch (InterruptedException ie) {
 584                         return false;
 585                 } catch (TimeoutException te) {
 586                         return false;
 587                 }
 588         }
 589
 590         public double monitor_battery() throws InterruptedException {
 591                 double  volts = AltosLib.MISSING;
 592
 593                 try {
 594                         AltosConfigData config_data = config_data();
 595                         int monitor_batt = AltosLib.MISSING;
 596
 597                         if (config_data.has_monitor_battery()) {
 598                                 String[] items = adc();
 599                                 for (int i = 0; i < items.length;) {
 600                                         if (items[i].equals("batt")) {
 601                                                 monitor_batt = Integer.parseInt(items[i+1]);
 602                                                 i += 2;
 603                                                 continue;
 604                                         }
 605                                         i++;
 606                                 }
 607                         }
 608                         if (monitor_batt != AltosLib.MISSING) {
 609                                 if (config_data.product.startsWith("TeleBT-v3") || config_data.product.startsWith("TeleBT-v4")) {
 610                                         volts = AltosConvert.tele_bt_3_battery(monitor_batt);
 611                                 } else {
 612                                         volts = AltosConvert.cc_battery_to_voltage(monitor_batt);
 613                                 }
 614                         }
 615
 616                 } catch (TimeoutException te) {
 617                 }
 618                 return volts;
 619         }
 620
 621         public AltosLink() {
 622                 callsign = "";
 623                 has_error = false;
 624         }
 625 }