Diff: main.cpp

Revision:

51:464a019d3191

Parent:

50:f7808934677c

--- a/main.cpp	Wed Nov 12 08:39:52 2014 +0000
+++ b/main.cpp	Fri Jan 23 10:28:48 2015 +0000
@@ -22,36 +22,76 @@
 
 BLEDevice  ble;
 DigitalOut led1(LED1);
+/* ---------------------------------------------------------------
+ * Below for PAH8001 code
+*///--------------------------------------------------------------
+#include "app_util_platform.h"
+#include "nrf_soc.h"
+#include "app_util.h"
+#include "PAH8001Set.h"
+extern "C"
+{
+    #include "pxialg.h"
+}
+Serial  pc(USBTX, USBRX);
+I2C i2c(I2C_SDA0, I2C_SCL0);
+Ticker  ticker;
+/* Power optimized, 1.0mA @disconnection, 4.5mA @connection */
+#define MIN_CONN_INTERVAL   MSEC_TO_UNITS(379, UNIT_1_25_MS)              /**< Minimum connection interval (379 ms) */
+#define MAX_CONN_INTERVAL   MSEC_TO_UNITS(399, UNIT_1_25_MS)              /**< Maximum connection interval (399 ms). */
+#define SLAVE_LATENCY       4                                             /**< Slave latency. */
+#define CONN_SUP_TIMEOUT    MSEC_TO_UNITS(6000, UNIT_10_MS)               /**< Connection supervisory timeout (6 seconds). */
+/* ---------------------------------------------------------------
+ * End of PAH8001 code
+*///--------------------------------------------------------------
 
-const static char     DEVICE_NAME[]        = "Nordic_HRM";
+const static char     DEVICE_NAME[]        = "PixArt_HRmbed";
 static const uint16_t uuid16_list[]        = {GattService::UUID_HEART_RATE_SERVICE,
                                               GattService::UUID_BATTERY_SERVICE,
                                               GattService::UUID_DEVICE_INFORMATION_SERVICE};
+ /* ---------------------------------------------------------------
+ * Below for PAH8001 code
+*///--------------------------------------------------------------
 static volatile bool  triggerSensorPolling = false;
-
 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
 {
     ble.startAdvertising(); // restart advertising
+    triggerSensorPolling = false;
+}
+void onconnectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *p_conn_param)
+{
+    triggerSensorPolling = true;
+    Gap::ConnectionParams_t gap_conn_params;
+    gap_conn_params.minConnectionInterval = MIN_CONN_INTERVAL;
+    gap_conn_params.maxConnectionInterval = MAX_CONN_INTERVAL;
+    gap_conn_params.slaveLatency = SLAVE_LATENCY;
+    gap_conn_params.connectionSupervisionTimeout = CONN_SUP_TIMEOUT;
+    ble.updateConnectionParams(handle, &gap_conn_params);
 }
 
 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;
+{  
+    if(triggerSensorPolling == true){
+        HR_Cnt++;
+        CRITICAL_REGION_ENTER();
+        Pixart_HRD();
+        CRITICAL_REGION_EXIT();
+    }
+    else
+        HR_Cnt = 0;
 }
 
 int main(void)
-{
-    I2C i2c(I2C_SDA0, I2C_SCL0);
+{   uint8_t tmp;
+    pc.baud (115200);
+    pc.printf("\n\rStart initialization\n\r");    
+    
     led1 = 1;
-    Ticker ticker;
-    ticker.attach(periodicCallback, 1);
+    PAH8001_init(); //PAH8001 initialization
 
     ble.init();
     ble.onDisconnection(disconnectionCallback);
+    ble.onConnection(onconnectionCallback);
 
     /* Setup primary service. */
     uint8_t hrmCounter = 100;
@@ -69,21 +109,325 @@
     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
     ble.setAdvertisingInterval(1600); /* 1000ms; in multiples of 0.625ms. */
     ble.startAdvertising();
-
+    
+    pc.printf("Before while 1\n\r");
     while (true) {
-        if (triggerSensorPolling) {
-            triggerSensorPolling = false;
+        if(triggerSensorPolling == true){
+            if(!isFIFOEmpty())
+            {
+                led1 = 1;
+                //pc.printf("PPG[8]: %d\n\r", ppg_mems_data.HRD_Data[8]);
+                if(Pop(&ppg_mems_data)) //Get data from FIFO
+                {
+                    MEMS_Data[0] = ppg_mems_data.MEMS_Data[0];
+                    MEMS_Data[1] = ppg_mems_data.MEMS_Data[1];
+                    MEMS_Data[2] = ppg_mems_data.MEMS_Data[2];
+                    
+                    tmp = PxiAlg_Process(ppg_mems_data.HRD_Data, MEMS_Data);
+                    //if( tmp == FLAG_DATA_READY)
+                    if( tmp != FLAG_DATA_READY)
+                    {
+                        pc.printf("AlgoProcssRtn: %d\n\r", tmp);
+                        pc.printf("PPG[8]: %d\n\r", ppg_mems_data.HRD_Data[8]);
+                    }
+                    else
+                    {
+                        //Check Flag
+                    }
+    
+                    //ready_flag = PxiAlg_GetReadyFlag();
+                    //motion_flag = PxiAlg_GetMotionFlag() ;
+                }
+                led1 = 0;
+            }
+            if(HR_Cnt > HR_Rpt)
+            {
+                HR_Cnt = 0;
+                PxiAlg_HrGet(&myHR);
+                pc.printf("HR: %d\n\r", (uint8_t)myHR);            
+    
+                hrmCounter = (uint8_t)myHR;
+                hrService.updateHeartRate(hrmCounter);
+            }
+        }
+        else
+            ble.waitForEvent();
+    }
+}
+
+
+
+void PAH8001_init()
+{   uint8_t q;
+    uint8_t bank=0, temp;
+
+    if(readRegister(0x00) == 0x30)
+        pc.printf("PAH8001 I2C Link Successful!\n\r");
+    else
+        pc.printf("PAH8001 I2C Link Fail!\n\r");
+        
+    alg_version = PxiAlg_Version();
+    pc.printf("Algo Ver: %d\n\r", alg_version);
+    PxiAlg_EnableFastOutput(true);
+
+#ifdef DEBUG_8001
+    writeRegister(0x09, 0x5A); writeRegister(0x54, 0xEE);
+    pc.printf("\n\r~~~Start Test Pattern~~~ \n\r");
+    pc.printf("Reg0x09: %d\n\r", readRegister(0x09));
+    pc.printf("Reg0x54: %d\n\r", readRegister(0x54));
+    pc.printf("Reg0x1E: %d\n\r", readRegister(0x1E));
+    
+    float MEMS_Data[3] = {0 ,0, 0}; //apply test pattern
+    float myHR = 0 ;
+    for(q=0;q<PPG_PATTERN_SIZE;q++)
+        PxiAlg_Process((unsigned char*)PPG_Data[q], MEMS_Data);
+    PxiAlg_HrGet(&myHR);
+    pc.printf("HR: %f\n\r", myHR);
+    pc.printf("~~~End of Test Pattern~~~ \n\r");
+    while(1);
+#endif
+    
+#ifndef DEBUG_8001
+    ticker.attach_us(&periodicCallback, PAH8001_Poll*1000);
+    
+    pc.printf("\n\r~~~Start Real-time HRM~~~ \n\r");
+    //Initialization settings
+    writeRegister(0x06, 0x82);  //Reset sensor
+    wait_ms(10);    //make a delay
+    
+    for(q=0;q<INIT_PPG_REG_ARRAY_SIZE;q++){
+        if(init_ppg_register_array[q][0] == 0x7F)
+            bank = init_ppg_register_array[q][1];
+
+        if((bank == 0) && (init_ppg_register_array[q][0] == 0x17) )
+        {
+            //read and write bit7=1
+            temp = readRegister(0x17);
+            temp |= 0x80 ;
+            writeRegister(0x17, temp) ;
+        }
+        else
+            writeRegister(init_ppg_register_array[q][0], init_ppg_register_array[q][1]);
+    }
+#endif
+}
+
+void writeRegister(uint8_t addr, uint8_t data)
+{
+    char data_write[2];
+    
+    data_write[0] = addr;
+    data_write[1] = data;
+    i2c.write(I2C_ADDR, data_write, 2, 0);
+}
+uint8_t readRegister(uint8_t addr)
+{
+    char data_write[2];
+    char data_read[2];
+    
+    data_write[0] = addr;
+    i2c.write(I2C_ADDR, data_write, 1, 0);
+    i2c.read(I2C_ADDR, data_read, 1, 0);
+    return data_read[0];
+}
+
+bool Pixart_HRD(void)
+{
+    uint8_t tmp=0;
+    char data_write[2];
+    char data_read[4];
+    ppg_mems_data_t ppg_mems_data;
+    //Check Touch Status for power saving
+    writeRegister(0x7F,0x00); //bank0
+    tmp = readRegister(0x00);
+    tmp = readRegister(0x59)&0x80;
+    led_ctrl(tmp);
+
+    //writeRegister(0x7F,0x01); //bank1
+    ppg_mems_data.HRD_Data[0]=readRegister(0x68)&0x0f; //check status: 0 is not ready, 1 is ready, 2 is loss one data?
+
+    if(ppg_mems_data.HRD_Data[0] ==0)
+    {
+        writeRegister(0x7F,0x00); //bank0
+        return false;
+    }
+    else
+    {
+        //Only support burst read (0x64~0x67), when using I2C interface
+        data_write[0] = 0x64;
+        i2c.write(PAH8001_ADDR, data_write, 1, 1);
+        i2c.read(PAH8001_ADDR, data_read, 4, 0);
+        ppg_mems_data.HRD_Data[1]=data_read[0]&0xff;
+        ppg_mems_data.HRD_Data[2]=data_read[1]&0xff;
+        ppg_mems_data.HRD_Data[3]=data_read[2]&0xff;
+        ppg_mems_data.HRD_Data[4]=data_read[3]&0xff;
 
-            /* Do blocking calls or whatever is necessary for sensor polling. */
-            /* In our case, we simply update the dummy HRM measurement. */
-            hrmCounter++;
-            if (hrmCounter == 175) {
-                hrmCounter = 100;
+        //Only support burst read (0x1A~0x1C), when using I2C interface
+        data_write[0] = 0x1A;
+        i2c.write(PAH8001_ADDR, data_write, 1, 1);
+        i2c.read(PAH8001_ADDR, data_read, 3, 0);
+        ppg_mems_data.HRD_Data[5]=data_read[0]&0xff;
+        ppg_mems_data.HRD_Data[6]=data_read[1]&0xff;
+        ppg_mems_data.HRD_Data[7]=data_read[2]&0xff;
+
+        ppg_mems_data.HRD_Data[8]=Frame_Count++;
+        ppg_mems_data.HRD_Data[9]=40;
+        ppg_mems_data.HRD_Data[10]=_led_current_change_flag;
+        writeRegister(0x7F,0x00); //bank0
+        //bit7 is Touch Flag (bit7=1 is meant Touch, and bit7=0 is meant No Touch)
+        ppg_mems_data.HRD_Data[11]=(readRegister(0x59)&0x80); //Check Touch Flag
+        ppg_mems_data.HRD_Data[12]= ppg_mems_data.HRD_Data[6];
+        
+        //If no G sensor, please set G_Sensor_Data[3] = {0};
+        ppg_mems_data.MEMS_Data[0] = 0;//ReadGSensorX();
+        ppg_mems_data.MEMS_Data[1] = 0;//ReadGSensorY();
+        ppg_mems_data.MEMS_Data[2] = 0;//ReadGSensorZ();
+        Push(&ppg_mems_data); //Save data into FIFO
+        
+        return true;
+    }
+}
+
+bool isFIFOEmpty(void)
+{
+    return (_write_index == _read_index);
+}
+
+bool Push(ppg_mems_data_t *data)
+{
+    int tmp = _write_index;
+    tmp++;
+    if(tmp >= FIFO_SIZE)
+        tmp = 0;
+    if(tmp == _read_index)
+        return false;
+    _ppg_mems_data[tmp] = *data;
+    _write_index = tmp;
+    
+    return true;
+}
+
+bool Pop(ppg_mems_data_t *data)
+{
+    int tmp;
+    if(isFIFOEmpty())
+        return false;
+    *data = _ppg_mems_data[_read_index];
+    tmp = _read_index + 1;
+    if(tmp >= FIFO_SIZE)
+        tmp = 0;
+    _read_index = tmp;
+    
+    return true;
+}
+
+/***********************LED Control Start***********************************/
+void led_ctrl(uint8_t touch)    {
+    if(touch == 0x80)   {
+        uint8_t data;
+        //uint16_t Frame_Average, EP_L, EP_H, Exposure_Line;
+        uint16_t EP_L, EP_H, Exposure_Line;
+        writeRegister(0x7f,0x00);
+        writeRegister(0x05,0x98);
+        writeRegister(0x7f,0x01);
+        //writeRegister(0x42,0xA4);
+        writeRegister(0x7f,0x00);
+        data = readRegister(0x33);
+        EP_H=data&0x03;
+        data = readRegister(0x32);
+        EP_L=data;
+        Exposure_Line=(EP_H<<8)+EP_L;
+        writeRegister(0x7f,0x01);
+        if(_sleepflag==1)   {
+            writeRegister(0x38, (0xE0|DEFAULT_LED_STEP));
+            _sleepflag = 0 ;
+        }
+
+        if (_state_count <= STATE_COUNT_TH) {
+            _state_count++;
+            _led_current_change_flag = 0;
+        }
+        else {
+            _state_count = 0;
+            if(_state == 0) {
+                if( (Exposure_Line>=LED_CTRL_EXPO_TIME_HI_BOUND) || (Exposure_Line<=LED_CTRL_EXPO_TIME_LOW_BOUND) ) {
+                    //writeRegister(0x7f,0x01);
+                    data = readRegister(0x38);
+                    _led_step=data&0x1f;
+                    if( (Exposure_Line>=LED_CTRL_EXPO_TIME_HI_BOUND) && (_led_step < LED_CURRENT_HI) )  {
+                        _state = 1 ;
+                        _led_step=_led_step+LED_INC_DEC_STEP;
+
+                        if(_led_step>LED_CURRENT_HI)
+                            _led_step=LED_CURRENT_HI;
+                        writeRegister(0x38, (_led_step|0xE0));
+                        _led_current_change_flag = 1;
+                    }
+                    else if((Exposure_Line<=LED_CTRL_EXPO_TIME_LOW_BOUND) && (_led_step > LED_CURRENT_LOW) )    {
+                        _state = 2 ;
+                        if(_led_step<=(LED_CURRENT_LOW+LED_INC_DEC_STEP))
+                            _led_step=LED_CURRENT_LOW;
+                        else
+                            _led_step=_led_step-LED_INC_DEC_STEP;
+                        writeRegister(0x38, (_led_step|0xE0));
+                        _led_current_change_flag = 1;
+                    }
+                    else    {
+                        _state = 0 ;
+                        _led_current_change_flag = 0;
+                    }
+                }
+                else {
+                    _led_current_change_flag = 0;
+                }
             }
-
-            hrService.updateHeartRate(hrmCounter);
-        } else {
-            ble.waitForEvent();
+            else if(_state == 1)    {
+                if(Exposure_Line > LED_CTRL_EXPO_TIME_HI)   {
+                    _state = 1 ;
+                    _led_step=_led_step+LED_INC_DEC_STEP;
+                    if(_led_step>=LED_CURRENT_HI)   {
+                        _state = 0 ;
+                        _led_step=LED_CURRENT_HI;
+                    }
+                    writeRegister(0x38, (_led_step|0xE0));
+                    _led_current_change_flag = 1;
+                }
+                else    {
+                    _state = 0 ;
+                    _led_current_change_flag = 0;
+                }
+            }
+            else    {
+                if(Exposure_Line < LED_CTRL_EXPO_TIME_LOW)  {
+                    _state = 2 ;
+                    if(_led_step<=(LED_CURRENT_LOW+LED_INC_DEC_STEP))   {
+                        _state = 0 ;
+                        _led_step=LED_CURRENT_LOW;
+                    }
+                    else
+                        _led_step=_led_step-LED_INC_DEC_STEP;
+                    writeRegister(0x38, (_led_step|0xE0));
+                    _led_current_change_flag = 1;
+                }
+                else    {
+                    _state = 0;
+                    _led_current_change_flag = 0;
+                }
+            }
         }
     }
+    else    {
+        writeRegister(0x7f,0x00);
+        writeRegister(0x05,0xB8);
+        writeRegister(0x7F,0x01);
+        //writeRegister(0x42,0xA0);
+        _led_step = DEFAULT_LED_STEP;
+        writeRegister(0x38, 0xFF);
+        _sleepflag = 1;
+        _led_current_change_flag = 0;
+    }
 }
+/***********************LED Control End ***********************************/
+/* ---------------------------------------------------------------
+ * End of PAH8001 code
+*///--------------------------------------------------------------