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