File content as of revision 77:3273a1d44741:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2015 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mbed.h"
#include "ble/BLE.h"
#include "ble/services/HeartRateService.h"
#include "ble/services/BatteryService.h"
#include "ble/services/DeviceInformationService.h"

DigitalOut led(p21);
Serial s(USBTX, USBRX);
SPI spi(p25, p28, p29); // mosi, miso, sclk
DigitalOut STE(p24);

Timer t;

#define CONTROL0        0x00
#define LED2STC         0x01
#define LED2ENDC        0x02
#define LED2LEDSTC      0x03
#define LED2LEDENDC     0x04
#define ALED2STC        0x05
#define ALED2ENDC       0x06
#define LED1STC         0x07
#define LED1ENDC        0x08
#define LED1LEDSTC      0x09
#define LED1LEDENDC     0x0a
#define ALED1STC        0x0b
#define ALED1ENDC       0x0c
#define LED2CONVST      0x0d
#define LED2CONVEND     0x0e
#define ALED2CONVST     0x0f
#define ALED2CONVEND    0x10
#define LED1CONVST      0x11
#define LED1CONVEND     0x12
#define ALED1CONVST     0x13
#define ALED1CONVEND    0x14
#define ADCRSTSTCT0     0x15
#define ADCRSTENDCT0    0x16
#define ADCRSTSTCT1     0x17
#define ADCRSTENDCT1    0x18
#define ADCRSTSTCT2     0x19
#define ADCRSTENDCT2    0x1a
#define ADCRSTSTCT3     0x1b
#define ADCRSTENDCT3    0x1c
#define PRPCOUNT        0x1d
#define CONTROL1        0x1e
#define SPARE1          0x1f
#define TIAGAIN         0x20
#define TIA_AMB_GAIN    0x21
#define LEDCNTRL        0x22
#define CONTROL2        0x23
#define SPARE2          0x24
#define SPARE3          0x25
#define SPARE4          0x26
#define SPARE4          0x26
#define RESERVED1       0x27
#define RESERVED2       0x28
#define ALARM           0x29
#define LED2VAL         0x2a
#define ALED2VAL        0x2b
#define LED1VAL         0x2c
#define ALED1VAL        0x2d
#define LED2ABSVAL      0x2e
#define LED1ABSVAL      0x2f
#define DIAG            0x30

void RegisterWrite(uint8_t address, uint32_t data) {
    STE = 0;
    spi.write(address); // send address to device
    spi.write((data >> 16) & 0xFF); // writing top 8 bits
    spi.write((data >> 8) & 0xFF); // writing middle 8 bits
    spi.write(data & 0xFF); // writing bottom 8 bits    
    STE = 1;
}

uint32_t RegisterRead(uint8_t address) {       
    uint32_t data=0;
    STE = 0;
    spi.write(address); // sends address to device
    data |= (spi.write(0)<<16); // reading top 8 bits data
    data |= (spi.write(0)<<8); // reading middle 8 bits  data
    data |= spi.write(0); // reading bottom 8 bits data
    STE = 1;
    return data; // returns with 24 bits of read data
}

int32_t ADCRead( uint8_t address )
{
    uint32_t adc;
    int32_t r;
    //float r;
    adc=RegisterRead( address );  
    if ( (adc&(1<<21)) ) adc|=0xFFC00000;
    //r = 1.2 * adc / (1<<21);
    r = *((int32_t*)&adc);
    return r;
}
    

void AFE4490Initialize() {
    RegisterWrite(CONTROL0, 0x000008); //SW_RST
    RegisterWrite(CONTROL0, 0x000000);
    RegisterWrite(LED2STC, 0x0017C0);
    RegisterWrite(LED2ENDC, 0x001F3E);
    RegisterWrite(LED2LEDSTC, 0x001770);
    RegisterWrite(LED2LEDENDC, 0x001F3F);
    RegisterWrite(ALED2STC, 0x000050);
    RegisterWrite(ALED2ENDC, 0x0007CE);
    RegisterWrite(LED1STC, 0x000820);
    RegisterWrite(LED1ENDC, 0x000F9E);
    RegisterWrite(LED1LEDSTC, 0x0007D0);
    RegisterWrite(LED1LEDENDC, 0x000F9F);
    RegisterWrite(ALED1STC, 0x000FF0);
    RegisterWrite(ALED1ENDC, 0x00176E);
    RegisterWrite(LED2CONVST, 0x000006);
    RegisterWrite(LED2CONVEND, 0x0007CF);
    RegisterWrite(ALED2CONVST, 0x0007D6);
    RegisterWrite(ALED2CONVEND, 0x000F9F);
    RegisterWrite(LED1CONVST, 0x000FA6);
    RegisterWrite(LED1CONVEND, 0x00176F);
    RegisterWrite(ALED1CONVST, 0x001776);
    RegisterWrite(ALED1CONVEND, 0x001F3F);
    RegisterWrite(ADCRSTSTCT0, 0x000000);
    RegisterWrite(ADCRSTENDCT0, 0x000005);
    RegisterWrite(ADCRSTSTCT1, 0x0007D0);
    RegisterWrite(ADCRSTENDCT1, 0x0007D5);
    RegisterWrite(ADCRSTSTCT2, 0x000FA0);
    RegisterWrite(ADCRSTENDCT2, 0x000FA5);
    RegisterWrite(ADCRSTSTCT3, 0x001770);
    RegisterWrite(ADCRSTENDCT3, 0x001775);
    RegisterWrite(PRPCOUNT, 0x001F3F);
    RegisterWrite(CONTROL1, 0x000102); /* Average 3 */
    RegisterWrite(TIAGAIN, 0x004102);  /* 0b1 000 001 00000 010 - stg2=1.5 5p */
    RegisterWrite(LEDCNTRL, 0x010606);
    /* TX_REF - 0.5 V */
    RegisterWrite(CONTROL2, 0b0000100000000000000000);
    RegisterWrite(RESERVED1, 0x000000);
    RegisterWrite(RESERVED2, 0x000000);
    RegisterWrite(ALARM, 0x000000);  
    s.printf("Initialization Complete.\r\n"); 
}

float mltplr = 0.00000057220459;
int idx = 0;
int nDelay=2000;
char inpBuffer[32];
int iDC=0;
int iGER=0;
int iREG=0;

char *command;
char data[32];
void executeChange() {
  s.printf("cmd: %s\r\n", inpBuffer);
  command = strtok(data, " ");
  RegisterWrite(CONTROL0, 0x000000);
  strcpy(data, (const char *)inpBuffer);
  int control = (int) strtol(command, NULL, 16);
  RegisterWrite(LEDCNTRL, control);
  command = strtok(NULL, " ");
  int gain1 = (int) strtol(command, NULL, 16);
  RegisterWrite(TIAGAIN, gain1);
  command = strtok(NULL, " ");
  if (command) {
    int gain2 = (int) strtol(command, NULL, 16);
    RegisterWrite(TIA_AMB_GAIN, gain2);
  }
  RegisterWrite(CONTROL0, 0x000001);
}

float convert(int data) {
  return data * mltplr;
}

#define counts 1000
unsigned int IRV[counts];
unsigned int REDV[counts];
unsigned int SPT[counts];
int spidx=0;

void spo2()
{
   //int counts=1000;
   float r;
   long red,ir,redMax,redMin,irMax,irMin;
   long redS=0,irS=0;
   unsigned long redAC_sq=0;
   unsigned long irAC_sq=0;
   long redDC,irDC;
   long irAC,redAC;
   float spo2;
   int i;
   
   RegisterWrite(CONTROL0, 0x000001);
   redMax=redMin=RegisterRead(LED2VAL);
   irMax=irMin=RegisterRead(LED1VAL);
   for ( i=0;i<counts;i++ )
   {
       REDV[i]=RegisterRead(LED2VAL);
       IRV[i]=RegisterRead(LED1VAL);
//       s.printf("READ %u %u\r\n",REDV[i],IRV[i]);
       if ( redMax<REDV[i] ) redMax=REDV[i];
       if ( redMin>REDV[i] ) redMin=REDV[i];
       if ( irMax<IRV[i] ) irMax=IRV[i];
       if ( irMin>IRV[i] ) irMin=IRV[i];
       wait_us(200);
   }
   
   redS=0;
   irS=0;
   for ( i=0;i<counts;i++ )
   {   
      redS+=REDV[i];
      irS+=IRV[i];
   }
   
   redDC = redS/i;
   irDC = irS/i;
   for ( i=0;i<counts;i++ )
   {
       redAC_sq += pow(((long)(REDV[i]-redDC)),2.0);
       irAC_sq += pow(((long)(IRV[i]-irDC)),2.0);
   }
   redAC=sqrt((float)redAC_sq/i);
   irAC=sqrt((float)irAC_sq/i);
   
   s.printf("redDC=%ld\r\n",redDC);
   s.printf("redMax=%ld\r\n",redMax);
   s.printf("redMin=%ld\r\n",redMin);
   s.printf("redAC2=%ld\r\n",redAC_sq);
   s.printf("redAC=%ld\r\n",redAC);
   
   s.printf("ir=%ld\r\n",ir);
   s.printf("irMax=%ld\r\n",irMax);
   s.printf("irMin=%ld\r\n",irMin);
   s.printf("irAC2=%ld\r\n",irAC_sq);
   s.printf("irAC=%ld\r\n",irAC);
   
   r=(float)((float)redAC/redDC)/(float)((float)irAC/irDC);
   spo2=110-25*r;
   s.printf("spo2=%.3lf\r\n\r\n",spo2);
       
}

int samples=500;
int start=0;
int ind = 0;   
int redRead;
int32_t greenRead=0;

int32_t fRedRead;
int32_t fGreenRead;

short int nSTAGE2EN1=0;
short int nSTG2GAIN1=0;
short int nCF_LED1=0;
short int nRF_LED1=0;

short int nLED1=0;
short int nLED2=0;
short int nLED_RANGE=1;
float dFSC=50.0;

long int nTIAGAIN=0;

void setTiaGain( void )
{
    /* STG2 enabled */
    s.printf("setTiaGain:\r\n");
    s.printf("  RF_LED1:%d CF_LED1:%d STG2GAIN1:%d STAGE2EN1:%d\r\n",
                nRF_LED1,nCF_LED1,nSTG2GAIN1,nSTAGE2EN1);
    nTIAGAIN=0;      
    nTIAGAIN|=((nSTAGE2EN1&1)<<14)&0b100000000000000;
    nTIAGAIN|=((nSTG2GAIN1&7)<<8)&0b11100000000;
    nTIAGAIN|=((nCF_LED1&0x1F)<<3)&0b11111000;
    nTIAGAIN|=nRF_LED1&0x7;
    s.printf("  TIAGAIN:%04X\r\n",nTIAGAIN);
}

void printTG(void)
{
    int v,cf;
    s.printf("TIAGAIN:0x%04X\r\n",nTIAGAIN);
    if ( nTIAGAIN&(1<<15) ) {
        s.printf("sep_gain_enabled ");
    } else {
        s.printf("sep_gain_disabled ");        
    }
    if ( nTIAGAIN&(1<<14) ) {
        s.printf("stage2_enabled ");
        
        v=(nTIAGAIN>>8)&7;
        if ( v==0 ) s.printf("0dB (0X) ");
        if ( v==1 ) s.printf("3.5dB (1.5X) ");
        if ( v==2 ) s.printf("6dB (2X) ");
        if ( v==3 ) s.printf("9.5dB (3X) ");
        if ( v==4 ) s.printf("12dB (4X) ");              
      
    } else {
        s.printf("stage2_disabled ");
    }    
    v=(nTIAGAIN>>3)&0x1F;
    cf=5;
    if ( v&0b1 ) cf+=5;
    if ( v&0b10 ) cf+=15;
    if ( v&0b100 ) cf+=25;
    if ( v&0b1000 ) cf+=50;
    if ( v&0b10000 ) cf+=150;
    s.printf("cf=%dpF ",cf);
    v=(nTIAGAIN&7);
    if (v==0) s.printf("rf=500k");
    if (v==1) s.printf("rf=250k");
    if (v==2) s.printf("rf=100k");
    if (v==3) s.printf("rf=50k");
    if (v==4) s.printf("rf=25k");
    if (v==5) s.printf("rf=10k");
    if (v==6) s.printf("rf=1M");
    s.printf("\r\n");
    s.printf("LED1: %02X (%.2lf mA) LED2: %02X (%.2lf mA)\r\n",
                nLED1,nLED1*dFSC/256,nLED2,nLED2*dFSC/256);
}

void loop_o() {
    
  /*greenRead=RegisterRead(LED2VAL);
  redRead=RegisterRead(LED1VAL);
  RegisterRead(ALED2VAL);
  RegisterRead(ALED1VAL);
  greenRead=RegisterRead(LED2ABSVAL);
  redRead=RegisterRead(LED1ABSVAL);
  */
  ADCRead(LED2VAL);
  ADCRead(LED1VAL);
  ADCRead(ALED2VAL);
  ADCRead(ALED1VAL);
  fGreenRead=ADCRead(LED2ABSVAL);
  fRedRead=ADCRead(LED1ABSVAL);
  wait_ms(20);
  /*
  s.printf("%.04f %.04f %.04f %.04f\r\n", convert(RegisterRead(LED2VAL))
                ,convert(RegisterRead(ALED2VAL))
                ,convert(RegisterRead(LED1VAL))
                ,convert(RegisterRead(ALED1VAL)));
  */
  //wait_ms(20);
}
int nINCMD=0;
void doCmd(void)
{
  char *p;
  char parm[10];
  static char sBuf[80];
  static char *pBuf=sBuf;
  if (s.readable()) {
     char c = s.getc();
     if ( c=='\r' )
     {
        s.printf("\r\nbuff=[%s]\r\n",sBuf);
        pBuf=sBuf;
        if ( strcasecmp(sBuf,"help")==0 )
        {
            s.printf("set stg2=[3.5|6|9.5|12]\r\n");
            s.printf("set cf=[5|10|20|25|30|35|45|50|55|60|70|75|80|85|95\r\n"
                     "        100|155|160|170|175|180|185|195|200|205|210\r\n"
                     "        220|225|230|235|245|250]\r\n");
            s.printf("set rf=[10k|25k|50k|250k|500k|1M]\r\n");
            s.printf("set prf=<freq>\r\n");
            s.printf("set adac=[1-10]uA\r\n");
            s.printf("set prf=<freq>\r\n");
            s.printf("set prp=<period>\r\n");
            s.printf("set led1=<current>mA\r\n");
            s.printf("set led2=<current>mA\r\n");
            s.printf("w led\r\n");
            s.printf("w tiagain\r\n");
            s.printf("wr\r\n");
            s.printf("creg\r\n");
            s.printf("diag\r\n");
            s.printf("get numavg\r\n");
            s.printf("parms\r\n");      
            s.printf("get numavg\r\n");      
        }
        if ( strcasecmp(sBuf,"ble_on")==0 ) {
            s.printf("BLE ON\r\n");
            nINCMD=0;
        }
        if ( strcasecmp(sBuf,"set stage2 on")==0 ) {
               nSTAGE2EN1=1;
        }
        if ( strncasecmp(sBuf,"set stg2=",9)==0 ) {
               p=strchr(sBuf,'=');
               p++;
               sscanf(p,"%s",parm);
               if ( strcmp(parm,"0")==0 ) nSTG2GAIN1=0;
               else if ( strcmp(parm,"3.5")==0 ) nSTG2GAIN1=1;
               else if ( strcmp(parm,"6")==0 ) nSTG2GAIN1=2;
               else if ( strcmp(parm,"9.5")==0 ) nSTG2GAIN1=3;
               else if ( strcmp(parm,"12")==0 ) nSTG2GAIN1=4;
               else {
                   s.printf("stg2 invalid\r\n 0,3.5,6,9.5,12 (db)\r\n");
               }
        }
        if ( strncasecmp(sBuf,"set cf=",7)==0 ) {
           int cf,bcf;
           p=strchr(sBuf,'=');
           p++;
           sscanf(p,"%s",parm);
           cf=atoi(parm);
           if ( cf<5 ) cf=5;
           
           bcf=0;
           cf-=5;
           if ( cf>0 ) bcf|=0b00001;           
           cf-=15;
           if ( cf>0 ) bcf|=0b00010;
           cf-=25;
           if ( cf>0 ) bcf|=0b00100;
           cf-=50;
           if ( cf>0 ) bcf|=0b01000;
           cf-=150;
           if ( cf>0 ) bcf|=0b10000;
           
           cf=5;
           if ( bcf&0b1 ) cf+=5;
           if ( bcf&0b10 ) cf+=15;
           if ( bcf&0b100 ) cf+=25;
           if ( bcf&0b1000 ) cf+=50;
           if ( bcf&0b10000 ) cf+=150;
           
           s.printf("\r\ncf=%d (0x%02X)\r\n",cf,bcf);
           s.printf("  5   10  20  25  30\r\n"
                    "  35  45  50  55  60\r\n"
                    "  70  75  80  85  95\r\n"
                    "  100 155 160 170 175\r\n"
                    "  180 185 195 200 205\r\n"
                    "  210 220 225 230 235\r\n"
                    "  245 250\r\n");
           nCF_LED1=bcf;
        }
        if ( strncasecmp(sBuf,"set rf=",7)==0 ) {
           int cf,bcf;
           p=strchr(sBuf,'=');
           p++;
           sscanf(p,"%s",parm);
           if ( strcasecmp(parm,"500k")==0 )
           {
              nRF_LED1=0;
           } else if ( strcasecmp(parm,"250k")==0 )
           {
              nRF_LED1=1;
           } else if ( strcasecmp(parm,"100k")==0 )
           {
              nRF_LED1=2;    
           } else if ( strcasecmp(parm,"50k")==0 )
           {
              nRF_LED1=3;
           } else if ( strcasecmp(parm,"25k")==0 )
           {
              nRF_LED1=4;
           } else if ( strcasecmp(parm,"10k")==0 )
           {
              nRF_LED1=5;
           } else if ( strcasecmp(parm,"1M")==0 )
           {
              nRF_LED1=6;
           } else {
              s.printf("\r\ninvalid rf value\r\n 10k,25k,50k,250k,500k,1M\r\n");
           }
           s.printf("RF_LED1=%d\n",nRF_LED1);           
        }
        if ( strncasecmp(sBuf,"set prf=",8)==0 ) {
           float prf;
           int nv;
           p=strchr(sBuf,'=');
           p++;
           sscanf(p,"%s",parm);
           prf=atof(parm);
           nv=5000/prf-2;
           s.printf("nv=%d\r\n",nv);                      
           RegisterWrite(CONTROL0,0);
           RegisterWrite(CONTROL1,((1<<8)|(nv&0xFF)));
           RegisterWrite(CONTROL0,1);
        }
        if ( strncasecmp(sBuf,"set adac=",9)==0 ) {
           int prf;
           int nv;
           p=strchr(sBuf,'=');
           p++;
           sscanf(p,"%s",parm);
           prf=atoi(parm);
           nv=RegisterRead(TIA_AMB_GAIN);
           if ( prf<0 ) prf=0;
           if ( prf>10) prf=10;
           prf=prf<<16;
           RegisterWrite(CONTROL0,0);
           RegisterWrite(TIA_AMB_GAIN,nv&0xFFFF|prf);
           RegisterWrite(CONTROL0,1);
           nv=RegisterRead(TIA_AMB_GAIN);
           s.printf("AMBDAC: %d FLTRCNRSEL: %s STAGE2EN2: %s STG2GAIN2: %d\r\n",
                      ((nv>>16)&0xF),
                       (0b1000000000000000&nv)?"1000Hz":"500Hz",                            
                    (nv&0b100000000000000)?"enabled":"bypassed",
                           ((nv>>8)&7));           
        }        
        if ( strncasecmp(sBuf,"set prp=",8)==0 ) {
           float prp;
           int nv;
           p=strchr(sBuf,'=');
           p++;
           sscanf(p,"%s",parm);
           prp=atof(parm);
           nv=prp/200-2;
           s.printf("nv=%d\r\n",nv);                      
        }
        if ( strncmp(sBuf,"set led1=",9)==0 )
        {
           float lc;
           int v;
           p=strchr(sBuf,'=');
           p++;
           sscanf(p,"%s",parm);
           lc=atof(parm);
           v=lc*256.0/dFSC;
           s.printf("nv=0X%X\r\n",v);
           nLED1=v;                                 
        }        
        if ( strncmp(sBuf,"set led2=",9)==0 )
        {
           float lc;
           int v;
           p=strchr(sBuf,'=');
           p++;
           sscanf(p,"%s",parm);
           lc=atof(parm);
           v=lc*256.0/dFSC;
           s.printf("v=0X%X\r\n",v);
           nLED2=v;                                 
        }       
        if ( strcasecmp(sBuf,"w led")==0 )
        {
           long int v;
           v = (nLED_RANGE<<16)|(nLED1<<8)|nLED2;           
           s.printf("WRLED: %X LED_RANGE=%d\n",v,nLED_RANGE);
           RegisterWrite(CONTROL0, 0x000000);
           RegisterWrite(LEDCNTRL, v); 
           RegisterWrite(CONTROL0, 0x000001);
        } 
        if ( strcasecmp(sBuf,"reset")==0 )
        {
           AFE4490Initialize();
           nINCMD=0;
        } 
        if ( strcasecmp(sBuf,"ger")==0 )
        {
            iGER=1-iGER;
        }
        if ( strcasecmp(sBuf,"reg")==0 )
        {
            iREG=1-iREG;
        }        
        if ( strcasecmp(sBuf,"w tiagain")==0 ) {
              setTiaGain();
              printTG();
              RegisterWrite(CONTROL0, 0x000000);
              //RegisterWrite(LEDCNTRL, control);
              RegisterWrite(TIAGAIN, nTIAGAIN);
              //RegisterWrite(TIA_AMB_GAIN, gain2);
              RegisterWrite(CONTROL0, 0x000001);
        }
        if ( strcasecmp(sBuf,"wr")==0 ) {
           long int v;
           v = (nLED_RANGE<<16)|(nLED1<<8)|nLED2;
           RegisterWrite(CONTROL0, 0x000000);
           RegisterWrite(LEDCNTRL, v); 
           RegisterWrite(CONTROL0, 0x000001);            
           s.printf("LEDCNTRL:%03X\r\n",v);
           setTiaGain();
           RegisterWrite(CONTROL0, 0x000000);
           //RegisterWrite(LEDCNTRL, control);
           RegisterWrite(TIAGAIN, nTIAGAIN);
           //RegisterWrite(TIA_AMB_GAIN, gain2);
           RegisterWrite(CONTROL0, 0x000001);
           printTG();
        }
#define PRINTCR(R) do{v=RegisterRead(R);s.printf(#R "%*s: %04X %d\r\n",15-strlen(#R),"",v,v);}while(0)        
        if ( strcasecmp(sBuf,"creg")==0 ) {
            int v;
            RegisterWrite(CONTROL0, 0x000001);
            PRINTCR(LED2STC);
            PRINTCR(LED2ENDC);            
            PRINTCR(LED2LEDSTC);
            PRINTCR(LED2LEDENDC);            
            PRINTCR(ALED2STC);
            PRINTCR(ALED2ENDC);
            PRINTCR(LED1STC);
            PRINTCR(LED1ENDC);            
            PRINTCR(LED1LEDSTC);
            PRINTCR(LED1LEDENDC);
            PRINTCR(ALED1STC);            
            PRINTCR(ALED1ENDC);
            PRINTCR(LED2CONVST);            
            PRINTCR(LED2CONVEND);
            PRINTCR(ALED2CONVST);            
            PRINTCR(ALED2CONVEND);
            PRINTCR(LED1CONVST);            
            PRINTCR(LED1CONVEND);
            PRINTCR(ALED1CONVST);
            PRINTCR(ALED1CONVEND);
            PRINTCR(ADCRSTSTCT0);
            PRINTCR(ADCRSTENDCT0);
            PRINTCR(ADCRSTSTCT1);
            PRINTCR(ADCRSTENDCT1);
            PRINTCR(ADCRSTSTCT2);
            PRINTCR(ADCRSTENDCT2);
            PRINTCR(ADCRSTSTCT3);
            PRINTCR(ADCRSTENDCT3);
            PRINTCR(PRPCOUNT);
            s.printf("\r\n");
        }
        if ( strcasecmp(sBuf,"diag")==0 ) {
           int v;
           v = RegisterRead(CONTROL0);
           v |= 0b100;
           RegisterWrite(CONTROL0,v);
           while ( v&0b100 ) {
               v=RegisterRead(CONTROL0);
           }
           v = RegisterRead(DIAG);
           s.printf("INPSCLED : %sFAULT\r\n",(v&(1<<0))?"":"NO ");
           s.printf("INNSCLED : %sFAULT\r\n",(v&(1<<1))?"":"NO ");
           s.printf("INPSCGND : %sFAULT\r\n",(v&(1<<2))?"":"NO ");
           s.printf("INNSCGND : %sFAULT\r\n",(v&(1<<3))?"":"NO ");
           s.printf("PDSC     : %sFAULT\r\n",(v&(1<<4))?"":"NO ");
           s.printf("PDOC     : %sFAULT\r\n",(v&(1<<5))?"":"NO ");
           s.printf("OUTNSHGND: %sFAULT\r\n",(v&(1<<6))?"":"NO ");
           s.printf("OUTPSHGND: %sFAULT\r\n",(v&(1<<7))?"":"NO ");
           s.printf("LEDSC    : %sFAULT\r\n",(v&(1<<8))?"":"NO ");
           s.printf("LED2OPEN : %sFAULT\r\n",(v&(1<<9))?"":"NO ");
           s.printf("LED1OPEN : %sFAULT\r\n",(v&(1<<10))?"":"NO ");
           s.printf("LED_ALM  : %sFAULT\r\n",(v&(1<<11))?"":"NO ");
           s.printf("PD_ALM   : %sFAULT\r\n",(v&(1<<12))?"":"NO ");
           s.printf("\r\n");
           
        }
        
        if ( strcasecmp(sBuf,"get numavg")==0 ) {
              long int d;
              s.printf("NUMAVG=%X\r\n",RegisterRead(CONTROL1));
        }
        if ( strcasecmp(sBuf,"parms")==0 ) {
            printTG();
        }
        
        s.printf(">");
     } else {
        s.printf("%c",c);
        pBuf[0]=c;
        pBuf[1]=0;
        pBuf++;
        nINCMD=1;
     } 
  }
}

void loop() {
  long int gain2;
  if (s.readable()) {
    char c = s.getc();
    s.printf("%c\n",c);
    if ( c=='+' )
    {
        //nDelay=(nDelay*3)/2;
        iDC++;
        if ( iDC>10 ) iDC=10;
        gain2=16*16*16*16*iDC;
        s.printf("AMBW=%04X\r\n",gain2);
        RegisterWrite(TIA_AMB_GAIN, gain2);
    } else if ( c=='-' )
    {
        iDC--;
        if (iDC<0 ) iDC=0;
        gain2=16*16*16*16*iDC;
        s.printf("AMBW=%04X\r\n",gain2);   
        RegisterWrite(TIA_AMB_GAIN, gain2);     
        //nDelay=(nDelay*2)/3;
        //if ( nDelay<20 ) nDelay=20;
    } else if ( c=='=' ) {
        spo2();
    } else if (c == '\n') {
      inpBuffer[ind] = 0;
      executeChange();
      for (int i = 0; i < 32; i++) {
        inpBuffer[i] = 0;
      }
      ind = 0;
    } else {
      inpBuffer[ind] = c;
      ind += 1;
    }
  }
  if ( t.read_ms()>nDelay /*&& 1==0*/ )
  {
     /*s.printf("read: %d %.04f %.04f %.04f %.04f\r\n", t.read_ms(),convert(RegisterRead(LED2VAL))
                   ,convert(RegisterRead(ALED2VAL))
                   ,convert(RegisterRead(LED1VAL))
                   ,convert(RegisterRead(ALED1VAL)));
     */
     s.printf("read: L2=%10zu AL2=%10zu L1=%10zu AL1=%10zu\r\n", RegisterRead(LED2VAL)
                   ,RegisterRead(ALED2VAL)
                   ,RegisterRead(LED1VAL)
                   ,RegisterRead(ALED1VAL));
     
     t.reset();
  }
  //wait_ms(nDelay);
}

#if 0
#define ASZ 1000
#define ASZ2 20
unsigned long int VIR[ASZ];
unsigned long int VRED[ASZ];
unsigned long int VIRA[ASZ2];
unsigned long int VREDA[ASZ2];
int acur=0;
int acur2=0;

void loop2() {
    int i;
    VIR[acur]=RegisterRead(LED1VAL)
    VRED[acur]=RegisterRead(LED2VAL)
    VIRA[acur2]=RegisterRead(ALED1VAL);
    VREDA[acur2]=RegisterRead(ALED2VAL);
    acur++;
    if ( acur>=ASZ ) acur=0;
    acur2++;
    if ( acur2>=ASZ2 ) acur2=0;
    
    ini = 0;
    end = acur;
    if ( end>=ini )
    {
        ini=end+1;
    }
    i=ini;
    while ( i!=end )
    {
        s+=VIRA[i];
        i++;
        if ( i>=ASZ ) i=0;
    }
    s=s/ASZ;
}
    
#endif

int main1() {
   s.baud(115200);
   spi.format(8);
   spi.frequency(1000000);
   AFE4490Initialize();
   RegisterWrite(CONTROL0, 0x000001); //SPI_Read Enable
   t.start();
   while (1) {
     loop_o();
   }
}












DigitalOut led1(LED1);

const static char     DEVICE_NAME[]        = "HRM1";
static const uint16_t uuid16_list[]        = {GattService::UUID_HEART_RATE_SERVICE,
                                              GattService::UUID_DEVICE_INFORMATION_SERVICE};
static volatile bool  triggerSensorPolling = false;

uint16_t hrmCounter = 100; // init HRM to 100bps
uint16_t hrmCounter2= 100;

HeartRateService         *hrService;
DeviceInformationService *deviceInfo;

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising(); // restart advertising
}

void periodicCallback(void)
{
    led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */

    /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
     * heavy-weight sensor polling from the main thread. */
    triggerSensorPolling = true;
}

void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE &ble          = params->ble;
    ble_error_t error = params->error;

    if (error != BLE_ERROR_NONE) {
        return;
    }

    ble.gap().onDisconnection(disconnectionCallback);

    /* Setup primary service. */
    hrService = new HeartRateService(ble, hrmCounter, hrmCounter2, HeartRateService::LOCATION_FINGER);

    /* Setup auxiliary service. */
    deviceInfo = new DeviceInformationService(ble, "ARM", "Model1", "SN1", "hw-rev1", "fw-rev1", "soft-rev1");

    /* Setup advertising. */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(1000); /* 1000ms */
    ble.gap().startAdvertising();
}

int main(void)
{
    led1 = 1;
    Ticker ticker;
    ticker.attach(periodicCallback, 1); // blink LED every second

    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
    ble.init(bleInitComplete);

    /* SpinWait for initialization to complete. This is necessary because the
     * BLE object is used in the main loop below. */
    while (ble.hasInitialized()  == false) { /* spin loop */ }
    
   s.baud(115200);
   spi.format(8);
   spi.frequency(1000000);
   AFE4490Initialize();
   RegisterWrite(CONTROL0, 0x000001); //SPI_Read Enable
   t.start();
   
  RegisterWrite(CONTROL0, 0x000000);
  RegisterWrite(LEDCNTRL, 0x10404); 
  RegisterWrite(TIAGAIN, 0x00);
  RegisterWrite(TIA_AMB_GAIN, 0x00);
  RegisterWrite(CONTROL0, 0x000001);


    // infinite loop
    while (1) {
        
        doCmd();
        
        // check for trigger from periodicCallback()
        if (/*triggerSensorPolling && */ble.getGapState().connected) {
            triggerSensorPolling = false;

            // Do blocking calls or whatever is necessary for sensor polling.
            // In our case, we simply update the HRM measurement.
            hrmCounter+=5;
            if (hrmCounter == 175) { //  100 <= HRM bps <=175
                hrmCounter = 100;
            }
            
            loop_o();
            hrmCounter=4096+redRead/512;
            uint16_t z=0;
            z=4096+greenRead/512;
            //hrService->updateHeartRate(greenRead,redRead);
            //hrService->updateHeartRate(fGreenRead,fRedRead);
            //fGreenRead=0;
            //fRedRead=76543;
            if ( iGER ) fGreenRead=fRedRead;
            if ( iREG ) fRedRead=fGreenRead;
            hrService->updateHeartRate(fGreenRead,fRedRead);
        } else {
            if ( !nINCMD ) ble.waitForEvent(); // low power wait for event
        }
    }
}