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