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