git.gag.com Git - fw/altos/blob - src/drivers/ao_lco.c

   1 /*
   2  * Copyright © 2012 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 #include <ao.h>
  19 #include <ao_lco.h>
  20 #include <ao_event.h>
  21 #include <ao_seven_segment.h>
  22 #include <ao_quadrature.h>
  23 #include <ao_lco_func.h>
  24 #include <ao_radio_cmac.h>
  25
  26 #define DEBUG   1
  27
  28 #if DEBUG
  29 static uint8_t  ao_lco_debug;
  30 #define PRINTD(...) do { if (!ao_lco_debug) break; printf ("\r%5u %s: ", ao_tick_count, __func__); printf(__VA_ARGS__); flush(); } while(0)
  31 #else
  32 #define PRINTD(...)
  33 #endif
  34
  35 #define AO_LCO_PAD_DIGIT        0
  36 #define AO_LCO_BOX_DIGIT_1      1
  37 #define AO_LCO_BOX_DIGIT_10     2
  38
  39 #define AO_LCO_DRAG_RACE_START_TIME     AO_SEC_TO_TICKS(5)
  40 #define AO_LCO_DRAG_RACE_STOP_TIME      AO_SEC_TO_TICKS(2)
  41
  42 #define AO_LCO_VALID_LAST       1
  43 #define AO_LCO_VALID_EVER       2
  44
  45 static uint8_t  ao_lco_min_box, ao_lco_max_box;
  46 static uint8_t  ao_lco_selected[AO_PAD_MAX_BOXES];
  47 static uint8_t  ao_lco_valid[AO_PAD_MAX_BOXES];
  48 static uint8_t  ao_lco_channels[AO_PAD_MAX_BOXES];
  49 static uint16_t ao_lco_tick_offset[AO_PAD_MAX_BOXES];
  50
  51 /* UI values */
  52 static uint8_t  ao_lco_armed;
  53 static uint8_t  ao_lco_firing;
  54 static uint16_t ao_lco_fire_tick;
  55 static uint8_t  ao_lco_fire_down;
  56 static uint8_t  ao_lco_drag_race;
  57 static uint8_t  ao_lco_pad;
  58 static int16_t  ao_lco_box;
  59
  60 #define AO_LCO_BOX_DRAG         0x1000
  61
  62 static struct ao_pad_query      ao_pad_query;
  63
  64 static uint8_t  ao_lco_display_mutex;
  65
  66 static void
  67 ao_lco_set_pad(uint8_t pad)
  68 {
  69         ao_mutex_get(&ao_lco_display_mutex);
  70         ao_seven_segment_set(AO_LCO_PAD_DIGIT, pad | (ao_lco_drag_race << 4));
  71         ao_mutex_put(&ao_lco_display_mutex);
  72 }
  73
  74 #define SEVEN_SEGMENT_d         ((0 << 0) |     \
  75                                  (0 << 1) |     \
  76                                  (1 << 2) |     \
  77                                  (1 << 3) |     \
  78                                  (1 << 4) |     \
  79                                  (1 << 5) |     \
  80                                  (1 << 6))
  81
  82
  83 #define SEVEN_SEGMENT_r         ((0 << 0) |     \
  84                                  (0 << 1) |     \
  85                                  (0 << 2) |     \
  86                                  (1 << 3) |     \
  87                                  (1 << 4) |     \
  88                                  (0 << 5) |     \
  89                                  (0 << 6))
  90
  91 static void
  92 ao_lco_set_box(uint16_t box)
  93 {
  94         ao_mutex_get(&ao_lco_display_mutex);
  95         if (box == AO_LCO_BOX_DRAG) {
  96                 ao_seven_segment_direct(AO_LCO_BOX_DIGIT_10, SEVEN_SEGMENT_d | (ao_lco_drag_race << 7));
  97                 ao_seven_segment_direct(AO_LCO_BOX_DIGIT_1, SEVEN_SEGMENT_r | (ao_lco_drag_race << 7));
  98         } else {
  99                 ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, box % 10 | (ao_lco_drag_race << 4));
 100                 ao_seven_segment_set(AO_LCO_BOX_DIGIT_10, box / 10 | (ao_lco_drag_race << 4));
 101         }
 102         ao_mutex_put(&ao_lco_display_mutex);
 103 }
 104
 105 static void
 106 ao_lco_set_voltage(uint16_t decivolts)
 107 {
 108         uint8_t tens, ones, tenths;
 109
 110         tenths = decivolts % 10;
 111         ones = (decivolts / 10) % 10;
 112         tens = (decivolts / 100) % 10;
 113         ao_mutex_get(&ao_lco_display_mutex);
 114         ao_seven_segment_set(AO_LCO_PAD_DIGIT, tenths);
 115         ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, ones | 0x10);
 116         ao_seven_segment_set(AO_LCO_BOX_DIGIT_10, tens);
 117         ao_mutex_put(&ao_lco_display_mutex);
 118 }
 119
 120 static void
 121 ao_lco_set_display(void)
 122 {
 123         if (ao_lco_pad == 0 && ao_lco_box != AO_LCO_BOX_DRAG) {
 124                 ao_lco_set_voltage(ao_pad_query.battery);
 125         } else {
 126                 if (ao_lco_box == AO_LCO_BOX_DRAG)
 127                         ao_lco_set_pad(ao_lco_drag_race);
 128                 else
 129                         ao_lco_set_pad(ao_lco_pad);
 130                 ao_lco_set_box(ao_lco_box);
 131         }
 132 }
 133
 134 #define MASK_SIZE(n)    (((n) + 7) >> 3)
 135 #define MASK_ID(n)      ((n) >> 3)
 136 #define MASK_SHIFT(n)   ((n) & 7)
 137
 138 static uint8_t  ao_lco_box_mask[MASK_SIZE(AO_PAD_MAX_BOXES)];
 139
 140 static uint8_t
 141 ao_lco_box_present(uint16_t box)
 142 {
 143         if (box == AO_LCO_BOX_DRAG)
 144                 return 1;
 145
 146         if (box >= AO_PAD_MAX_BOXES)
 147                 return 0;
 148         return (ao_lco_box_mask[MASK_ID(box)] >> MASK_SHIFT(box)) & 1;
 149 }
 150
 151 static uint8_t
 152 ao_lco_pad_present(uint8_t box, uint8_t pad)
 153 {
 154         /* voltage measurement is always valid */
 155         if (pad == 0)
 156                 return 1;
 157         if (!ao_lco_channels[box])
 158                 return 0;
 159         if (pad > AO_PAD_MAX_CHANNELS)
 160                 return 0;
 161         return (ao_lco_channels[box] >> (pad - 1)) & 1;
 162 }
 163
 164 static uint8_t
 165 ao_lco_pad_first(uint8_t box)
 166 {
 167         uint8_t pad;
 168
 169         for (pad = 1; pad <= AO_PAD_MAX_CHANNELS; pad++)
 170                 if (ao_lco_pad_present(box, pad))
 171                         return pad;
 172         return 0;
 173 }
 174
 175 static struct ao_task   ao_lco_drag_task;
 176 static uint8_t          ao_lco_drag_active;
 177 static uint8_t          ao_lco_drag_beep_count;
 178 static uint8_t          ao_lco_drag_beep_on;
 179 static uint16_t         ao_lco_drag_beep_time;
 180 static uint16_t         ao_lco_drag_warn_time;
 181
 182 #define AO_LCO_DRAG_BEEP_TIME   AO_MS_TO_TICKS(50)
 183 #define AO_LCO_DRAG_WARN_TIME   AO_SEC_TO_TICKS(5)
 184
 185 static void
 186 ao_lco_drag_beep_start(void)
 187 {
 188         ao_beep(AO_BEEP_HIGH);
 189         PRINTD("beep start\n");
 190         ao_lco_drag_beep_on = 1;
 191         ao_lco_drag_beep_time = ao_time() + AO_LCO_DRAG_BEEP_TIME;
 192 }
 193
 194 static void
 195 ao_lco_drag_beep_stop(void)
 196 {
 197         ao_beep(0);
 198         PRINTD("beep stop\n");
 199         ao_lco_drag_beep_on = 0;
 200         if (ao_lco_drag_beep_count) {
 201                 --ao_lco_drag_beep_count;
 202                 if (ao_lco_drag_beep_count)
 203                         ao_lco_drag_beep_time = ao_time() + AO_LCO_DRAG_BEEP_TIME;
 204         }
 205 }
 206
 207 static void
 208 ao_lco_drag_beep(uint8_t beeps)
 209 {
 210         PRINTD("beep %d\n", beeps);
 211         if (!ao_lco_drag_beep_count)
 212                 ao_lco_drag_beep_start();
 213         ao_lco_drag_beep_count += beeps;
 214 }
 215
 216 static uint16_t
 217 ao_lco_drag_beep_check(uint16_t now, uint16_t delay)
 218 {
 219         PRINTD("beep check count %d delta %d\n",
 220                ao_lco_drag_beep_count,
 221                (int16_t) (now - ao_lco_drag_beep_time));
 222         if (ao_lco_drag_beep_count) {
 223                 if ((int16_t) (now - ao_lco_drag_beep_time) >= 0) {
 224                         if (ao_lco_drag_beep_on)
 225                                 ao_lco_drag_beep_stop();
 226                         else
 227                                 ao_lco_drag_beep_start();
 228                 }
 229         }
 230
 231         if (ao_lco_drag_beep_count) {
 232                 if (delay > AO_LCO_DRAG_BEEP_TIME)
 233                         delay = AO_LCO_DRAG_BEEP_TIME;
 234         }
 235         return delay;
 236 }
 237
 238 static void
 239 ao_lco_drag_enable(void)
 240 {
 241         PRINTD("Drag enable\n");
 242         ao_lco_drag_race = 1;
 243         memset(ao_lco_selected, 0, sizeof (ao_lco_selected));
 244         ao_lco_drag_beep(5);
 245         ao_lco_set_display();
 246         ao_lco_fire_down = 0;
 247 }
 248
 249 static void
 250 ao_lco_drag_disable(void)
 251 {
 252         PRINTD("Drag disable\n");
 253         ao_lco_drag_race = 0;
 254         memset(ao_lco_selected, 0, sizeof (ao_lco_selected));
 255         ao_lco_drag_beep(2);
 256         ao_lco_set_display();
 257         ao_lco_fire_down = 0;
 258 }
 259
 260 static uint16_t
 261 ao_lco_drag_button_check(uint16_t now, uint16_t delay)
 262 {
 263         uint16_t        button_delay = ~0;
 264
 265         /*
 266          * Check to see if the button has been held down long enough
 267          * to switch in/out of drag race mode
 268          */
 269         if (ao_lco_fire_down) {
 270                 if (ao_lco_drag_race) {
 271                         if ((int16_t) (now - ao_lco_fire_tick) >= AO_LCO_DRAG_RACE_STOP_TIME)
 272                                 ao_lco_drag_disable();
 273                         else
 274                                 button_delay = ao_lco_fire_tick + AO_LCO_DRAG_RACE_STOP_TIME - now;
 275                 } else {
 276                         if ((int16_t) (now - ao_lco_fire_tick) >= AO_LCO_DRAG_RACE_START_TIME)
 277                                 ao_lco_drag_enable();
 278                         else
 279                                 button_delay = ao_lco_fire_tick + AO_LCO_DRAG_RACE_START_TIME - now;
 280                 }
 281                 if (delay > button_delay)
 282                         delay = button_delay;
 283         }
 284         return delay;
 285 }
 286
 287 static uint16_t
 288 ao_lco_drag_warn_check(uint16_t now, uint16_t delay)
 289 {
 290         uint16_t        warn_delay = ~0;
 291
 292         if (ao_lco_drag_race) {
 293                 if ((int16_t) (now - ao_lco_drag_warn_time) >= 0) {
 294                         ao_lco_drag_beep(1);
 295                         ao_lco_drag_warn_time = now + AO_LCO_DRAG_WARN_TIME;
 296                 }
 297                 warn_delay = ao_lco_drag_warn_time - now;
 298         }
 299         if (delay > warn_delay)
 300                 delay = warn_delay;
 301         return delay;
 302 }
 303
 304 static void
 305 ao_lco_drag_monitor(void)
 306 {
 307         uint16_t        delay = ~0;
 308         uint16_t        now;
 309
 310         for (;;) {
 311                 PRINTD("Drag monitor active %d delay %d\n", ao_lco_drag_active, delay);
 312                 if (delay == (uint16_t) ~0)
 313                         ao_sleep(&ao_lco_drag_active);
 314                 else
 315                         ao_sleep_for(&ao_lco_drag_active, delay);
 316
 317                 delay = ~0;
 318                 if (!ao_lco_drag_active)
 319                         continue;
 320
 321                 now = ao_time();
 322                 delay = ao_lco_drag_button_check(now, delay);
 323                 delay = ao_lco_drag_warn_check(now, delay);
 324                 delay = ao_lco_drag_beep_check(now, delay);
 325
 326                 /* check to see if there's anything left to do here */
 327                 if (!ao_lco_fire_down && !ao_lco_drag_race && !ao_lco_drag_beep_count) {
 328                         delay = ~0;
 329                         ao_lco_drag_active = 0;
 330                 }
 331         }
 332 }
 333
 334 static void
 335 ao_lco_input(void)
 336 {
 337         static struct ao_event  event;
 338         int8_t          dir, new_pad;
 339         int16_t         new_box;
 340
 341         ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200));
 342         for (;;) {
 343                 ao_event_get(&event);
 344                 PRINTD("event type %d unit %d value %d\n",
 345                        event.type, event.unit, event.value);
 346                 switch (event.type) {
 347                 case AO_EVENT_QUADRATURE:
 348                         switch (event.unit) {
 349                         case AO_QUADRATURE_PAD:
 350                                 if (!ao_lco_armed) {
 351                                         dir = (int8_t) event.value;
 352                                         new_pad = ao_lco_pad;
 353                                         do {
 354                                                 new_pad += dir;
 355                                                 if (new_pad > AO_PAD_MAX_CHANNELS)
 356                                                         new_pad = 0;
 357                                                 if (new_pad < 0)
 358                                                         new_pad = AO_PAD_MAX_CHANNELS;
 359                                                 if (new_pad == ao_lco_pad)
 360                                                         break;
 361                                         } while (!ao_lco_pad_present(ao_lco_box, new_pad));
 362                                         if (new_pad != ao_lco_pad) {
 363                                                 ao_lco_pad = new_pad;
 364                                                 ao_lco_set_display();
 365                                         }
 366                                 }
 367                                 break;
 368                         case AO_QUADRATURE_BOX:
 369                                 if (!ao_lco_armed) {
 370                                         dir = (int8_t) event.value;
 371                                         new_box = ao_lco_box;
 372                                         do {
 373                                                 if (new_box == AO_LCO_BOX_DRAG) {
 374                                                         if (dir < 0)
 375                                                                 new_box = ao_lco_max_box;
 376                                                         else
 377                                                                 new_box = ao_lco_min_box;
 378                                                 } else {
 379                                                         new_box += dir;
 380                                                         if (new_box > ao_lco_max_box)
 381                                                                 new_box = AO_LCO_BOX_DRAG;
 382                                                         else if (new_box < ao_lco_min_box)
 383                                                                 new_box = AO_LCO_BOX_DRAG;
 384                                                 }
 385                                                 if (new_box == ao_lco_box)
 386                                                         break;
 387                                         } while (!ao_lco_box_present(new_box));
 388                                         if (ao_lco_box != new_box) {
 389                                                 ao_lco_box = new_box;
 390                                                 ao_lco_pad = 1;
 391                                                 if (ao_lco_box != AO_LCO_BOX_DRAG)
 392                                                         ao_lco_channels[ao_lco_box] = 0;
 393                                                 ao_lco_set_display();
 394                                         }
 395                                 }
 396                                 break;
 397                         }
 398                         break;
 399                 case AO_EVENT_BUTTON:
 400                         switch (event.unit) {
 401                         case AO_BUTTON_ARM:
 402                                 ao_lco_armed = event.value;
 403                                 PRINTD("Armed %d\n", ao_lco_armed);
 404                                 if (ao_lco_armed) {
 405                                         if (ao_lco_drag_race) {
 406                                                 uint8_t box;
 407
 408                                                 for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) {
 409                                                         if (ao_lco_selected[box]) {
 410                                                                 ao_wakeup(&ao_lco_armed);
 411                                                                 break;
 412                                                         }
 413                                                 }
 414                                         } else {
 415                                                 memset(ao_lco_selected, 0, sizeof (ao_lco_selected));
 416                                                 if (ao_lco_pad != 0 && ao_lco_box != AO_LCO_BOX_DRAG)
 417                                                         ao_lco_selected[ao_lco_box] = (1 << (ao_lco_pad - 1));
 418                                                 else
 419                                                         ao_lco_armed = 0;
 420                                         }
 421                                 }
 422                                 ao_wakeup(&ao_lco_armed);
 423                                 break;
 424                         case AO_BUTTON_FIRE:
 425                                 if (ao_lco_armed) {
 426                                         ao_lco_fire_down = 0;
 427                                         ao_lco_firing = event.value;
 428                                         PRINTD("Firing %d\n", ao_lco_firing);
 429                                         ao_wakeup(&ao_lco_armed);
 430                                 } else {
 431                                         if (event.value) {
 432                                                 if (ao_lco_box == AO_LCO_BOX_DRAG) {
 433                                                         ao_lco_fire_down = 1;
 434                                                         ao_lco_fire_tick = ao_time();
 435                                                         ao_lco_drag_active = 1;
 436                                                 }
 437                                                 if (ao_lco_drag_race) {
 438                                                         if (ao_lco_pad != 0 && ao_lco_box != AO_LCO_BOX_DRAG) {
 439                                                                 ao_lco_selected[ao_lco_box] ^= (1 << (ao_lco_pad - 1));
 440                                                                 PRINTD("Toggle box %d pad %d (pads now %x) to drag race\n",
 441                                                                        ao_lco_pad, ao_lco_box, ao_lco_selected[ao_lco_box]);
 442                                                                 ao_lco_drag_beep(ao_lco_pad);
 443                                                         }
 444                                                 }
 445                                                 ao_wakeup(&ao_lco_drag_active);
 446                                         } else {
 447                                                 ao_lco_fire_down = 0;
 448                                                 if (ao_lco_drag_active)
 449                                                         ao_wakeup(&ao_lco_drag_active);
 450                                         }
 451                                 }
 452                                 break;
 453                         }
 454                         break;
 455                 }
 456         }
 457 }
 458
 459 static AO_LED_TYPE      continuity_led[AO_LED_CONTINUITY_NUM] = {
 460 #ifdef AO_LED_CONTINUITY_0
 461         AO_LED_CONTINUITY_0,
 462 #endif
 463 #ifdef AO_LED_CONTINUITY_1
 464         AO_LED_CONTINUITY_1,
 465 #endif
 466 #ifdef AO_LED_CONTINUITY_2
 467         AO_LED_CONTINUITY_2,
 468 #endif
 469 #ifdef AO_LED_CONTINUITY_3
 470         AO_LED_CONTINUITY_3,
 471 #endif
 472 #ifdef AO_LED_CONTINUITY_4
 473         AO_LED_CONTINUITY_4,
 474 #endif
 475 #ifdef AO_LED_CONTINUITY_5
 476         AO_LED_CONTINUITY_5,
 477 #endif
 478 #ifdef AO_LED_CONTINUITY_6
 479         AO_LED_CONTINUITY_6,
 480 #endif
 481 #ifdef AO_LED_CONTINUITY_7
 482         AO_LED_CONTINUITY_7,
 483 #endif
 484 };
 485
 486 static uint8_t
 487 ao_lco_get_channels(uint8_t box, struct ao_pad_query *query)
 488 {
 489         int8_t                  r;
 490
 491         r = ao_lco_query(box, query, &ao_lco_tick_offset[box]);
 492         if (r == AO_RADIO_CMAC_OK) {
 493                 ao_lco_channels[box] = query->channels;
 494                 ao_lco_valid[box] = AO_LCO_VALID_LAST | AO_LCO_VALID_EVER;
 495         } else
 496                 ao_lco_valid[box] &= ~AO_LCO_VALID_LAST;
 497         PRINTD("ao_lco_get_channels(%d) rssi %d valid %d ret %d offset %d\n", box, ao_radio_cmac_rssi, ao_lco_valid[box], r, ao_lco_tick_offset[box]);
 498         ao_wakeup(&ao_pad_query);
 499         return ao_lco_valid[box];
 500 }
 501
 502 static void
 503 ao_lco_update(void)
 504 {
 505         if (ao_lco_box != AO_LCO_BOX_DRAG) {
 506                 uint8_t previous_valid = ao_lco_valid[ao_lco_box];
 507
 508                 if (ao_lco_get_channels(ao_lco_box, &ao_pad_query) & AO_LCO_VALID_LAST) {
 509                         if (!(previous_valid & AO_LCO_VALID_EVER)) {
 510                                 if (ao_lco_pad != 0)
 511                                         ao_lco_pad = ao_lco_pad_first(ao_lco_box);
 512                                 ao_lco_set_display();
 513                         }
 514                         if (ao_lco_pad == 0)
 515                                 ao_lco_set_display();
 516                 }
 517         }
 518 }
 519
 520 static void
 521 ao_lco_box_reset_present(void)
 522 {
 523         ao_lco_min_box = 0xff;
 524         ao_lco_max_box = 0x00;
 525         memset(ao_lco_box_mask, 0, sizeof (ao_lco_box_mask));
 526 }
 527
 528 static void
 529 ao_lco_box_set_present(uint8_t box)
 530 {
 531         if (box < ao_lco_min_box)
 532                 ao_lco_min_box = box;
 533         if (box > ao_lco_max_box)
 534                 ao_lco_max_box = box;
 535         if (box >= AO_PAD_MAX_BOXES)
 536                 return;
 537         ao_lco_box_mask[MASK_ID(box)] |= 1 << MASK_SHIFT(box);
 538 }
 539
 540 static void
 541 ao_lco_search(void)
 542 {
 543         int8_t          r;
 544         int8_t          try;
 545         uint8_t         box;
 546         uint8_t         boxes = 0;
 547
 548         ao_lco_box_reset_present();
 549         ao_lco_set_pad(0);
 550         for (box = 0; box < AO_PAD_MAX_BOXES; box++) {
 551                 if ((box % 10) == 0)
 552                         ao_lco_set_box(box);
 553                 for (try = 0; try < 3; try++) {
 554                         ao_lco_tick_offset[box] = 0;
 555                         r = ao_lco_query(box, &ao_pad_query, &ao_lco_tick_offset[box]);
 556                         PRINTD("box %d result %d offset %d\n", box, r, ao_lco_tick_offset[box]);
 557                         if (r == AO_RADIO_CMAC_OK) {
 558                                 ++boxes;
 559                                 ao_lco_box_set_present(box);
 560                                 ao_lco_set_pad(boxes % 10);
 561                                 ao_delay(AO_MS_TO_TICKS(30));
 562                                 break;
 563                         }
 564                 }
 565         }
 566         if (ao_lco_min_box <= ao_lco_max_box)
 567                 ao_lco_box = ao_lco_min_box;
 568         else
 569                 ao_lco_min_box = ao_lco_max_box = ao_lco_box = 0;
 570         memset(ao_lco_valid, 0, sizeof (ao_lco_valid));
 571         memset(ao_lco_channels, 0, sizeof (ao_lco_channels));
 572         ao_lco_pad = 1;
 573         ao_lco_set_display();
 574 }
 575
 576 static void
 577 ao_lco_igniter_status(void)
 578 {
 579         uint8_t         c;
 580         uint8_t         t = 0;
 581
 582         for (;;) {
 583                 ao_sleep(&ao_pad_query);
 584                 PRINTD("RSSI %d VALID %d\n", ao_radio_cmac_rssi, ao_lco_box == AO_LCO_BOX_DRAG ? -1 : ao_lco_valid[ao_lco_box]);
 585                 if (ao_lco_box == AO_LCO_BOX_DRAG) {
 586                         ao_led_off(AO_LED_RED|AO_LED_GREEN|AO_LED_AMBER);
 587                         for (c = 0; c < AO_LED_CONTINUITY_NUM; c++)
 588                                 ao_led_off(continuity_led[c]);
 589                 } else {
 590                         if (!(ao_lco_valid[ao_lco_box] & AO_LCO_VALID_LAST)) {
 591                                 ao_led_on(AO_LED_RED);
 592                                 ao_led_off(AO_LED_GREEN|AO_LED_AMBER);
 593                                 continue;
 594                         }
 595                         if (ao_radio_cmac_rssi < -90) {
 596                                 ao_led_on(AO_LED_AMBER);
 597                                 ao_led_off(AO_LED_RED|AO_LED_GREEN);
 598                         } else {
 599                                 ao_led_on(AO_LED_GREEN);
 600                                 ao_led_off(AO_LED_RED|AO_LED_AMBER);
 601                         }
 602                         if (ao_pad_query.arm_status)
 603                                 ao_led_on(AO_LED_REMOTE_ARM);
 604                         else
 605                                 ao_led_off(AO_LED_REMOTE_ARM);
 606
 607                         for (c = 0; c < AO_LED_CONTINUITY_NUM; c++) {
 608                                 uint8_t status;
 609
 610                                 if (ao_lco_drag_race) {
 611                                         if (ao_lco_selected[ao_lco_box] & (1 << c) && t)
 612                                                 ao_led_on(continuity_led[c]);
 613                                         else
 614                                                 ao_led_off(continuity_led[c]);
 615                                 } else {
 616                                         if (ao_pad_query.channels & (1 << c))
 617                                                 status = ao_pad_query.igniter_status[c];
 618                                         else
 619                                                 status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN;
 620                                         if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN)
 621                                                 ao_led_on(continuity_led[c]);
 622                                         else
 623                                                 ao_led_off(continuity_led[c]);
 624                                 }
 625                         }
 626                         t = 1-t;
 627                 }
 628         }
 629 }
 630
 631 static void
 632 ao_lco_arm_warn(void)
 633 {
 634         for (;;) {
 635                 while (!ao_lco_armed)
 636                         ao_sleep(&ao_lco_armed);
 637                 ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200));
 638                 ao_delay(AO_MS_TO_TICKS(200));
 639         }
 640 }
 641
 642 static struct ao_task ao_lco_input_task;
 643 static struct ao_task ao_lco_monitor_task;
 644 static struct ao_task ao_lco_arm_warn_task;
 645 static struct ao_task ao_lco_igniter_status_task;
 646
 647 static void
 648 ao_lco_monitor(void)
 649 {
 650         uint16_t                delay;
 651         uint8_t                 box;
 652
 653         ao_lco_search();
 654         ao_add_task(&ao_lco_input_task, ao_lco_input, "lco input");
 655         ao_add_task(&ao_lco_arm_warn_task, ao_lco_arm_warn, "lco arm warn");
 656         ao_add_task(&ao_lco_igniter_status_task, ao_lco_igniter_status, "lco igniter status");
 657         ao_add_task(&ao_lco_drag_task, ao_lco_drag_monitor, "drag race");
 658         for (;;) {
 659                 PRINTD("monitor armed %d firing %d\n",
 660                        ao_lco_armed, ao_lco_firing);
 661
 662                 if (ao_lco_armed && ao_lco_firing) {
 663                         ao_lco_ignite();
 664                 } else {
 665                         ao_lco_update();
 666                         if (ao_lco_armed) {
 667                                 for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) {
 668                                         if (ao_lco_selected[box]) {
 669                                                 PRINTD("Arming box %d pads %x\n",
 670                                                        box, ao_lco_selected[box]);
 671                                                 if (ao_lco_valid[box] & AO_LCO_VALID_EVER) {
 672                                                         ao_lco_arm(box, ao_lco_selected[box], ao_lco_tick_offset[box]);
 673                                                         ao_delay(AO_MS_TO_TICKS(10));
 674                                                 }
 675                                         }
 676                                 }
 677                         }
 678                 }
 679                 if (ao_lco_armed && ao_lco_firing)
 680                         delay = AO_MS_TO_TICKS(100);
 681                 else
 682                         delay = AO_SEC_TO_TICKS(1);
 683                 ao_sleep_for(&ao_lco_armed, delay);
 684         }
 685 }
 686
 687 #if DEBUG
 688 void
 689 ao_lco_set_debug(void)
 690 {
 691         ao_cmd_decimal();
 692         if (ao_cmd_status == ao_cmd_success)
 693                 ao_lco_debug = ao_cmd_lex_i != 0;
 694 }
 695
 696 __code struct ao_cmds ao_lco_cmds[] = {
 697         { ao_lco_set_debug,     "D <0 off, 1 on>\0Debug" },
 698         { ao_lco_search,        "s\0Search for pad boxes" },
 699         { 0, NULL }
 700 };
 701 #endif
 702
 703 void
 704 ao_lco_init(void)
 705 {
 706         ao_add_task(&ao_lco_monitor_task, ao_lco_monitor, "lco monitor");
 707 #if DEBUG
 708         ao_cmd_register(&ao_lco_cmds[0]);
 709 #endif
 710 }