File content as of revision 5:38b853bb1afa:

/* Host_Coms - Communications with Host PC
 *
 * Copyright (c) 2011 Wim Huiskamp
 *
 * Released under the MIT License: http://mbed.org/license/mit
 *
 * version 0.2 Initial Release
*/
#include "mbed.h"
#include "Host_Coms.h"


/** Create a Host Coms object connected to the proper channel
 *
 * @param  Serial serial channel to connect to 
 * @brief 
*/
Host_Coms::Host_Coms(Serial &hostChannel) : _hostChannel(hostChannel) {
  
  _init();  
}   


/** Send a formatted string to the Host PC
 * @param
 * @returns int message length
 */
int Host_Coms::sendString(char * format, ...) {
   va_list args;
   va_start (args, format);
   int rv;

   rv = _hostChannel.printf(format, args);

   va_end (args);

   return rv;
}



/** Send a formatted message to the Host PC
 * @param
 * @returns int message length
 */
int Host_Coms::sendMessage(char * format, ...) {
   va_list args;
   va_start (args, format);
   int rv=vsprintf (_sendBuffer, format, args);

//   _hostChannel.printf("%s:len=%d\r\n", _sendBuffer, strlen(_sendBuffer));
   
   _hostChannel.printf("%s*%02X\r\n", _sendBuffer, _calcChecksum());

   va_end (args);

   return (rv + 5);  // length + * + checksum + <CR> + <NL>
}

/** Compute the XOR checksum on the message
 * @param
 * @returns int checksum
 */
int Host_Coms::_calcChecksum() {
  int checksum = 0;
  int i, last;

  //_sendBuffer[0] should be '$' and is ignored in checksum  
  last = strlen(_sendBuffer);
  for (i=1; i<last; i++) {
    checksum ^= (int) _sendBuffer[i];
  }  
  
  return checksum;
}


/** Parse the LFRNG message and decode the fields
 * @param
 * @returns bool valid message 
 * @brief
 */
bool Host_Coms::_parseLFRNG(char *data) {
   char *token;
   int tokenfield;
   bool result=true;
   
//   char hostmessage[] = "1234,5678,0";
//   strcpy(hostmessage, "1234,5678,0");
      
   token = strtok(data, ","); // get first token, separator is ','
   tokenfield = 1;   

   while ((token != NULL) && (tokenfield <= 3)) {     
     switch (tokenfield) {
        case 1:
                // Range First
                _hostChannel.printf("#%d: %s\r\n", tokenfield, token);
                
                // Convert and perform sanity check
                rangeFirst = atoi(token);
                if ((rangeFirst < 0) || (rangeFirst > 9999)) {
                  rangeFirst = 0;
                  result=false;
                }
                _hostChannel.printf("#%d: int=%04d\r\n", tokenfield, rangeFirst);                
                break;

        case 2:
                // Range Last
                _hostChannel.printf("#%d: %s\r\n", tokenfield, token);

                // Convert and perform sanity check
                rangeLast = atoi(token);
                if ((rangeLast < 0) || (rangeLast > 9999)) {
                  rangeLast = 0;
                  result=false;
                }

                _hostChannel.printf("#%d: int=%04d\r\n", tokenfield, rangeLast);                                
                break;

        case 3:
                // Multiple Returns
                _hostChannel.printf("#%d: %s\r\n", tokenfield, token);

                // Convert and perform sanity check
                mult = atoi(token);
                if ((mult < 0) || (mult > 1)) {
                  mult = 0;
                  result=false;
                }

                _hostChannel.printf("#%d: int=%1d\r\n", tokenfield, mult);                
                break;

        default:
                break;
     } // switch
     
     token = strtok(NULL, ","); // Get next token
     tokenfield++;     
   } // while

   // Could do additional sanity check on correct number of fields
      
   return result;
}

/** Parse the LFRES message and decode the fields
 * @param
 * @returns bool valid message
 * @brief
 */
bool Host_Coms::_parseLFRES(char *data) {
   char *token;
   int tokenfield;
   int STANAG_Code;
   bool result=true;
      
   token = strtok(data, ","); // get first token, separator is ','
   tokenfield = 1;   
   
   while ((token != NULL) && (tokenfield <= D_HOST_STANAG_CODES)) {   
     switch (tokenfield) {
        case 1:
        case 2:        
        case 3:        
        case 4:
        case 5:                        
        case 6:
        case 7:        
        case 8:        
        case 9:
        case 10:                        
        case 11:
        case 12:        
        case 13:        
        case 14:
        case 15:                        
        case 16:
        case 17:        
        case 18:        
        case 19:
        case 20:                        
                _hostChannel.printf("#%d: %s\r\n", tokenfield, token);
                
                // STANAG_Code[i] and sanity check
                STANAG_Code = atoi(token);
                if ((STANAG_Code < 0) || (STANAG_Code > 9999)) {
//                if (STANAG_Code < 0) || (STANAG_Code > 1999) {    // Check on limited code value            
                  STANAG_Code = 0;
                  result = false;
                }
                 
                STANAG_Codes[tokenfield - 1] = STANAG_Code;                 

                _hostChannel.printf("#%d: %s\r\n", tokenfield, STANAG_Codes[tokenfield - 1]);

                break;

        default:
                break;
     } // switch
     
     token = strtok(NULL, ","); // Get next token
     tokenfield++;     
   } // while
   
   // Could do additonal sanity check on correct number of fields
   
   return result;
}


/** Receive and Parse the Host message
 * @param
 * @returns Host_Mess message type that was received
 * @brief 
 *              Check LF28A Host channel and Parse received data for valid messages.
 *              Since the parser is a state machine, partial data may
 *              be supplied where the next time this method is called, the
 *              rest of the partial data will complete the message and 
 *              parsing will begin. A typical message is constructed as:
 *
 *                  $LFCMD,DDDD,DDDD,....DD*CS<CR><LF>
 *
 *              Where:
 *                        '$'            Hex 24 (Start of Message)
 *                        'LFCMD'        LF28A command or data
 *                        ',DDDD'        Zero or more data fields
 *                        '*CS'          Checksum field (optional)
 *                        <CR><LF>       Hex 0D 0A (End of Message)
 *
 *              When a complete message is received, this function sends the
 *              LF28A command and data to the parseCommand methods for
 *              individual field parsing. The received data will be stored
 *              in a datastructure. The return value informs the caller which
 *              command has been received to decide on proper action. 
 */
Host_Mess Host_Coms::parseMessage() {
  char data;
  static int cmdIdx = 0;                    // index for received command
  static int datIdx = 0;                    // index for received data
  static int computedChecksum = 0;          // computed from the received data   
  static int receivedChecksum = 0;          // checksum that was part of the received data
  static bool checksumReceived = false;     // received data included the optional checksum
  static bool messageComplete = false;      // message has been received that is ready for decoding
  Host_Mess   messageType;                  // valid decoded message type   
  static LFP_State parseState = LFP_STATE_SOM;
     
  // init while loop, no valid decoded message is available initially
  messageType = HOST_CMD_NONE;
  
  // Continue reading from host as long as data is available or until a
  // valid message has been received that needs to be processed first.
  while ((_hostChannel.readable()) && (messageType == HOST_CMD_NONE)) {
    data = _hostChannel.getc();

    // The main state machine which processes individual characters
    // until a complete message has been received that is ready for decoding.
    switch(parseState) {
      // Search for start of message '$'
      case LFP_STATE_SOM :
                            if(data == '$') {
                              cmdIdx = 0;                   // reset index
                              computedChecksum = 0;        // reset checksum
                              receivedChecksum = 0; 
                              messageComplete = false;     // restart                               
                              checksumReceived = false;    // restart                                                             
                              parseState = LFP_STATE_CMD;  // next state
                              
_hostChannel.printf("#start cmd\r\n");                              
                            }
  
  
                            break;

      // Retrieve command field
      case LFP_STATE_CMD :
                            switch (data) {
                              case ',':  
                                        // terminate command
                                        _command[cmdIdx] = '\0';

                                        // update checksum
                                        computedChecksum ^= (int) data;

                                        // goto get data state
                                        datIdx = 0;          // reset index                                                                                
                                        parseState = LFP_STATE_DATA;
_hostChannel.printf("#, end cmd\r\n");                                        
                                        break;
                              case '*':  
                                        // terminate command
                                        _command[cmdIdx] = '\0';

                                        // goto get checksum state 
                                        checksumReceived = true;                                                                               
                                        parseState = LFP_STATE_CS1;   
_hostChannel.printf("#* end cmd\r\n");
                                        break;                                        

                              case '\r':  
                              case '\n':                                
                                        // terminate command
                                        _command[cmdIdx] = '\0';
                                        
                                        // done, decode data and restart
                                        messageComplete = true;
                                        parseState = LFP_STATE_SOM;
_hostChannel.printf("#cr nl end cmd\r\n");
                                        break;
                                        
                              case '$' : 
                                        // unexpected restart 
                                        cmdIdx = 0;                     // reset index
                                        computedChecksum = 0;        // reset checksum 
_hostChannel.printf("#$ restart cmd\r\n");                                        
                                        break;

                              default :  
                                        // store command
                                        _command[cmdIdx++] = data;                                        
                                        
                                        // update checksum
                                        computedChecksum ^= (int) data;

                                        // Check for command overflow
                                        if (cmdIdx > D_HOST_MAX_CMD_LEN) {
                                          parseState = LFP_STATE_SOM;  // something went wrong, restart
                                        }
                                        
_hostChannel.printf("#in cmd\r\n");                                        
                                        break;                                                                               
                                        
                           } // switch (data)
                           break;

        // Store data and check for end of sentence or checksum flag
        case LFP_STATE_DATA :
                           switch (data) {
                              case '*':  
                                        // terminate data
                                        _data[datIdx] = '\0';

                                        // goto get checksum state
                                        checksumReceived = true;
                                        parseState = LFP_STATE_CS1;
_hostChannel.printf("#* end data\r\n");                                                                                                                                                                   
                                        break;                                        

                              case '\r':  
                              case '\n':                                
                                        // terminate data
                                        _data[datIdx] = '\0';
                                        
                                        // done, decode data and restart
                                        messageComplete = true;
                                        parseState = LFP_STATE_SOM;
_hostChannel.printf("#cr nl end data\r\n");                                                                                    
                                        break;

                              case '$' : 
                                        // unexpected restart 
                                        cmdIdx = 0;                     // reset index
                                        computedChecksum = 0;        // reset checksum 

                                        parseState = LFP_STATE_CMD;  // restart                                      
_hostChannel.printf("#$ in data, restart cmd\r\n");                                        
                                        break;
                                        
                              default :  
                                        // store data
                                        _data[datIdx++] = data;                                        

                                        // update checksum
                                        computedChecksum ^= (int) data;
                                        
                                        // Check for data overflow
                                        if (datIdx > D_HOST_MAX_DATA_LEN) {
                                          parseState = LFP_STATE_SOM;  // something went wrong, restart
                                        }
                                        
_hostChannel.printf("#in data\r\n");                                                                                
                                        break;                                                                               
                                        
                           } // switch (data)
                           break;
        
        // Handle checksum (part1) from sentence
        case LFP_STATE_CS1 :
        
                           if ((data >= '0') && (data <= '9')) {
                             receivedChecksum = (data - '0') << 4;
                             parseState = LFP_STATE_CS2;
                           }
                           else if ((data >= 'a') && (data <= 'f')) {
                             receivedChecksum = ((data - 'a') + 10) << 4;
                             parseState = LFP_STATE_CS2;                             
                           }
                           else if ((data >= 'A') && (data <= 'F')) {
                             receivedChecksum = ((data - 'A') + 10) << 4;
                             parseState = LFP_STATE_CS2;                             
                           }
                           else {
                             // not a valid checksum
                             parseState = LFP_STATE_SOM;  // something went wrong, restart
                           }
                      
                           break;

        // Handle checksum (part2) from sentence
        case LFP_STATE_CS2 :
        
                           if ((data >= '0') && (data <= '9')) {
                             receivedChecksum |= (data - '0');
                             
                             // done, decode data and restart
                             messageComplete = true;
                             parseState = LFP_STATE_SOM;                            
                           }
                           else if ((data >= 'a') && (data <= 'f')) {
                             receivedChecksum |= (data - 'a') + 10;

                             // done, decode data and restart
                             messageComplete = true;
                             parseState = LFP_STATE_SOM;                            
                           }
                           else if ((data >= 'A') && (data <= 'F')) {
                             receivedChecksum |= (data - 'A') + 10;
                             parseState = LFP_STATE_SOM;                             

                             // done, decode data and restart
                             messageComplete = true;
                             parseState = LFP_STATE_SOM;                                                        
                           }
                           else {
                             // not a valid checksum
                             parseState = LFP_STATE_SOM;  // something went wrong, restart
                           }
                      
                           break;
        

        // Something went wrong, restart
        default :
                           parseState = LFP_STATE_SOM;    
    } // switch (_parseState)
    
    
    // Check if done with this message
    if (messageComplete) {
_hostChannel.printf("#complete\r\n");                                                                                    
      if (!checksumReceived) {      
        // No checksum received, process message
        messageType = _parseCommand(_command, _data);     
      }
      else if (computedChecksum == receivedChecksum) {        
        // Correct checksum, process message              
        messageType = _parseCommand(_command, _data);
      }
      else {
       // Ignore messages with an invalid checksum
        messageType = HOST_CMD_NONE;               
      }
      
      // done processing this received message
      messageComplete = false;
    }

                                
  } // while readable and no valid message available

  
  // inform the caller when a valid message is available
  return messageType;
} // _parseMessage


/** Receive and Parse the Host command from the message
 * @param
 * @returns Host_Mess valid command type
 * @brief 
 *          Process LFCMD message - Use the _command and _data strings to call the
 *          appropriate command sentence data processor.
 *          Return which command has been received to let caller decide on proper action !!
*/
Host_Mess Host_Coms::_parseCommand(char *command, char *data) {
  Host_Mess result = HOST_CMD_NONE; 

  //
  // LFRES
  //
  if (strcmp(command, "LFRES") == NULL) {
    if (_parseLFRES(data))
      result = HOST_CMD_RES;
  }
  //
  // LFRNG
  //
  else if (strcmp(command, "LFRNG") == NULL) {
    if (_parseLFRNG(data))
      result = HOST_CMD_RNG;
  } 

  return result;
}



/** Init Host_Coms
 * @param
 * @returns 
 */
void Host_Coms::_init(void) {
  int i;
  
  rangeFirst=0;
  rangeLast=0;
  mult=0;
  
  for (i=0; i<D_HOST_STANAG_CODES; i++) {
    STANAG_Codes[i]=0;
  }  
}