/////////////////////////////////////////////////////////////////////////////////////////
//
// Accelerometer Demonstration for Freescale DEMOQE Development Board
// --------------------------------------------------------------------------------------
//
// CodeWarrior V6.0 for MCUs
//
/////////////////////////////////////////////////////////////////////////////////////////

#include <hidef.h> /* for EnableInterrupts macro */
#include "derivative.h" /* include peripheral declarations */
#include "accelerometer.h" /* include main program defines and declarations */

/////////////////////////////////////////////////////////////////////////////////////////
// ICS_FEI
// --------------------------------------------------------------------------------------
// intitializes ICS for FEI mode with DCOH
/////////////////////////////////////////////////////////////////////////////////////////
void ICS_FEI() {

  if (NVICSTRM != 0xFF)
    ICSTRM = NVICSTRM;				    	// load trim value if NV location not blank
  else
    ICSTRM = 0xAD;                  // use a default value if NVICSTRM is blank
  ICSC1 = ICSC1_FEI;
  ICSC2 = ICSC2_FEI;
  ICSSC = ICSSC_FEI;
  while (ICSC1_CLKS != ICSSC_CLKST) {}  // wait for clk state to match clk select
} //end InitICG

/////////////////////////////////////////////////////////////////////////////////////////
// InitKBI
// --------------------------------------------------------------------------------------
// initializes 4 switches on DEMO or EVB as KBI's  
/////////////////////////////////////////////////////////////////////////////////////////
void InitKBI() {
// Enable KBI1P[3:2] as interrupt
  KBI1PE = KBI_SW;
  KBI1SC = 0b00000110;
/*               ||||
                 |||+---- KBIMOD = KBI detection mode: 0=edge only
                 ||+----- KBIE   = KBI int enable: 1=enabled
                 |+------ KBACK  = KBI int acknowledge: 1=clr IRQF
                 +------- KBF    = KBI flag
*/
}

/////////////////////////////////////////////////////////////////////////////////////////
// InitSCI
// --------------------------------------------------------------------------------------
// initializes SCI1 to specified baudrate  
/////////////////////////////////////////////////////////////////////////////////////////
void InitSCI(word baud) {

  SCI1BD = baud;  // set baud
} //end InitSCI
  
/////////////////////////////////////////////////////////////////////////////////////////
// RecChar & SendChar
// --------------------------------------------------------------------------------------
// receives/sends an ascii char on SCI1 at preset baudrate  
/////////////////////////////////////////////////////////////////////////////////////////
char RecChar() {
  byte rec_char;

  if (SCI1S1_RDRF)  // 1st half of RDRF clear procedure
    rec_char = SCI1D;  // 2nd half of RDRF clear procedure
  SCI1C2_RE = 1;    // enable Rx
  while(!SCI1S1_RDRF){  };
  rec_char = SCI1D; // get recieved character
  SendChar((char) rec_char); // echo received character
  return (char) SCI1D;
} //end RecChar  

void SendChar(char s_char) {
  
  SCI1C2 = 0x08;    // enable Tx
  while(!SCI1S1_TDRE){ }
  SCI1D = (byte) s_char;   // 2nd half of TDRE clear procedure
} //end SendChar

/////////////////////////////////////////////////////////////////////////////////////////
// SendMsg
// --------------------------------------------------------------------------------------
// sends an ascii string out SCI1 at preset baudrate  
/////////////////////////////////////////////////////////////////////////////////////////
void SendMsg(char msg[]) {
  byte i=0;
  char nxt_char;
  
  SCI1C2 = 0x08;    // enable Tx
  nxt_char = msg[i++];
  while(nxt_char != 0x00) {
    while(!SCI1S1_TDRE){}
    SCI1D = (byte) nxt_char; // 2nd half of TDRE clear procedure
    nxt_char = msg[i++];
  } //end while((SCI1D
} //end SendMsg

/////////////////////////////////////////////////////////////////////////////////////////
// hex2bcd
// --------------------------------------------------------------------------------------
// converts hexadecimal word into a binary-coded decimal word  
/////////////////////////////////////////////////////////////////////////////////////////
word hex2bcd(word hex){
byte dec[4],i;
word bcd;


  for (i=0;i<4;i++){
    dec[i] = (byte) (hex%10);
    hex = (word) (hex/10);
  }
  
  if (hex>0){
    bcd=0xffff;
  }else{
    bcd=(word)((word)(dec[3]<<12) + (word)(dec[2]<<8) + (dec[1]<<4) + dec[0]);
  }
  return bcd;
} //end hex2bcd
  
/////////////////////////////////////////////////////////////////////////////////////////
// asc2byte & asc2word
// --------------------------------------------------------------------------------------
// converts an ascii string of 2 or 4 numeric chars into a byte or word  
/////////////////////////////////////////////////////////////////////////////////////////
byte asc2byte(char n_asc) {
byte n;

  n = (byte)(n_asc - 0x30);      //convert from ascii to int
  if(n > 0x09)           // if num is $a or larger...
    n -= 0x07;           // ...sub $7 to correct
  if(n > 0x0f)           // if lower case was used...
    n -= 0x20;           // ...sub $20 to correct
  if(n > 0x0f)           // if non-numeric character...
    n = 0x00;            // ...default to '0'
  return n;
} //end asc2num

word asc2word(byte n_asc[2]) {
word n,n2;

// assumes n_asc[0] is MSB, n_asc[1] is LSB
  n = (word)(n_asc[0] - 0x30);   //convert from ascii to int
  if(n > 0x09)           // if num is $a or larger...
    n -= 0x07;           // ...sub $7 to correct
  if(n > 0x0f)           // if lower case was used...
    n -= 0x20;           // ...sub $20 to correct
  if(n > 0x0f)           // if non-numeric character...
    n = 0x00;            // ...default to '0'
  n = (word)(n<<8);              // shift into high byte
  n2 = (word)(n_asc[1] - 0x30);  //convert from ascii to int
  if(n2 > 0x09)          // if num is $a or larger...
    n2 -= 0x07;          // ...sub $7 to correct
  if(n2 > 0x0f)          // if lower case was used...
    n2 -= 0x20;          // ...sub $20 to correct
  if(n2 > 0x0f)          // if non-numeric character...
    n2 = 0x00;           // ...default to '0'
  n += n2;               //     
  return n;
} //end asc2word

/////////////////////////////////////////////////////////////////////////////////////////
// byte2asc & word2asc
// --------------------------------------------------------------------------------------
// converts a byte or word into an ascii string of 2 or 4 chars
/////////////////////////////////////////////////////////////////////////////////////////
char * byte2asc(byte num, byte base) {
byte n;

  if (base){
    n=(byte)(hex2bcd(num));
  }else{
    n=num;
  } //end if (base)
  n_str[0] = (byte)((n>>0x04)+0x30);  // convert MSN to ascii
  if(n_str[0]>0x39)           // if MSN is $a or larger...
    n_str[0]+=0x07;           // ...add $7 to correct
  n_str[1] = (byte)((n&0x0f)+0x30);   // convert LSN to ascii
  if(n_str[1]>0x39)           // if LSN is $a or larger...
    n_str[1]+=0x07;           // ...add $7 to correct
  n_str[2] = 0x00;            // add line feed
  return  (char *) n_str;
} //end byte2asc

char * word2asc(word num, byte base) {
word n;

  if (base){
    n=hex2bcd(num);
  }else{
    n=num;
  } //end if (base)

  n_str[0] = (byte)((n>>12)+0x30);    // convert MSN to ascii
  if(n_str[0]>0x39)           // if MSN is $a or larger...
    n_str[0]+=0x07;           // ...add $7 to correct
  n_str[1] = (byte)(((n>>8)&0x0f)+0x30);   // convert 2nd MSN to ascii
  if(n_str[1]>0x39)           // if LSN is $a or larger...
    n_str[1]+=0x07;           // ...add $7 to correct
  n_str[2] = (byte)(((n>>4)&0x0f)+0x30);   // convert 2nd MSN to ascii
  if(n_str[2]>0x39)           // if LSN is $a or larger...
    n_str[2]+=0x07;           // ...add $7 to correct
  n_str[3] = (byte)((n&0x0f)+0x30);   // convert 2nd MSN to ascii
  if(n_str[3]>0x39)           // if LSN is $a or larger...
    n_str[3]+=0x07;           // ...add $7 to correct
  n_str[4] = 0x00;    // add line feed
  return  (char *) n_str;
    
} //end word2asc

/////////////////////////////////////////////////////////////////////////////////////////
// StartTPM & StopTPM
// --------------------------------------------------------------------------------------
// Starts and stops TPM1 at busclk rate
/////////////////////////////////////////////////////////////////////////////////////////
void StartTPM(byte PS){
  TPM1SC = (byte)(0x08 | (0x07&PS));
  StartCount = TPM1CNT;
}// end StartTPM

word StopTPM(void){
  StopCount = (word)(TPM1CNT - StartCount);
  TPM1SC = 0;
  return StopCount;
}// end StopTPM

/////////////////////////////////////////////////////////////////////////////////////////
// PeriphInit