File content as of revision 5:38b853bb1afa:

/* LF28A Simulator - Main
 * Copyright (c) 2011 Wim Huiskamp
 *
 * Released under the MIT License: http://mbed.org/license/mit
 *
 * version 0.2 Initial Release
*/
#include "mbed.h"
#include "BusDefines.h"
#include "PCF8574_DataBus.h"
#include "PCF8574_AddressBus.h"
#include "PCF8574_EnableBus.h"
#include "MBED_ControlBus.h"
#include "HDSP253X_Display.h"
#include "Status_Display.h"
#include "Keyboard.h"
#include "STANAG_Codes.h"
#include "Host_Coms.h"
  
// Debug stuff
#define __DEBUG  
#include "Dbg.h"

//#define __TESTCODE  


// Software version
#define LF28A_SOFTWARE_DESCRIPTOR    "LF28A Testsoftware"
#define LF28A_SW_VERSION_MAJOR       0
#define LF28A_SW_VERSION_MINOR       2
#define LF28A_COPYRIGHT_DESCRIPTOR   "(C)TNO 2011"

// Laser Time-out in Seconds
//#define LF28A_RANGE_TIME_OUT_S           1      
//#define LF28A_DESIG_TIME_OUT_S  (3*60 + 20)      
//Test only:
#define LF28A_RANGE_TIME_OUT_S           5      
#define LF28A_DESIG_TIME_OUT_S          10      
 
// mbed Interface Hardware definitions
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut heartbeatLED(LED4);

// Host PC Communication channels
Serial pc(USBTX, USBRX);
Host_Coms host_coms(pc);

//I2C Bus
I2C i2c(D_SDA, D_SCL);

// CDA Interface Hardware definitions
PCF8574_DataBus    databus = PCF8574_DataBus(i2c, D_I2C_DATA_BUS);  //Copy constructors..
PCF8574_AddressBus addressbus = PCF8574_AddressBus(i2c, D_I2C_ADDR_BUS);
PCF8574_EnableBus  enablebus = PCF8574_EnableBus(i2c, D_I2C_ENA_BUS);
MBED_ControlBus  controlbus = MBED_ControlBus(D_WR, D_RD, D_DTR, D_CDBUF, D_CDINT, D_FIRE);

// CDA Hardware definitions
HDSP253X_Display LF28A_display = HDSP253X_Display(databus, addressbus, enablebus, controlbus);
Status_Display LF28A_status = Status_Display(databus, enablebus, controlbus);
Keyboard LF28A_keyboard = Keyboard(databus, enablebus, controlbus);


//Enums for LF28A Statemachines
enum Mode { IDLE,
            INIT_ENTRY, INIT, INIT_EXIT,  
            DESIG_ENTRY, DESIG_DISP, DESIG_LASER, DESIG_EXIT,
            RANGE_ENTRY, RANGE_DISP, RANGE_FIRE, RANGE_EXIT,
            CODE_ENTRY, CODE_DISP, CODE_EDIT, CODE_EXIT,
            ADDR_ENTRY, ADDR, ADDR_EXIT,
            FREQ_ENTRY, FREQ, FREQ_EXIT,
            PATH_ENTRY, PATH, PATH_EXIT };                                    

enum RangeSelect { RNG_F, RNG_L };

enum MessageToHost { LFPON, LFRDY_0, LFRDY_1, LFSTA_D, LFSTA_R, LFSTA_C, LFSTA_A, LFSTA_F, LFSTA_P, LFIDL };
//enum MessageToHost { LFPON, LFRDY_0, LFRDY_1, LFSTA, LFIDL };
enum MessageFromHost { LFRES, LFRNG };

//Typedef for Laser Range finder data
typedef struct {
 uint16_t first;
 uint16_t last;
 RangeSelect select; 
} Range_t;
 
// Variables for Statemachine
Mode mode; 
Brightness brightness, graticule; 
Key_Code keycode;
Range_t range;
bool laser;                           // Laser On/Off
bool hostResetCmd;
MessageToHost messageToHost;
STANAG_Codes STANAG_codes;

// Variables for Heartbeat and Status monitoring
Ticker heartbeat;
bool heartbeatflag=false;

// Laser Time-out
Timer timer;

// Cycle Timer
Timer cycletimer;
int cyclecount = 0;
const int maxcount = 1000000;

// Local functions
void clear_screen() {
//ANSI Terminal Commands
    pc.printf("\x1B[2J");
    pc.printf("\x1B[H");
}

void version_string() {
    pc.printf("\r\r");            
    pc.printf("%s\r", LF28A_SOFTWARE_DESCRIPTOR);
    pc.printf("Version v%1d.%1d\r", LF28A_SW_VERSION_MAJOR, LF28A_SW_VERSION_MINOR);
    pc.printf("Build on %s at %s\r", __DATE__, __TIME__);    
    pc.printf("%s\r", LF28A_COPYRIGHT_DESCRIPTOR);
    pc.printf("\r\n");            
}

void init_interfaces() {
// Init Host PC communication, default is 9600
    pc.baud(D_BAUDRATE);
      
// Init LF28A CDA I/F hardware
    i2c.frequency(100000);  
//    pc.printf("SCL0 H, L = 0x%x, 0x%x\r", LPC_I2C0->I2SCLH, LPC_I2C0->I2SCLL);
//    pc.printf("SCL1 H, L = 0x%x, 0x%x\r", LPC_I2C1->I2SCLH, LPC_I2C1->I2SCLL);
//    pc.printf("SCL2 H, L = 0x%x, 0x%x\r", LPC_I2C2->I2SCLH, LPC_I2C2->I2SCLL);

// Reset LF28A CDA
// NOTE: On LF28A CDA the Reset* pin is connected to the Display and to the Latches.
//       That implies they are all reset when the Reset* pin is used !
    enablebus.reset(LOW);   
    wait_ms(50);    
    enablebus.reset(HIGH);             
  
    //Done, Tell me about it
    myled1 = 1;
    DBG("Init Interfaces Done, Main, Step = %d\r", 10);    
}
      
void BITE() {
  int count;
  
    LF28A_status.set_brightness(BRT_MED);           
    LF28A_status.NoGo(LED_ON);
    
    for (count=0; count<5; count++) {
      LF28A_display.locate(0);
      LF28A_display.printf("BITE  - ");    
      LF28A_display.locate(0);    
      LF28A_display.printf("BITE  \ ");    
      LF28A_display.locate(0);        
      LF28A_display.printf("BITE  | ");        
      LF28A_display.locate(0);    
      LF28A_display.printf("BITE  / ");    
      LF28A_display.locate(0);    
      LF28A_display.printf("BITE  - ");                
    };

    LF28A_display.locate(0);    
    LF28A_display.printf("BITE  OK");    
    LF28A_status.NoGo(LED_OFF);  
    
    wait(2.0);          
    
    LF28A_display.cls();  
//    LF28A_status.set_brightness(D_STATUS_LED_BRIGHT_OFF); 

    //Done, Tell me about it                        
    DBG("BITE Done, Main, Step = %d\r", 30);    
}

void lamp_test() {
    LF28A_status.set_brightness(BRT_LOW);       
    LF28A_status.lamptest(LED_ON);  // All LEDs On, including Backlight and NoGo

    LF28A_display.locate(0);
    LF28A_display.printf("XXXXXXXX");

    wait(2.0);      

    LF28A_status.lamptest(LED_OFF);  // All LEDs Off, including Backlight and NoGo
    
    LF28A_display.cls();  
//    LF28A_status.set_brightness(D_STATUS_LED_BRIGHT_OFF); 

    //Done, Tell me about it          
    //DBG("Lamp Test Done, Main, Step = %d\r", 20);    
}


void init_state() {
    // Default modes and settings after Power On or Host Reset
    brightness = BRT_LOW;   // Should be BRT_OFF, but then you dont see anything.. 
    graticule  = BRT_LOW;   // Should be BRT_OFF, but then you dont see anything.. 

    range.last = 0;
    range.first = 0;
    range.select = RNG_F;

    laser = false;
    timer.stop();
    timer.reset();

// Read Config File if needed..
// Reload STANAG Codes ?
    STANAG_codes.setCodeIdx(0);
    STANAG_codes.setDigitIdx(0);
                
    // Init Status LEDs 
    LF28A_status.lamptest(LED_OFF); // All LEDs off   
    LF28A_status.backlight(LED_ON);
    LF28A_status.set_brightness(brightness);          
   
    // Init Alphanumeric Display
    LF28A_display.cls();                    // Clear display and Cursor home
    LF28A_display.set_flash_mode(false);    // Disable flashing digits                                     
    
    //Done, Tell me about it 
    //DBG("Init Done, Originator = %d\r", hostResetCmd);               
}


void grat_bright_selector() {
  int result;
  
  switch (graticule) {
    case BRT_OFF : graticule = BRT_LOW;
                   break;
    case BRT_LOW : graticule = BRT_MED;
                   break;
    case BRT_MED : graticule = BRT_HIGH;
                   break;
    case BRT_HIGH : graticule = BRT_OFF;
                   break;
    default:       // Oops, we should never end up here....
                   graticule = BRT_LOW;
                   result = -1;
                   break;                   
  } //end switch              
     
  //Done, Tell me about it 
  //DBG("Grat Bright Chnge, val = %d\r", graticule);
}

void disp_bright_selector() {
  int result;
  
  switch (brightness) {
    case BRT_OFF : brightness = BRT_LOW;
                   LF28A_status.set_brightness(BRT_LOW);    
                   break;
    case BRT_LOW : brightness = BRT_MED;
                   LF28A_status.set_brightness(BRT_MED);        
                   break;
    case BRT_MED : brightness = BRT_HIGH;
                   LF28A_status.set_brightness(BRT_HIGH);    
                   break;
    case BRT_HIGH : brightness = BRT_OFF;
                   LF28A_status.set_brightness(BRT_OFF);        
                   break;
    default:       // Oops, we should never end up here....
                   brightness = BRT_LOW;
                   LF28A_status.set_brightness(BRT_LOW);                       
                   result = -1;
                   break;                   
  } //end switch              
        
  //Done, Tell me about it 
  //DBG("Disp Bright Chng, val = %d\r", brightness);
}

void range_selector() {
  int result;
  
  switch (range.select) {
    case RNG_F : range.select = RNG_L;
                 break;
    case RNG_L : range.select = RNG_F;
                 break;
    default:     // Oops, we should never end up here....
                 range.select = RNG_F;
                 result = -1;
                 break;                 
  } //end switch              
     
  //Done, Tell me about it   
  //DBG("Rng Chng, val = %d\r", range.select);
}

// Heartbeat monitor
void pulse() {
  heartbeatLED = !heartbeatLED;
}

void heartbeat_start() {
  heartbeat.attach(&pulse, 0.5);
  heartbeatflag = true;
}

void heartbeat_stop() {
  heartbeat.detach();
  heartbeatflag = false;
}

// Construct and Send messages to Host PC. Need to complete this !!
void SendHostMessage(MessageToHost messageToHost) {
  int result;

  switch (messageToHost) {
    case LFPON:    host_coms.sendMessage("$LFPON");
                   break;
                   
    case LFRDY_0:  // No hostResetCmd
                   host_coms.sendMessage("$LFRDY,0");
                   //pc.printf("$LFRDY,0*00\r\n");
                   break;
                   
    case LFRDY_1:  // hostResetCmd
                   host_coms.sendMessage("$LFRDY,1");
//                   pc.printf("$LFRDY,1*00\r\n");
                   break;

    case LFSTA_D:  // Designate
                   host_coms.sendMessage("$LFSTA,D,%1d,%04d,%1d", laser, STANAG_codes.getCode(), graticule);    
//                   pc.printf("$LFSTA,D,0,1234,1*00\r\n");
                   break;

    case LFSTA_R:  // Range
                   host_coms.sendMessage("$LFSTA,R,%1d,%04d,%1d", laser, STANAG_codes.getCode(), graticule);        
//                   pc.printf("$LFSTA,D,0,1234,1*00\r\n");
                   break;

    case LFSTA_C:  // Code
                   host_coms.sendMessage("$LFSTA,C,%1d,%04d,%1d", laser, STANAG_codes.getCode(), graticule);    
//                   pc.printf("$LFSTA,D,0,1234,1*00\r\n");
                   break;

    case LFSTA_A:
    case LFSTA_F:
    case LFSTA_P:        
    case LFIDL:    // No Message
                   break;

    default:     // Oops, we should never end up here....

                   result = -1;
                   break;                 
  } //end switch              
     
  //Done, Tell me about it  
  //DBG("To Host Msg, type = %d\r", messageToHost);
}


//------------------------------------------------------------//
// Testing Stuff                                              //   
//------------------------------------------------------------//
#ifdef __TESTCODE
#include "Testloop.h"
#endif
//------------------------------------------------------------//
// End Testing Stuff                                          //   
//------------------------------------------------------------//


int main() {
    int address, result;
   
    init_interfaces();
    
    heartbeat_start();
          
    clear_screen(); 
    version_string();    
        
#ifndef __TESTCODE
    lamp_test();
    
    BITE();

    DBG("Start Main Loop, Step = %d\r", 50);    

    // Prepare main State Machine 
    mode = INIT_ENTRY;    // Start with Init
    hostResetCmd = false; // Start with regular PowerOn reset   

//Test cycletime
    cycletimer.start();      
    cycletimer.reset();
          
    // Main Controlloop:
    //   Check keyboard input and respond as required in the current device mode
    //   Check Host commands and respond as required
    while(1) {
        
        // Handle mode statemachine
        switch (mode) {
        
          // INIT MODEs - Init LF28A after Power On or Host Reset
          case INIT_ENTRY:  // Transitional state
                            mode = INIT;
                            break;

          case INIT:        // Init LF28A after Power On or Host Reset
                            init_state(); 

                           // Inform Host that Init has completed
                            if (hostResetCmd) {
                              // Host initiated a Reset
                              SendHostMessage(LFRDY_1);
                              hostResetCmd = false;                            
                            }
                            else {
                              // Regular PowerOn Reset                                                        
                              SendHostMessage(LFRDY_0);                            
                            }
                                                          
                            mode = INIT_EXIT;                            
                            break;                            
        
          case INIT_EXIT:  // Transitional state
                            mode = DESIG_ENTRY;
                            break;
        
          // DESIG MODEs - Laser Designator
          case DESIG_ENTRY:  // Transitional state

                            // Status LEDs
                            LF28A_status.LED(LED_DESIG, LED_ON);

                            //Display current STANAG Code
                            LF28A_display.locate (0);                      
                            LF28A_display.printf ("    %04d",STANAG_codes.getCode() );                      
                          
                            // Inform Host of change
                            SendHostMessage(LFSTA_D);  
                          
                            mode = DESIG_DISP;
                            break;

          case DESIG_DISP:
                            // Check and handle Keyboard
                            if (LF28A_keyboard.readable()) {
                              keycode = LF28A_keyboard.getkey();

                              switch (keycode) {
                                case KEY_GRAT_RT: grat_bright_selector();
                                                  // Inform Host of change
                                                  SendHostMessage(LFSTA_D);  
                                                  break;

                                case KEY_BRIGHT:  disp_bright_selector();
                                                  break;                              

                                case KEY_MODE:    mode = DESIG_EXIT;
                                                  break;
                                                  
                                case KEY_FIRE:    // Check and handle Fire key
                                                  
                                                  // Toggle Laser On/Off
                                                  if (laser) {
                                                    // Laser is On, Switch Off
                                                    laser=false;
                                                    timer.stop();  // Stop time-out timer
                                                    timer.reset();
                                
                                                    // Status LEDs
                                                    LF28A_status.LED(LED_LASER, LED_OFF);                                                    
                                                  }
                                                  else {
                                                    // Laser is Off, Switch On
                                                    laser=true;
                                                    timer.start(); // Start time-out timer

                                                    // Status LEDs
                                                    LF28A_status.LED(LED_LASER, LED_ON);                                                                                                         
                                                  };
                                                  
                                                  // Inform Host of change (laser on/off)
                                                  SendHostMessage(LFSTA_D);  
                                
                                                  break;                              
                                                                                

                                default:          // Ignore other keys
                                                  break;                                                            
                              }; //End Keyswitch            
                            }; // End Keyread
                              
                            // Check Automatic Time-out for Laser
                            if (laser) {
                              // Laser is On, Test Time-out to Switch Off
                              if (timer.read() > LF28A_DESIG_TIME_OUT_S) {
                                laser=false;   // Laser Off
                                timer.stop();  // Stop time-out timer
                                timer.reset();

                                // Status LEDs
                                LF28A_status.LED(LED_LASER, LED_OFF); 

                                // Inform Host of change (laser off)
                                SendHostMessage(LFSTA_D);
                              }                                                                                     
                            };
                                                       
                            break;                              

          case DESIG_EXIT:  // Transitional state

                            // Check Laser
                            if (laser) {
                              // Laser is On, Switch Off
                              laser=false;   // Laser Off
                              timer.stop();  // Stop time-out timer
                              timer.reset();
                                
                              // Status LEDs
                              LF28A_status.LED(LED_LASER, LED_OFF); 
                            };
          
                            // Status LEDs
                            LF28A_status.LED(LED_DESIG, LED_OFF);        
          
                            mode = RANGE_ENTRY;
                            break;                              

          // RANGE MODEs
          case RANGE_ENTRY: // Transitional state
          
                            // Status LEDs
                            LF28A_status.LED(LED_RANGE, LED_ON);
                  
                            //Display current STANAG Code ...Is this correct on entry ??
                            LF28A_display.locate (0);                      
                            LF28A_display.printf ("    %04d",STANAG_codes.getCode() );                      
                            
                            // Inform Host of change
                            SendHostMessage(LFSTA_R);  
                           
                            mode = RANGE_DISP;
                            break;

          case RANGE_DISP:
                            // Check and handle Keyboard
                            if (LF28A_keyboard.readable()) {
                              keycode = LF28A_keyboard.getkey();

                              switch (keycode) {
                                case KEY_GRAT_RT: grat_bright_selector();

                                                  // Inform Host of change
                                                  SendHostMessage(LFSTA_R);  

                                                  break;

                                case KEY_BRIGHT:  disp_bright_selector();
                                                  break;                              

                                case KEY_MODE:    mode = RANGE_EXIT;
                                                  break;                              

                                case KEY_F_L_UP : //Toggle First/Last
                                                  range_selector();                                

                                                  //Display current Range
                                                  if (range.select == RNG_F) {
                                                    LF28A_display.locate (0);                      
                                                    LF28A_display.printf ("F   %04d", range.first );
                                                  }
                                                  else {
                                                    LF28A_display.locate (0);                      
                                                    LF28A_display.printf ("L   %04d", range.last);                                                     
                                                  };
                                                                                                 
                                                  // Inform Host of change ??
                                                  //SendHostMessage(LFSTA);  
                                                  
                                                  break;

                                case KEY_FIRE:    // Check and handle Fire key
                                                  
                                                  // Toggle Laser On/Off
                                                  if (laser) {
                                                    // Laser is On, Switch Off
                                                    laser=false;
                                                    timer.stop();  // Stop time-out timer
                                                    timer.reset();                                                    

                                                    // Status LEDs
                                                    LF28A_status.LED(LED_LASER, LED_OFF);                                                    
                                                  }
                                                  else {
                                                    // Laser is Off, Switch On
                                                    laser=true;
                                                    timer.start(); // Start time-out timer

                                                    // Status LEDs
                                                    LF28A_status.LED(LED_LASER, LED_ON);                                                                                                         
                                                  };
                                                  
                                                  // Inform Host of change (laser on/off)
                                                  SendHostMessage(LFSTA_R);  
                                
                                                  break;                              
  
                                default:          // Ignore other keys
                                                  break;                                                              
                              }; //End Keyswitch            
                            }; // End Keyread          

                            // Check Automatic Time-out for Laser
                            if (laser) {
                              // Laser is On, Test Time-out to Switch Off
                              if (timer.read() > LF28A_RANGE_TIME_OUT_S) {
                                laser=false;   // Laser Off
                                timer.stop();  // Stop time-out timer
                                timer.reset();                                

                                // Status LEDs
                                LF28A_status.LED(LED_LASER, LED_OFF); 

                                // Inform Host of change (laser off)
                                SendHostMessage(LFSTA_R);
                              }                                                                                     
                            };

                            break;                              

          case RANGE_EXIT:  // Transitional state

                            // Check Laser
                            if (laser) {
                              // Laser is On, Switch Off
                              laser=false;   // Laser Off
                              timer.stop();  // Stop time-out timer
                              timer.reset();                              

                              // Status LEDs
                              LF28A_status.LED(LED_LASER, LED_OFF); 
                            };
        
                            // Status LEDs
                            LF28A_status.LED(LED_RANGE, LED_OFF);

                            mode = CODE_ENTRY;
                            break;                              


          // CODE MODEs
          case CODE_ENTRY:  // Transitional state

                            // Status LEDs
                            LF28A_status.LED(LED_CODE, LED_ON);          
          
                            //Enable flashing Code                                
                            LF28A_display.set_flash_mode(true);
 
                            //Display current STANAG Code
                            LF28A_display.locate (0);                      
                            LF28A_display.printf ("    %04d",STANAG_codes.getCode() );                      
 
                            // Inform Host of change
                            SendHostMessage(LFSTA_C);  
                            
                            mode = CODE_DISP;
                            break;

          case CODE_DISP:
                            // Check and handle Keyboard
                            if (LF28A_keyboard.readable()) {
                              keycode = LF28A_keyboard.getkey();

                              switch (keycode) {
                                case KEY_GRAT_RT: grat_bright_selector();

                                                  // Inform Host of change
                                                  SendHostMessage(LFSTA_C);  
                                
                                                  break;

                                case KEY_BRIGHT:  disp_bright_selector();
                                                  break;                              

                                case KEY_MODE:    mode = CODE_EXIT;
                                                  break;                              

                                case KEY_EDIT_PATH:
                                                  mode = CODE_EDIT;
                                                  
                                                  // Enable current flashing digit
                                                  LF28A_display.set_char_flash_state(true, 4 + STANAG_codes.getDigitIdx());                          
                                                                                                                                                         
                                                  break;                              

                                case KEY_F_L_UP : //Incr STANAG code idx
                                                  STANAG_codes.incCodeIdx();
                                                  
                                                  //Display current STANAG Code
                                                  LF28A_display.locate (0);                      
                                                  LF28A_display.printf ("    %04d",STANAG_codes.getCode() );                      
                                                                                                   

                                                  // Inform Host of change ??
                                                  //SendHostMessage(LFSTA);  
                                                  
                                                  break;
                                                    
                                default:          // Ignore other keys
                                                  break;                                
                              }; //End Keyswitch            
                            }; //End Keyread            

                            break;                              

          case CODE_EDIT:
                            // Check and handle Keyboard
                            if (LF28A_keyboard.readable()) {
                              keycode = LF28A_keyboard.getkey();

                              switch (keycode) {
                                case KEY_GRAT_RT: //Cursor Right;
                                                  // Disable current flashing digit
                                                  LF28A_display.set_char_flash_state(false, 4 + STANAG_codes.getDigitIdx());                          

                                                  // Incr selected digit
                                                  STANAG_codes.incDigitIdx();

                                                  // Enable current flashing digit
                                                  LF28A_display.set_char_flash_state(true, 4 + STANAG_codes.getDigitIdx());                          
                                
                                                  break;

                                case KEY_BRIGHT:  disp_bright_selector();
                                                  break;                              

                                case KEY_MODE:    // Done, exit Code mode
                                                                                    
                                                  // Disable current flashing digit
                                                  LF28A_display.set_char_flash_state(false, 4 + STANAG_codes.getDigitIdx());                          
                                
                                                  // Inform Host of change
                                                  SendHostMessage(LFSTA_C);  
                                                  
                                                  mode = CODE_EXIT;                                                                                  
                                                  
                                                  break;                              

                                case KEY_EDIT_PATH: // Done, return to Code display mode
                                
                                                  // Disable current flashing digit
                                                  LF28A_display.set_char_flash_state(false, 4 + STANAG_codes.getDigitIdx());                          
                                 
                                                  // Inform Host of change
                                                  SendHostMessage(LFSTA_C);  
                                
                                                  mode = CODE_DISP;
                                                  
                                                  break;                              

                                case KEY_F_L_UP : //Incr current digit
                                                  STANAG_codes.incDigit();

                                                  // Display Current STANAG Code
                                                  LF28A_display.locate (0);                      
                                                  LF28A_display.printf ("    %04d",STANAG_codes.getCode() );                                                                       
                                
                                                  // Inform Host of change or wait until done editing ??
                                                  //SendHostMessage(LFSTA);  
                                                                                  
                                                  break;                              
                                                                                                
                                default:          // Ignore other keys
                                                  break;                                                              
                              }; //End Keyswitch            
                            }; //End Keyread                      

                            break;                              

          case CODE_EXIT:  // Transitional state
          
                            // Status LEDs
                            LF28A_status.LED(LED_CODE, LED_OFF); 

                            //Disable flashing Code                                
                            LF28A_display.set_flash_mode(false);
                                            
                            mode = DESIG_ENTRY;
                            break;                              

          // ADDR MODEs 
          case ADDR_ENTRY: // Transitional state                    
          case ADDR:       // Transitional state                              
          case ADDR_EXIT:  // Transitional state          
          // FREQ MODEs 
          case FREQ_ENTRY: // Transitional state                    
          case FREQ:       // Transitional state                              
          case FREQ_EXIT:  // Transitional state          
          // PATH MODEs
          case PATH_ENTRY: // Transitional state                    
          case PATH:       // Transitional state                              
          case PATH_EXIT:  // Transitional state          
                        
          default:          // Oops, we should never end up here....try to recover
                            mode = INIT_ENTRY;
                            hostResetCmd = false;
                            
                            result = -1;
                            DBG("Error - Result = %d\r", result);                            
                            break;                                                          
        }; //End mode statemachine switch              


        // Handle Host commands
        //  Reset command 
        //  Range data         

#if(0)
        // Just for Info, lets see how fast this cycle is...
        cyclecount++;
        if (cyclecount == maxcount) {
          pc.printf("Freq = %d Hz\r", (cyclecount * 1000) / cycletimer.read_ms());
          cyclecount = 0;          
          cycletimer.reset();      
        }
#endif
      
    } // end while(1)


#else
//testing stuff
    testloop();
    
#endif

    DBG("I'll be back...\r\r");
}