Diff: main.cpp

Revision:

77:3273a1d44741

Parent:

75:8128a06c0a21

--- a/main.cpp	Thu Nov 26 14:35:42 2015 +0000
+++ b/main.cpp	Wed Oct 26 00:25:49 2016 +0000
@@ -20,6 +20,776 @@
 #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";
@@ -27,7 +797,8 @@
                                               GattService::UUID_DEVICE_INFORMATION_SERVICE};
 static volatile bool  triggerSensorPolling = false;
 
-uint8_t hrmCounter = 100; // init HRM to 100bps
+uint16_t hrmCounter = 100; // init HRM to 100bps
+uint16_t hrmCounter2= 100;
 
 HeartRateService         *hrService;
 DeviceInformationService *deviceInfo;
@@ -58,7 +829,7 @@
     ble.gap().onDisconnection(disconnectionCallback);
 
     /* Setup primary service. */
-    hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
+    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");
@@ -85,23 +856,50 @@
     /* 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) {
+        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++;
+            hrmCounter+=5;
             if (hrmCounter == 175) { //  100 <= HRM bps <=175
                 hrmCounter = 100;
             }
-
-            hrService->updateHeartRate(hrmCounter);
+            
+            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 {
-            ble.waitForEvent(); // low power wait for event
+            if ( !nINCMD ) ble.waitForEvent(); // low power wait for event
         }
     }
 }