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