Revision 14:c3b080cdf36b, committed 2018-04-11

Comitter:

asong

Date:

Wed Apr 11 01:46:49 2018 +0000

Parent:

13:37cd579208e9

Child:

15:330794a9f347

Commit message:

Added heart rate measurement, simulation, correct leds, and other heart rate improvements. WDT needs messing with to work better with the heart rate measurements.;

Changed in this revision

--- a/main.cpp	Mon Apr 09 13:31:46 2018 +0000
+++ b/main.cpp	Wed Apr 11 01:46:49 2018 +0000
@@ -20,6 +20,9 @@
 Display data screen update routines
 
 **************   Versions  ****************
+v2.13 - Added heart rate measurement, heart rate simulation, 
+improved heart rate leds, and optimized heart rate functionalities
+
 v2.12 - Added hidden reset feature (push left button 9+ times, on screen zero, to reset), added line that 
 might put K64 into low power run mode, added dimming feature to display, changed humid/temp and battery 
 measurement to a rolling average. Made new screen for sending panic alert.
@@ -66,13 +69,20 @@
 #include "images.h"
 
 
-//***************  Definitions
-#define SW_Ver          2.12    // For displaying software version
+/* General  Definitions */
+#define SW_Ver          2.13    // For displaying software version
 #define LED_ON          0
 #define LED_OFF         1
 #define SCRN_TIME       10.0    // Set OLED screen turn off time to 10.0 seconds
-#define WDT_TIME        2.0     // Set Watch Dog timer to 1.5 seconds (1.5s reset in certain subroutines)
+#define WDT_TIME        5.0     // Set Watch Dog timer to 1.5 seconds (1.5s reset in certain subroutines)
 #define Debug           1       // If "Debug" is defined, our code will compile for debug.  Comment out for Production code.
+
+#define FXOS8700_I2C_ADDRESS_ (0x1E<<1) //pins SA0,SA1=0 
+#define FXAS21002_I2C_ADDRESS_ 0x40      //
+//#define TSL2561_I2C_ADDRESS_  (0x29 << 1) // Address select line is grounded
+//#define MPL3115A2_I2C_ADDRESS_  ? // 
+
+/*Heart Rate Definitions */
 #define HIGHEST_ZONE    4       // Highest heart rate zone
 #define LOWEST_ZONE     1
 #define ENTER_BELOW     25
@@ -80,10 +90,44 @@
 #define EXIT_BELOW      75
 #define EXIT_ABOVE      100
 #define VIB_OPT_2       75      // Haptic vibration pattern option for heart rate functions
-#define FXOS8700_I2C_ADDRESS_ (0x1E<<1) //pins SA0,SA1=0 
-#define FXAS21002_I2C_ADDRESS_ 0x40      //
-//#define TSL2561_I2C_ADDRESS_  (0x29 << 1) // Address select line is grounded
-//#define MPL3115A2_I2C_ADDRESS_  ? // 
+/* I2C Address */
+#define HR_W_ADDR          0xAE
+#define HR_R_ADDR          0xAF 
+
+/* Status */
+#define REG_INT_MSB        0x00 /* Interrupt Status 1 */
+#define REG_INT_LSB        0x01 /* Interrupt Status 2 */
+#define REG_INT_ENB_MSB    0x02 /* Interrupt Enable 1 */
+#define REG_INT_ENB_LSB    0x03 /* Interrupt Enable 2 */
+/* FIFO */
+#define REG_FIFO_WR_PTR    0x04 /* FIFO Write Pointer */
+#define REG_OVF_COUNTER    0x05 /* Overflow Counter */
+#define REG_FIFO_RD_PTR    0x06 /* FIFO Read Pointer */
+#define REG_FIFO_DATA      0x07 /* FIFO Data Register */
+/* Configuration */
+#define REG_FIFO_CONFIG    0x08 /* FIFO Configuration */
+#define REG_MODE_CONFIG    0x09 /* Mode Configuration */
+#define REG_SPO2_CONFIG    0x0A /* SpO2 Configuration */
+/* reserved                0x0B */
+#define REG_LED1_PA        0x0C /* RED LED Pulse Amplitude 1 */
+#define REG_LED2_PA        0x0D /* IR LED Pulse Amplitude 2 */
+#define REG_LED3_PA        0x0E /* GREEN LED Pulse Amplitude 3 */
+/* reserved                0x0F */
+#define REG_PILOT_PA       0x10 /* Proximity LED Pulse Amplitude */
+#define REG_SLOT_MSB       0x11 /* Multi-LED Mode Control Registers 2, 1 */
+#define REG_SLOT_LSB       0x12 /* Multi-LED Mode Control Registers 4, 3 */
+/* DIE Temperature */
+#define REG_TEMP_INT       0x1F /* Die Temperature Integer */
+#define REG_TEMP_FRAC      0x20 /* Die Temperature Fraction */
+#define REG_TEMP_EN        0x21 /* Die Temperature Config */
+/* Proximity Function */
+#define REG_PROX_INT_THR   0x30 /* Proximity Interrupt Threshold */
+/* Part ID */
+#define REG_REV_ID         0xFE /* Revision ID */
+#define REG_PART_ID        0xFF /* Part ID: 0x15 */
+/* Depth of FIFO */
+#define FIFO_DEPTH         32
+/* End of heart rate definitions */
 
 
 void StartHaptic(void);
@@ -135,6 +179,33 @@
 void WDT_Timeout(void);         // WDT Routine used reset
 void CLRWDT(void);              // function to clear WDT
 void UpDate_Ave(void);          // function used to update slow changing measurments
+void HR_Simulation(void);       // Step through a constrained random heart rate simulation to demo zones
+void readRegs(int, uint8_t *, int); //I2c read for maxim chip
+void writeRegs(uint8_t, int);       //I2c write for maxim chip
+/* Functions for setting maxim chip values */
+void setIntEnable(uint16_t);
+void setFIFO_WR_PTR(uint8_t);
+void setOVF_COUNTER(uint8_t);
+void setFIFO_RD_PTR(uint8_t);
+void setFIFO_DATA(uint8_t);
+void setFIFO_CONFIG(uint8_t);
+void setMODE_CONFIG(uint8_t);
+void setSPO2_CONFIG(uint8_t);
+void setLED1_PA(uint8_t);
+void setLED2_PA(uint8_t);
+void setLED3_PA(uint8_t);
+void setPILOT_PA(uint8_t);
+void setSLOT(uint16_t);  
+void setPROX_INT_THR(uint8_t);  
+void clearFIFO(void);
+uint32_t readFIFO(void); //Read from the maxim chips fifo
+void maxReset(void);     //Reset maxim chip
+void maxInit(void);      //Initialize the maxim chip with necessary values
+uint16_t getIntEnable(void);
+void maxEnable(void);   //Begin the maxim chip light measurements
+void maxDeinit(void);   //Turn off the maxim chip
+uint32_t isPeak(uint32_t *); //Calculate the peak of a sample from the maxim chip
+void processHeartRate(void); //Calculate the heart rate measurement
 
 // *****************  Global variables  ***********************
 char text_1[20];            // Text buffer - Do we need more?
@@ -166,7 +237,6 @@
 uint8_t Current_Zone = 1;
 uint8_t Prev_Zone = 1;
 uint8_t Heart_Rate = 100;
-uint8_t HR_buff[250];
 uint8_t *HR_return;
 uint8_t Age = 50;            // Initialize age
 uint8_t Max_Bpm = 220 - Age;      // Initialize Max BPM
@@ -186,7 +256,9 @@
 int sample_humid = 0;               // used in Heat index calc
 uint8_t batt_per_level = 0;         //
 uint8_t Ave_Num  = 0; 
-
+bool randomized = 0; //Initialize to 0, since srand has not been called
+int simulation_stage = 0;
+uint32_t hr_data[100];
 // Pointers for screen images 
 const uint8_t *Hexi_Heart_ = Hexi_Heart_bmp;
 //const uint8_t *NB_Linkedin = NB_Linkedin_bmp;  
@@ -225,7 +297,7 @@
 
 DigitalOut OLED_PWR(PTC13);     // this pin turns on/off 15V supply to OLED display
 DigitalOut PowerEN (PTB12);     // 3V3B Power Enable for HTU21D (Temp/Hum sensor) and Light sensor Sensor supply
-DigitalOut HR_PWR(PTA29);       // this pin turns on/off power to heart rate sensor
+DigitalOut maxim(PTA29);       // this pin turns on/off power to heart rate sensor
 //DigitalIn   Sw1(PTA12);       //Switch 'T1' on docking station AND Led_clk1!!
 //DigitalIn   Sw2(PTA13);       //Switch 'T2' on docking station AND Led_clk2!!
 DigitalIn   Sw3(PTA15);         //Switch 'T3' on docking station 
@@ -257,6 +329,10 @@
 Ticker chk_motion;  // Ticker used to check on post fall motion
 Ticker Haptic_Timer;
 Ticker WDT_Timer;
+Ticker hr_led;
+Ticker hr_simulation;
+Ticker hr_ticker;
+Ticker hr_measure_ticker;
 
 /*****************************************************************************
 Name: timout_timer()
@@ -365,13 +441,11 @@
                 break;
             }
             case 7: {// Heart Rate Zone
-                StartHaptic(50);
-                Enable_Heart_Rate();
-                //heart.enable();
-                //HR_Enable = 1;
-                //while(HR_Enable == 1)
-                //    heart.readRawData(HR_buff, HR_return);
-                //update_display();
+                StartHaptic();
+                maxInit();
+                maxEnable();
+                hr_led.attach(&Led_Zone_Indicator, 1);
+                hr_ticker.attach(&processHeartRate, 5);
                 break;
             }
             case 8: {// Alert History
@@ -456,8 +530,26 @@
                 break;
             }
             case 31: { //Manually Increment Heart Rate by 1
-                Increment_Heart_Rate();
-                Determine_Current_Zone();
+                //StartHaptic();
+                if(maxim == 0)
+                {
+                    Increment_Heart_Rate();
+                   // Determine_Current_Zone();
+                    update_display();
+                }
+                break;
+            }
+            case 32: {//Turn on HR led blinking for manual demonstration
+                hr_led.attach(&Led_Zone_Indicator, 1);
+                 break;
+            }
+            case 33:{//Start heart rate simulation
+                StartHaptic();
+                if(maxim == 0)
+                {
+                    hr_led.attach(&Led_Zone_Indicator, 1);
+                    hr_simulation.attach(&HR_Simulation, 5.0);                  
+                }
                 update_display();
                 break;
             }
@@ -609,8 +701,8 @@
                 break;
             }
             case 7: {// Heart Rate Zone
-                StartHaptic(100);
-                Disable_Heart_Rate();
+                StartHaptic();
+                maxDeinit();              
                 break;
             }
             case 8: {// Alert History
@@ -704,8 +796,27 @@
                 break;
             }
             case 31: { //Manually decrement heart rate by 1
-                Decrement_Heart_Rate();
-                Determine_Current_Zone();
+                if(maxim == 0)
+                {
+                    Decrement_Heart_Rate();
+                   // Determine_Current_Zone();
+                   update_display();
+                }
+                
+                break;
+            }
+            case 32: {//End HR led used for manual demonstration
+                hr_led.detach();
+                break;
+            }
+            case 33: {//End HR Simulation early
+                StartHaptic();
+                if(maxim == 0)
+                {
+                    hr_simulation.detach();
+                    simulation_stage = 0;
+                    hr_led.detach();
+                }
                 update_display();
                 break;
             }
@@ -912,18 +1023,17 @@
                 update_display();
                 break;
             }
-            case 31: {
+            case 32: {
                 StartHaptic();
-                Screen_Num = 32;
+                Screen_Num = 33;
                 update_display();
                 break;
             }
-            case 32: {
+            case 33: {
                 StartHaptic();
                 Screen_Num = 7;
                 update_display();
-                break;
-            }
+            } 
             case 41: {// Fall-Mode adj
                 StartHaptic();
                 Screen_Num = 42;  //Change to screen 42
@@ -1173,6 +1283,12 @@
                 update_display();
                 break;
             }
+            case 33: {
+                StartHaptic();
+                Screen_Num = 32;
+                update_display();
+                break;
+            }
             case 41: {// Fall mode screen
                 StartHaptic();
                 Screen_Num = 22;
@@ -1298,11 +1414,11 @@
 // *****************  Reset sensors   ***********************
     OLED_PWR = 0;  // Turn off OLED power supply
     PowerEN = 1;    // Turn off (=1)HTU21(Temp/Hum) and TSL2561(Light) sensors to reset them
-    HR_PWR = 0;     // Turn off (=0) Heart rate sensor 1.8V and HRM(3.3V) supply to reset
+    maxim = 0;     // Turn off (=0) Heart rate sensor 1.8V and HRM(3.3V) supply to reset
     wait(0.2);      // how long should we wait for sensors to power down?
     
     PowerEN = 0;    // Turn on (=0) HTU21(Temp/Hum) and TSL2561(Light) sensors to reset them
-    HR_PWR = 1;     // Turn on (=1)Heart rate sensor 
+//    maxim = 1;     // Turn on (=1)Heart rate sensor 
     OLED_PWR = 1;   // Turn on OLED power supply
     wait(0.2);      // how long should we wait for sensors to power up?
         
@@ -1597,18 +1713,20 @@
             oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
             break;
         }
-        case 7: {// Heart Rate Zone
+         case 7: {// Heart Rate Zone
 
             oled.FillScreen(COLOR_BLACK); // Clear screen
+            textProperties.fontColor = COLOR_WHITE;
+            oled.SetTextProperties(&textProperties);
             oled.Label((uint8_t *)"Heart Rate",18,5); // Display at x,y
-            oled.Label((uint8_t *)"HR:",15,25); // Display at x,y
+            oled.Label((uint8_t *)"HR:",10,25); // Display at x,y
             sprintf(display_buff, "%u", Heart_Rate);
             textProperties.fontColor = COLOR_RED; //Change font to red
             oled.SetTextProperties(&textProperties);//Implement color change
             oled.Label((uint8_t *)display_buff,43,25); // Display at x,y
             textProperties.fontColor = COLOR_WHITE;
             oled.SetTextProperties(&textProperties);
-            oled.Label((uint8_t *)"Age: ",15,45); // Display at x,y
+            oled.Label((uint8_t *)"Age: ",10,45); // Display at x,y
             textProperties.fontColor = COLOR_GREEN;
             oled.SetTextProperties(&textProperties); //implements the color change
             sprintf(display_buff, "%u", Age); //Convert int to char array for displaying user age
@@ -1617,13 +1735,8 @@
             oled.SetTextProperties(&textProperties);
             oled.Label((uint8_t *)"On",80,15); // "+" at x,y
             oled.Label((uint8_t *)"Off",78,60); // "-" at x,y
-            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
+            oled.Label((uint8_t *)"Menu",10,80); // Display "Back" at x,y
             oled.Label((uint8_t *)"Next",60,80); // Display "Next" at x,y
-
-            //heart.enable();
-            //sprintf(display_buff, "%u", heart.getRevisionID()); //Convert int to char array for displaying user age
-            //oled.Label((uint8_t *)display_buff,45,25); // Display at x,y
-            //update_display();
             break;
         }
         case 8: {// Alert History
@@ -2050,12 +2163,9 @@
         }
         case 31: {
             oled.FillScreen(COLOR_BLACK);
-            Heart_Rate_Vibrations();
             oled.Label((uint8_t *)"Enter HR", 10, 5);// Display at x,y
             oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
             oled.Label((uint8_t *)"Next",60,80);  //Display "Next" at x,y
-            oled.Label((uint8_t *)"+",85,15); // "+" at x,y
-            oled.Label((uint8_t *)"-",85,60); // "-" at x,y
             oled.Label((uint8_t *)"HR:", 10, 25);
             sprintf(display_buff, "%u", Heart_Rate); // Convert int to char to display
             textProperties.fontColor = COLOR_RED; //Change font to red
@@ -2099,8 +2209,6 @@
             oled.Label((uint8_t *)display_buff, 71, 60);
             textProperties.fontColor = COLOR_WHITE; //Change font to white
             oled.SetTextProperties(&textProperties);//Implement color change
-            CLRWDT();
-            Led_Zone_Indicator();
             break;
         }
         case 32: {
@@ -2145,37 +2253,33 @@
             oled.Label((uint8_t *)"Next",60,80);  //Display "Next" at x,y
             break;
         }  
-        case 41: { //Fall mode
+        case 33: {//Heart rate simulator
             oled.FillScreen(COLOR_BLACK);
-            oled.Label((uint8_t *)"Adj Fall Mode", 10, 5);
-            sprintf(display_buff, "%u", Fall_Alert_Mode); //Convert int to char array for displaying mode
-            oled.Label((uint8_t *)"Mode:", 5, 30);
-            oled.Label((uint8_t *)"+",85,15); // "*" at x,y
-            oled.Label((uint8_t *)"-",85,60); // "*" at x,y
+            textProperties.fontColor = COLOR_WHITE;
+            oled.Label((uint8_t *)"HR Simulation", 10, 5);// Display at x,y
+            oled.Label((uint8_t *)"HR: ", 10, 40);// Display at x,y
             oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
             oled.Label((uint8_t *)"Next",60,80);  //Display "Next" at x,y
-            textProperties.fontColor = COLOR_GREEN;
-            oled.SetTextProperties(&textProperties);
-            oled.Label((uint8_t *)display_buff,43,30); // Display at x,y
+            oled.Label((uint8_t *)"On",75,25); // "*" at x,y
+            oled.Label((uint8_t *)"Off",75,55); // "*" at x,y
+            sprintf(display_buff, "%u", Heart_Rate); // Convert int to char to display
+            if(Current_Zone == 1) {
+                textProperties.fontColor = COLOR_YELLOW;
+                oled.SetTextProperties(&textProperties);
+            } else if(Current_Zone == 2) {
+                textProperties.fontColor = COLOR_BLUE;
+                oled.SetTextProperties(&textProperties);
+            } else if(Current_Zone == 3) {
+                textProperties.fontColor = COLOR_GREEN;
+                oled.SetTextProperties(&textProperties);
+            } else if(Current_Zone == 4) {
+                textProperties.fontColor = COLOR_RED;
+                oled.SetTextProperties(&textProperties);
+            }
+            oled.Label((uint8_t *)display_buff, 35, 40);// Display at x,y
             textProperties.fontColor = COLOR_WHITE;
             oled.SetTextProperties(&textProperties);
-            break;
-        }
-        case 42: { //Fall Thresh
-            oled.FillScreen(COLOR_BLACK);
-            oled.Label((uint8_t *)"Adj F-Th", 10, 5);
-            sprintf(display_buff, "%2.2f g", Fall_Thresh); //Convert int to char array for displaying mode
-            oled.Label((uint8_t *)"F_Th:", 5, 30);
-            oled.Label((uint8_t *)"+",85,15); // "*" at x,y
-            oled.Label((uint8_t *)"-",85,60); // "*" at x,y
-            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
-            oled.Label((uint8_t *)"Next",60,80);  //Display "Next" at x,y
-            textProperties.fontColor = COLOR_GREEN;
-            oled.SetTextProperties(&textProperties);
-            oled.Label((uint8_t *)display_buff,43,30); // Display at x,y
-            textProperties.fontColor = COLOR_WHITE;
-            oled.SetTextProperties(&textProperties);
-            break;
+            break;           
         }
         case 43: { //Impact Thresh
             oled.FillScreen(COLOR_BLACK);
@@ -3979,17 +4083,23 @@
                
                
                 case 10: {// reset
-                    char d[2];    
-                    d[0] = 0x09;                      // Mod_conf reg - SHDN, reset, modes
-                    d[1] = 0b01000000;                                //resets IC
-                    if(i2c_bus0.write(0xaf, d, 2) ==1){
-                        oled.Label((uint8_t *)"MAX_W_Err10a",5,1); // Display "error" at x,y 
-                        wait(2.0);  // wait 0 seconds
-                    }//end if   
-                    else {
-                   // oled.Label((uint8_t *)"MAX_Reset",20,30); // Display "reset" at x,y
+                    if(maxim == 0)
+                    {
                     }
-                    wait(0.01);  // wait 0.01 seconds
+                    else
+                    {
+                        char d[2];    
+                        d[0] = 0x09;                      // Mod_conf reg - SHDN, reset, modes
+                        d[1] = 0b01000000;                                //resets IC
+                        if(i2c_bus0.write(0xaf, d, 2) ==1){
+                            oled.Label((uint8_t *)"MAX_W_Err10a",5,1); // Display "error" at x,y 
+                            wait(2.0);  // wait 0 seconds
+                        }//end if   
+                        else {
+                       // oled.Label((uint8_t *)"MAX_Reset",20,30); // Display "reset" at x,y
+                        }
+                        wait(0.01);  // wait 0.01 seconds
+                    }
                     break;    
                 }// end of case 10  
                 
@@ -4286,4 +4396,316 @@
     if(Ave_Num>4){
         Ave_Num = 4;//set limit 
         }    
-    }// end UpDate_Ave
\ No newline at end of file
+}// end UpDate_Ave
+void HR_Simulation(void)
+{
+    if(randomized == 0)
+    {
+        srand(hi_calc);
+        randomized = 1;
+    }
+    if(simulation_stage == 0)
+    {
+        Heart_Rate = HR_Zone1[0] + rand() %( HR_Zone1[1] - HR_Zone1[0] + 1);
+       
+    }
+    else if(simulation_stage == 1)
+    {
+        Heart_Rate = HR_Zone2[0] + rand() %( HR_Zone2[1] - HR_Zone2[0] + 1);
+        
+    }
+    else if(simulation_stage == 2)
+    {
+        Heart_Rate = HR_Zone3[0] + rand() %( HR_Zone3[1] - HR_Zone3[0] + 1);
+        
+    }
+    else if(simulation_stage == 3)
+    {
+        Heart_Rate = HR_Zone4[0] + rand() %( HR_Zone4[1] - HR_Zone4[0] + 1);
+        
+    }
+    else if(simulation_stage == 4)
+    {
+        Heart_Rate = HR_Zone3[0] + rand() %( HR_Zone3[1] - HR_Zone3[0] + 1);
+        
+    }
+    else 
+    {
+        simulation_stage = -1;
+    }
+    simulation_stage++;
+    Determine_Current_Zone();  
+    update_display();
+}
+
+void readRegs(int addr, uint8_t * data, int len)
+{
+    char temp[1] = {addr};
+    i2c_bus0.write(HR_W_ADDR, temp, 1, true);
+    i2c_bus0.read(HR_R_ADDR, (char*)data, len);
+        
+}
+
+void writeRegs(uint8_t * data, int len)
+{
+    i2c_bus0.write(HR_W_ADDR, (char*)data, len);    
+}
+
+void setIntEnable(uint16_t mask)
+{
+    uint8_t res[3];
+    res[0] = REG_INT_ENB_MSB;
+    res[1] = (mask >> 8) & 0xFF;
+    res[2] = (mask & 0xFF);
+    writeRegs(res, 3);       
+}
+
+
+void setFIFO_WR_PTR(uint8_t data)
+{
+    uint8_t res[2];
+    res[0] = REG_FIFO_WR_PTR;
+    res[1] = data;
+    writeRegs(res, 2);    
+}
+
+void setOVF_COUNTER(uint8_t data)
+{
+    uint8_t res[2];
+    res[0] = REG_OVF_COUNTER;
+    res[1] = data;
+    writeRegs(res, 2);
+}
+
+uint8_t getFIFO_RD_PTR(void)
+{
+    uint8_t data;
+    readRegs(REG_FIFO_RD_PTR, &data, 1);
+    return (data);
+}
+
+void setSLOT(uint16_t data) 
+{
+    uint8_t res[3];
+    res[0] = REG_SLOT_MSB; 
+    res[1] = (data >> 8) & 0xFF;
+    res[2] = data & 0xFF;
+    writeRegs(res, 3);
+}
+
+void setFIFO_RD_PTR(uint8_t data)
+{
+    uint8_t res[2];
+    res[0] = REG_FIFO_RD_PTR;
+    res[1] = data;
+    writeRegs(res, 2);    
+}
+
+void setFIFO_DATA(uint8_t data)
+{
+    uint8_t res[2];
+    res[0] = REG_FIFO_DATA;
+    res[1] = data;
+    writeRegs(res, 2);    
+}
+
+void setFIFO_CONFIG(uint8_t data)
+{
+    uint8_t res[2];
+    res[0] = REG_FIFO_CONFIG;
+    res[1] = data;
+    writeRegs(res, 2);        
+}
+
+
+void setMODE_CONFIG(uint8_t data)
+{
+    uint8_t res[2];
+    res[0] = REG_MODE_CONFIG;
+    res[1] = data;
+    writeRegs (res, 2);
+}
+ 
+void  setSPO2_CONFIG(uint8_t data)
+{
+    uint8_t res[2] ;
+    res[0] = REG_SPO2_CONFIG ;
+    res[1] = data ;
+    writeRegs(res, 2) ;
+}
+ 
+void setLED1_PA(uint8_t data)
+{
+    uint8_t res[2] ;
+    res[0] = REG_LED1_PA ;
+    res[1] = data ;
+    writeRegs(res, 2) ;
+}
+ 
+void setLED2_PA(uint8_t data)
+{
+    uint8_t res[2] ;
+    res[0] = REG_LED2_PA ;
+    res[1] = data ;
+    writeRegs(res, 2) ;
+}
+ 
+void setLED3_PA(uint8_t data)
+{
+    uint8_t res[2] ;
+    res[0] = REG_LED3_PA ;
+    res[1] = data ;
+    writeRegs(res, 2) ;
+}
+ 
+void setPILOT_PA(uint8_t data)
+{
+    uint8_t res[2] ;
+    res[0] = REG_PILOT_PA ;
+    res[1] = data ;
+    writeRegs(res, 2) ;
+}
+ 
+
+
+ 
+void setPROX_INT_THR(uint8_t data)  
+{
+    uint8_t res[2] ;
+    res[0] = REG_PROX_INT_THR ;
+    res[1] = data ;
+    writeRegs(res, 2) ;
+}
+ 
+void clearFIFO(void)
+{
+    uint8_t res[5] ;
+    res[0] = REG_FIFO_WR_PTR ;
+    res[1] = 0x00 ; /* FIFO_WR_PTR */
+    res[2] = 0x00 ; /* OVF_COUNTER */
+    res[3] = 0x00 ; /* FIFO_RD_PTR */
+    res[4] = 0x00 ; /* FIFO_DATA (do we need to clear this?) */
+    writeRegs(res, 5) ;
+}
+
+/*
+ * readFIFO(void)
+ * FIFO data is always a 3-bytes data
+ * byte1[1:0] : FIFO_DATA[17]-FIFO_DATA[16]
+ * byte2[7:0] : FIFO_DATA[15]-FIFO_DATA[8]
+ * byte3[7:0] : FIFO_DATA[7]-FIFO_DATA[0]
+ * The data is left aligned, so FIFO_DATA[17]
+ * is always MSB, although the data length 
+ * can be 18-bit ~ 15-bit
+ */
+uint32_t readFIFO(void)
+{
+    uint32_t data = 0 ;
+    uint8_t res[3] ;
+    readRegs(REG_FIFO_DATA, res, 3) ;
+    data = 
+        ((res[0] & 0x03)<<16)
+        | (res[1] << 8) 
+        | res[2] ;
+    return( data ) ;
+}
+ 
+void maxReset(void)
+{
+    uint8_t res[2] ;
+    res[0] = REG_MODE_CONFIG ;
+    res[1] = 0x40 ; /* reset */
+    writeRegs(res, 2) ;
+}
+
+void maxInit(void)
+{
+    maxim = 1;
+    maxReset();
+    wait(0.05);
+    setLED1_PA(0xFF);
+    setLED2_PA(0x33);
+    setLED3_PA(0xFF);
+    setPILOT_PA(0x19);
+    wait(0.05);
+    setSLOT(0x0300);  
+    setFIFO_CONFIG(0x06);
+    setSPO2_CONFIG(0x42);
+    wait(0.05);
+    setPROX_INT_THR(0x14);
+    clearFIFO();
+    wait(0.05);
+}
+
+void maxEnable(void)
+{
+    setIntEnable(0x8000);
+    setMODE_CONFIG(0x07);
+    Enable_Heart_Rate();    
+}
+
+void maxDeinit(void)
+{
+    maxim = 0;  
+    hr_ticker.detach();
+    hr_measure_ticker.detach();  
+    Disable_Heart_Rate();
+    hr_led.detach();
+}
+
+void processHeartRate(void)
+{
+    
+    uint32_t peak;
+ 
+    for(int i = 0; i < 100; i++)
+    {
+        hr_data[i] = readFIFO();
+        if(i % 3 == 1)
+            wait(0.05);
+    }
+  
+    
+    peak = isPeak(hr_data);
+    
+    uint8_t hr = (uint8_t)((60 * peak * 50)/ 100);
+    if(hr >= HR_Zone1[0] && hr <= HR_Zone4[1])
+    {
+        Heart_Rate = hr;
+        update_display();
+    }
+    Determine_Current_Zone();
+   
+    
+}
+
+uint32_t isPeak(uint32_t  hr_array[])
+{
+    uint32_t peaks[100];
+    uint32_t numPeaks = 0;
+    uint32_t index = 0;
+    uint32_t currentPeak = 0;
+    
+    for(int i = 1; i < 100; i++)
+    {
+        if((hr_array[i] > hr_array[i + 1]) && (hr_array[i] > hr_array[i - 1]))
+        {  
+            peaks[index] = hr_array[i];
+            numPeaks++;
+            index++;
+        }
+    }    
+    if(numPeaks > 0)
+    {
+        currentPeak = peaks[0];
+        for(int i = 0; i < numPeaks; i++)
+        {
+           if(currentPeak < peaks[i])
+           {
+                currentPeak = peaks[i];    
+           }     
+        }   
+    }
+    
+    return currentPeak;
+}    
\ No newline at end of file