Greg Toth / Mbed OS MAX30101WING_HR_SPO2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MAX30101 MAX32620FTHR

main.cpp@11:976c80cc99d5, 2017-12-14 (annotated)

Committer:
johnGreeneMaxim
Date:
Thu Dec 14 22:13:18 2017 +0000
Revision:
11:976c80cc99d5
Parent:
10:fcfa9adc99a9
Child:
12:ac85295f8713
Implemented SKA algorithm.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
coreyharris 5:2f708191f1bd 1 /*******************************************************************************
coreyharris 5:2f708191f1bd 2 * Copyright (C) 2017 Maxim Integrated Products, Inc., All Rights Reserved.
coreyharris 5:2f708191f1bd 3 *
coreyharris 5:2f708191f1bd 4 * Permission is hereby granted, free of charge, to any person obtaining a
coreyharris 5:2f708191f1bd 5 * copy of this software and associated documentation files (the "Software"),
coreyharris 5:2f708191f1bd 6 * to deal in the Software without restriction, including without limitation
coreyharris 5:2f708191f1bd 7 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
coreyharris 5:2f708191f1bd 8 * and/or sell copies of the Software, and to permit persons to whom the
coreyharris 5:2f708191f1bd 9 * Software is furnished to do so, subject to the following conditions:
coreyharris 5:2f708191f1bd 10 *
coreyharris 5:2f708191f1bd 11 * The above copyright notice and this permission notice shall be included
coreyharris 5:2f708191f1bd 12 * in all copies or substantial portions of the Software.
coreyharris 5:2f708191f1bd 13 *
coreyharris 5:2f708191f1bd 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
coreyharris 5:2f708191f1bd 15 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
coreyharris 5:2f708191f1bd 16 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
coreyharris 5:2f708191f1bd 17 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
coreyharris 5:2f708191f1bd 18 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
coreyharris 5:2f708191f1bd 19 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
coreyharris 5:2f708191f1bd 20 * OTHER DEALINGS IN THE SOFTWARE.
coreyharris 5:2f708191f1bd 21 *
coreyharris 5:2f708191f1bd 22 * Except as contained in this notice, the name of Maxim Integrated
coreyharris 5:2f708191f1bd 23 * Products, Inc. shall not be used except as stated in the Maxim Integrated
coreyharris 5:2f708191f1bd 24 * Products, Inc. Branding Policy.
coreyharris 5:2f708191f1bd 25 *
coreyharris 5:2f708191f1bd 26 * The mere transfer of this software does not imply any licenses
coreyharris 5:2f708191f1bd 27 * of trade secrets, proprietary technology, copyrights, patents,
coreyharris 5:2f708191f1bd 28 * trademarks, maskwork rights, or any other form of intellectual
coreyharris 5:2f708191f1bd 29 * property whatsoever. Maxim Integrated Products, Inc. retains all
coreyharris 5:2f708191f1bd 30 * ownership rights.
coreyharris 5:2f708191f1bd 31 *******************************************************************************
coreyharris 5:2f708191f1bd 32 */
johnGreeneMaxim 11:976c80cc99d5 33
coreyharris 5:2f708191f1bd 34
coreyharris 0:0bd4103885bf 35 #include "mbed.h"
coreyharris 0:0bd4103885bf 36 #include "max32630fthr.h"
coreyharris 0:0bd4103885bf 37 #include "MAX30101.h"
johnGreeneMaxim 11:976c80cc99d5 38 #include <stdio.h>
johnGreeneMaxim 11:976c80cc99d5 39 #include <stdlib.h>
johnGreeneMaxim 11:976c80cc99d5 40 #include <math.h>
johnGreeneMaxim 11:976c80cc99d5 41 #include <string.h>
johnGreeneMaxim 9:affd4e6372a0 42
johnGreeneMaxim 11:976c80cc99d5 43 //variable for the algorithm
johnGreeneMaxim 11:976c80cc99d5 44 uint16_t sampleRate =100;
johnGreeneMaxim 11:976c80cc99d5 45 uint16_t compSpO2=1;
johnGreeneMaxim 11:976c80cc99d5 46 int16_t ir_ac_comp =0;
johnGreeneMaxim 11:976c80cc99d5 47 int16_t red_ac_comp=0;
johnGreeneMaxim 11:976c80cc99d5 48 int16_t green_ac_comp=0;
johnGreeneMaxim 11:976c80cc99d5 49 int16_t ir_ac_mag=0;
johnGreeneMaxim 11:976c80cc99d5 50 int16_t red_ac_mag=0;
johnGreeneMaxim 11:976c80cc99d5 51 int16_t green_ac_mag=0;
johnGreeneMaxim 11:976c80cc99d5 52 uint16_t HRbpm2=0;
johnGreeneMaxim 11:976c80cc99d5 53 uint16_t SpO2B=0;
johnGreeneMaxim 11:976c80cc99d5 54 uint16_t DRdy=0;
johnGreeneMaxim 10:fcfa9adc99a9 55
johnGreeneMaxim 11:976c80cc99d5 56 //Heart rate and SpO2 algorithm
johnGreeneMaxim 11:976c80cc99d5 57 void HRSpO2Func(uint32_t dinIR, uint32_t dinRed, uint32_t dinGreen, uint32_t ns,uint16_t SampRate,uint16_t compSpO2,
johnGreeneMaxim 11:976c80cc99d5 58 int16_t *ir_ac_comp,int16_t *red_ac_comp, int16_t *green_ac_comp, int16_t *ir_ac_mag,int16_t *red_ac_mag, int16_t *green_ac_mag, uint16_t *HRbpm2,uint16_t *SpO2B,uint16_t *DRdy );
coreyharris 0:0bd4103885bf 59
johnGreeneMaxim 11:976c80cc99d5 60 //helper functions for the heart rate and SpO2 function
johnGreeneMaxim 11:976c80cc99d5 61 uint16_t avg_dc_est(int32_t *p, uint16_t x);
johnGreeneMaxim 11:976c80cc99d5 62
johnGreeneMaxim 11:976c80cc99d5 63 void lp_dfir_flt(int16_t din0,int16_t din1,int16_t din2, int16_t *dout0,int16_t *dout1,int16_t *dout2) ;
johnGreeneMaxim 11:976c80cc99d5 64 int32_t mul16(int16_t x, int16_t y);
johnGreeneMaxim 11:976c80cc99d5 65
johnGreeneMaxim 11:976c80cc99d5 66 //set logic level to 3.3V
coreyharris 0:0bd4103885bf 67 MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
coreyharris 0:0bd4103885bf 68
johnGreeneMaxim 11:976c80cc99d5 69 //IC configuration functions
coreyharris 0:0bd4103885bf 70 bool op_sensor_config(MAX30101 &op_sensor);
johnGreeneMaxim 8:a1538e8a3fd9 71 void pmic_config(I2C & i2c_bus, DigitalOut & pmic_en);
coreyharris 0:0bd4103885bf 72
coreyharris 0:0bd4103885bf 73 /* Op Sensor FIFO nearly full callback */
johnGreeneMaxim 11:976c80cc99d5 74 volatile bool op_sensorIntFlag = 0;
coreyharris 0:0bd4103885bf 75 void op_sensor_callback()
coreyharris 0:0bd4103885bf 76 {
coreyharris 1:471e2b722d24 77 op_sensorIntFlag = 1;
coreyharris 0:0bd4103885bf 78 }
coreyharris 0:0bd4103885bf 79
johnGreeneMaxim 11:976c80cc99d5 80 //declare large variables outside of main
johnGreeneMaxim 11:976c80cc99d5 81 uint32_t redData[500];//set array to max fifo size
johnGreeneMaxim 11:976c80cc99d5 82 uint32_t irData[500];//set array to max fifo size
johnGreeneMaxim 11:976c80cc99d5 83 uint32_t greenData[500];//set array to max fifo size
johnGreeneMaxim 11:976c80cc99d5 84
johnGreeneMaxim 11:976c80cc99d5 85
coreyharris 0:0bd4103885bf 86 int main()
coreyharris 0:0bd4103885bf 87 {
coreyharris 1:471e2b722d24 88 Serial pc(USBTX, USBRX); // Use USB debug probe for serial link
coreyharris 1:471e2b722d24 89 pc.baud(115200); // Baud rate = 115200
johnGreeneMaxim 11:976c80cc99d5 90
johnGreeneMaxim 11:976c80cc99d5 91 DigitalOut rLed(LED1, LED_OFF); // Debug LED
johnGreeneMaxim 11:976c80cc99d5 92
coreyharris 1:471e2b722d24 93 InterruptIn op_sensor_int(P3_2); // Config P3_2 as int. in for
coreyharris 1:471e2b722d24 94 op_sensor_int.fall(&op_sensor_callback); // FIFO ready interrupt
johnGreeneMaxim 11:976c80cc99d5 95
johnGreeneMaxim 11:976c80cc99d5 96 I2C i2cBus(I2C1_SDA, I2C1_SCL); // I2C bus, P3_4 = SDA, P3_5 = SCL
coreyharris 0:0bd4103885bf 97
johnGreeneMaxim 8:a1538e8a3fd9 98 DigitalOut VLED_EN(P3_3,0); //Enable for VLEDs
johnGreeneMaxim 8:a1538e8a3fd9 99 pmic_config(i2cBus, VLED_EN);
johnGreeneMaxim 11:976c80cc99d5 100
coreyharris 4:14d1b87cd3c4 101 MAX30101 op_sensor(i2cBus); // Create new MAX30101 on i2cBus
coreyharris 4:14d1b87cd3c4 102 int rc = op_sensor_config(op_sensor); // Config sensor, return 0 on success
johnGreeneMaxim 11:976c80cc99d5 103
coreyharris 5:2f708191f1bd 104 MAX30101::InterruptBitField_u ints; // Read interrupt status to clear
johnGreeneMaxim 11:976c80cc99d5 105 rc = op_sensor.getInterruptStatus(ints); // power on interrupt
johnGreeneMaxim 11:976c80cc99d5 106
coreyharris 0:0bd4103885bf 107 uint8_t fifoData[MAX30101::MAX_FIFO_BYTES];
coreyharris 0:0bd4103885bf 108 uint16_t idx, readBytes;
coreyharris 7:6075af57668e 109 int32_t opSample;
coreyharris 2:54182d6a168f 110 uint32_t sample;
johnGreeneMaxim 11:976c80cc99d5 111 uint16_t HRTemp;
johnGreeneMaxim 11:976c80cc99d5 112 uint16_t spo2Temp;
johnGreeneMaxim 10:fcfa9adc99a9 113
johnGreeneMaxim 11:976c80cc99d5 114 int r=0; //counter for redData position
johnGreeneMaxim 11:976c80cc99d5 115 int ir=0; //counter for irData position
johnGreeneMaxim 11:976c80cc99d5 116 int g =0; //counter for greenData position
johnGreeneMaxim 11:976c80cc99d5 117 int c=0; //counter to print values
johnGreeneMaxim 11:976c80cc99d5 118
johnGreeneMaxim 10:fcfa9adc99a9 119 pc.printf("Starting Program...Please wait a few seconds while data is being collected.\r\n");
johnGreeneMaxim 11:976c80cc99d5 120 while(1) {
johnGreeneMaxim 11:976c80cc99d5 121 if( rc == 0 ) {
johnGreeneMaxim 11:976c80cc99d5 122
coreyharris 4:14d1b87cd3c4 123 // Check if op_sensor interrupt asserted
johnGreeneMaxim 11:976c80cc99d5 124 if(op_sensorIntFlag) {
johnGreeneMaxim 10:fcfa9adc99a9 125 //pc.printf("Entered op_sensorIntFlag check\r\n");
coreyharris 1:471e2b722d24 126 op_sensorIntFlag = 0; // Lower interrupt flag
johnGreeneMaxim 11:976c80cc99d5 127 rc = op_sensor.getInterruptStatus(ints); // Read interrupt status
johnGreeneMaxim 11:976c80cc99d5 128 //to clear interrupt
johnGreeneMaxim 11:976c80cc99d5 129
johnGreeneMaxim 11:976c80cc99d5 130 // Confirms proper read prior to executing
johnGreeneMaxim 11:976c80cc99d5 131 if((rc == 0)) {
johnGreeneMaxim 11:976c80cc99d5 132 // Read FIFO
johnGreeneMaxim 11:976c80cc99d5 133 rc = op_sensor.readFIFO(MAX30101::ThreeLedChannels, fifoData, readBytes);
johnGreeneMaxim 11:976c80cc99d5 134
johnGreeneMaxim 11:976c80cc99d5 135 if(rc == 0) {
johnGreeneMaxim 11:976c80cc99d5 136
coreyharris 4:14d1b87cd3c4 137 // Convert read bytes into samples
johnGreeneMaxim 11:976c80cc99d5 138 for(idx = 0, c=0; idx < readBytes; idx+=3) {
johnGreeneMaxim 10:fcfa9adc99a9 139
coreyharris 2:54182d6a168f 140 sample = (fifoData[idx]<<16) | (fifoData[idx+1]<<8) | (fifoData[idx+2]);
johnGreeneMaxim 10:fcfa9adc99a9 141 opSample = sample;
johnGreeneMaxim 10:fcfa9adc99a9 142 opSample&=0x03FFFF; //Mask MSB [23:18]
johnGreeneMaxim 11:976c80cc99d5 143
johnGreeneMaxim 11:976c80cc99d5 144 if(idx==0) {
johnGreeneMaxim 11:976c80cc99d5 145
johnGreeneMaxim 11:976c80cc99d5 146 redData[r] = opSample;//saves to buff for calculations
johnGreeneMaxim 11:976c80cc99d5 147 r++;
johnGreeneMaxim 11:976c80cc99d5 148
johnGreeneMaxim 11:976c80cc99d5 149 } else if(idx==3) {
johnGreeneMaxim 11:976c80cc99d5 150
johnGreeneMaxim 11:976c80cc99d5 151 irData[ir] = opSample; //saves to buff for calculations
johnGreeneMaxim 11:976c80cc99d5 152 ir++;
johnGreeneMaxim 11:976c80cc99d5 153
johnGreeneMaxim 11:976c80cc99d5 154 } else {
johnGreeneMaxim 11:976c80cc99d5 155
johnGreeneMaxim 11:976c80cc99d5 156 greenData[ir] = opSample; //saves to buff for calculations
johnGreeneMaxim 11:976c80cc99d5 157 g++;
johnGreeneMaxim 11:976c80cc99d5 158
johnGreeneMaxim 11:976c80cc99d5 159 }
johnGreeneMaxim 11:976c80cc99d5 160
johnGreeneMaxim 11:976c80cc99d5 161 }
johnGreeneMaxim 11:976c80cc99d5 162
johnGreeneMaxim 11:976c80cc99d5 163
johnGreeneMaxim 11:976c80cc99d5 164 if(r>=500 & ir>=500 & g>500)//checks to make sure there are 500
johnGreeneMaxim 11:976c80cc99d5 165 //samples in data buffers
johnGreeneMaxim 11:976c80cc99d5 166 {
johnGreeneMaxim 11:976c80cc99d5 167
johnGreeneMaxim 11:976c80cc99d5 168 //runs the heart rate and SpO2 algorithm
johnGreeneMaxim 11:976c80cc99d5 169 for(c=0, HRTemp = 0; c<r; c++) {
johnGreeneMaxim 11:976c80cc99d5 170
johnGreeneMaxim 11:976c80cc99d5 171 HRSpO2Func(irData[c], redData[c],greenData[c], c,sampleRate, compSpO2,
johnGreeneMaxim 11:976c80cc99d5 172 &ir_ac_comp,&red_ac_comp, &green_ac_comp, &ir_ac_mag,&red_ac_mag,
johnGreeneMaxim 11:976c80cc99d5 173 &green_ac_mag, &HRbpm2,&SpO2B,&DRdy);
johnGreeneMaxim 11:976c80cc99d5 174 if(DRdy)
johnGreeneMaxim 11:976c80cc99d5 175 {
johnGreeneMaxim 11:976c80cc99d5 176 HRTemp = HRbpm2;
johnGreeneMaxim 11:976c80cc99d5 177 spo2Temp = SpO2B;
johnGreeneMaxim 11:976c80cc99d5 178 }
johnGreeneMaxim 11:976c80cc99d5 179
johnGreeneMaxim 11:976c80cc99d5 180 }
johnGreeneMaxim 11:976c80cc99d5 181
johnGreeneMaxim 11:976c80cc99d5 182 //If the above algorithm returns a valid heart rate on the last sample, it is printed
johnGreeneMaxim 11:976c80cc99d5 183 if(DRdy==1) {
johnGreeneMaxim 11:976c80cc99d5 184 pc.printf("Heart Rate = %i\r\n",HRbpm2);
johnGreeneMaxim 11:976c80cc99d5 185 pc.printf("SPO2 = %i\r\n",SpO2B);
johnGreeneMaxim 11:976c80cc99d5 186 }
johnGreeneMaxim 11:976c80cc99d5 187 else if (HRTemp!=0)//if a valid heart was calculated at all, it is printed
johnGreeneMaxim 9:affd4e6372a0 188 {
johnGreeneMaxim 11:976c80cc99d5 189 pc.printf("Heart Rate = %i\r\n",HRTemp);
johnGreeneMaxim 11:976c80cc99d5 190 pc.printf("SPO2 = %i\r\n",spo2Temp);
johnGreeneMaxim 9:affd4e6372a0 191 }
johnGreeneMaxim 9:affd4e6372a0 192 else
johnGreeneMaxim 9:affd4e6372a0 193 {
johnGreeneMaxim 11:976c80cc99d5 194 pc.printf("Calculation failed...waiting for more samples...\r\n");
johnGreeneMaxim 11:976c80cc99d5 195 pc.printf("Please keep your finger on the MAX30101 sensor with minimal movement.\r\n");
johnGreeneMaxim 9:affd4e6372a0 196 }
johnGreeneMaxim 11:976c80cc99d5 197
johnGreeneMaxim 11:976c80cc99d5 198 //dump the first hundred samples after caluclaiton
johnGreeneMaxim 11:976c80cc99d5 199 for(c=100; c<500; c++)
johnGreeneMaxim 11:976c80cc99d5 200
johnGreeneMaxim 9:affd4e6372a0 201 {
johnGreeneMaxim 9:affd4e6372a0 202 redData[c-100]=redData[c];
johnGreeneMaxim 9:affd4e6372a0 203 irData[c-100]=irData[c];
johnGreeneMaxim 11:976c80cc99d5 204 greenData[c-100] = greenData[c];
johnGreeneMaxim 9:affd4e6372a0 205
johnGreeneMaxim 9:affd4e6372a0 206 }
johnGreeneMaxim 11:976c80cc99d5 207 //reset counters
johnGreeneMaxim 11:976c80cc99d5 208 r=400;
johnGreeneMaxim 11:976c80cc99d5 209 ir=400;
johnGreeneMaxim 11:976c80cc99d5 210 g=400;
johnGreeneMaxim 11:976c80cc99d5 211 }
coreyharris 0:0bd4103885bf 212 }
coreyharris 0:0bd4103885bf 213 }
coreyharris 0:0bd4103885bf 214 }
johnGreeneMaxim 11:976c80cc99d5 215
johnGreeneMaxim 11:976c80cc99d5 216 // If rc != 0, a communication error has occurred
johnGreeneMaxim 11:976c80cc99d5 217 } else {
johnGreeneMaxim 11:976c80cc99d5 218
coreyharris 4:14d1b87cd3c4 219 pc.printf("Something went wrong, "
coreyharris 4:14d1b87cd3c4 220 "check the I2C bus or power connections... \r\n");
johnGreeneMaxim 11:976c80cc99d5 221 //bLed = LED_OFF;
johnGreeneMaxim 11:976c80cc99d5 222 //gLed = LED_OFF;
johnGreeneMaxim 11:976c80cc99d5 223
johnGreeneMaxim 11:976c80cc99d5 224 while(1) {
coreyharris 0:0bd4103885bf 225 rLed = !rLed;
johnGreeneMaxim 11:976c80cc99d5 226 wait(0.5);
johnGreeneMaxim 11:976c80cc99d5 227 }
johnGreeneMaxim 11:976c80cc99d5 228 }
johnGreeneMaxim 11:976c80cc99d5 229
johnGreeneMaxim 11:976c80cc99d5 230 }
coreyharris 0:0bd4103885bf 231 }
coreyharris 0:0bd4103885bf 232
coreyharris 0:0bd4103885bf 233
johnGreeneMaxim 11:976c80cc99d5 234 bool op_sensor_config(MAX30101 &op_sensor)
johnGreeneMaxim 11:976c80cc99d5 235 {
johnGreeneMaxim 11:976c80cc99d5 236
coreyharris 0:0bd4103885bf 237 //Reset Device
coreyharris 0:0bd4103885bf 238 MAX30101::ModeConfiguration_u modeConfig;
coreyharris 0:0bd4103885bf 239 modeConfig.all = 0;
coreyharris 0:0bd4103885bf 240 modeConfig.bits.reset = 1;
johnGreeneMaxim 11:976c80cc99d5 241 modeConfig.bits.mode = MAX30101::MultiLedMode; // Sets SPO2 Mode
coreyharris 0:0bd4103885bf 242 int32_t rc = op_sensor.setModeConfiguration(modeConfig);
johnGreeneMaxim 11:976c80cc99d5 243
johnGreeneMaxim 11:976c80cc99d5 244
coreyharris 0:0bd4103885bf 245 //enable MAX30101 interrupts
coreyharris 0:0bd4103885bf 246 MAX30101::InterruptBitField_u ints;
johnGreeneMaxim 11:976c80cc99d5 247 if(rc == 0) {
coreyharris 0:0bd4103885bf 248 ints.all = 0;
johnGreeneMaxim 9:affd4e6372a0 249 ints.bits.a_full = 1; // Enable FIFO almost full interrupt
johnGreeneMaxim 11:976c80cc99d5 250 ints.bits.ppg_rdy =1; //Enables an interrupt when a new sample is ready
coreyharris 0:0bd4103885bf 251 rc = op_sensor.enableInterrupts(ints);
coreyharris 0:0bd4103885bf 252 }
johnGreeneMaxim 11:976c80cc99d5 253
coreyharris 0:0bd4103885bf 254 //configure FIFO
coreyharris 0:0bd4103885bf 255 MAX30101::FIFO_Configuration_u fifoConfig;
johnGreeneMaxim 11:976c80cc99d5 256 if(rc == 0) {
coreyharris 0:0bd4103885bf 257 fifoConfig.all = 0;
johnGreeneMaxim 11:976c80cc99d5 258 fifoConfig.bits.fifo_a_full = 10; // Max level of 17 samples
johnGreeneMaxim 11:976c80cc99d5 259 fifoConfig.bits.sample_average = MAX30101::AveragedSamples_0;// Average 0 samples
coreyharris 0:0bd4103885bf 260 rc = op_sensor.setFIFOConfiguration(fifoConfig);
coreyharris 0:0bd4103885bf 261 }
johnGreeneMaxim 11:976c80cc99d5 262
coreyharris 0:0bd4103885bf 263 MAX30101::SpO2Configuration_u spo2Config;
johnGreeneMaxim 11:976c80cc99d5 264 if(rc == 0) {
johnGreeneMaxim 11:976c80cc99d5 265 spo2Config.all = 0; // clears register
johnGreeneMaxim 11:976c80cc99d5 266 spo2Config.bits.spo2_adc_range = 1; //sets resolution to 4096 nAfs
johnGreeneMaxim 11:976c80cc99d5 267 spo2Config.bits.spo2_sr = MAX30101::SR_100_Hz; // SpO2 SR = 100Hz
johnGreeneMaxim 11:976c80cc99d5 268 spo2Config.bits.led_pw = MAX30101::PW_3; // 18-bit ADC resolution ~400us
coreyharris 0:0bd4103885bf 269 rc = op_sensor.setSpO2Configuration(spo2Config);
coreyharris 0:0bd4103885bf 270 }
johnGreeneMaxim 11:976c80cc99d5 271
johnGreeneMaxim 11:976c80cc99d5 272 //Set time slots for LEDS
johnGreeneMaxim 11:976c80cc99d5 273 MAX30101::ModeControlReg_u multiLED;
johnGreeneMaxim 11:976c80cc99d5 274 if(rc==0) {
johnGreeneMaxim 11:976c80cc99d5 275 //sets timing for control register 1
johnGreeneMaxim 11:976c80cc99d5 276 multiLED.bits.lo_slot=1;
johnGreeneMaxim 11:976c80cc99d5 277 multiLED.bits.hi_slot=2;
johnGreeneMaxim 11:976c80cc99d5 278 rc = op_sensor.setMultiLEDModeControl(MAX30101::ModeControlReg1, multiLED);
johnGreeneMaxim 11:976c80cc99d5 279 if(rc==0) {
johnGreeneMaxim 11:976c80cc99d5 280 multiLED.bits.lo_slot=3;
johnGreeneMaxim 11:976c80cc99d5 281 multiLED.bits.hi_slot=0;
johnGreeneMaxim 11:976c80cc99d5 282 rc = op_sensor.setMultiLEDModeControl(MAX30101::ModeControlReg2, multiLED);
johnGreeneMaxim 9:affd4e6372a0 283 }
coreyharris 0:0bd4103885bf 284 }
johnGreeneMaxim 11:976c80cc99d5 285
johnGreeneMaxim 11:976c80cc99d5 286 //Set LED drive currents
johnGreeneMaxim 11:976c80cc99d5 287 if(rc == 0) {
johnGreeneMaxim 11:976c80cc99d5 288 // Heart Rate only, 1 LED channel, Pulse amp. = ~7mA
johnGreeneMaxim 11:976c80cc99d5 289 rc = op_sensor.setLEDPulseAmplitude(MAX30101::LED1_PA, 0x24);
johnGreeneMaxim 11:976c80cc99d5 290 //To include SPO2, 2 LED channel, Pulse amp. ~7mA
johnGreeneMaxim 11:976c80cc99d5 291 if(rc==0) {
johnGreeneMaxim 11:976c80cc99d5 292 rc = op_sensor.setLEDPulseAmplitude(MAX30101::LED2_PA, 0x24);
johnGreeneMaxim 11:976c80cc99d5 293 }
johnGreeneMaxim 11:976c80cc99d5 294 if(rc==0) {
johnGreeneMaxim 11:976c80cc99d5 295 rc = op_sensor.setLEDPulseAmplitude(MAX30101::LED3_PA, 0x24);
johnGreeneMaxim 11:976c80cc99d5 296 }
johnGreeneMaxim 11:976c80cc99d5 297
johnGreeneMaxim 11:976c80cc99d5 298 }
johnGreeneMaxim 11:976c80cc99d5 299
coreyharris 0:0bd4103885bf 300 //Set operating mode
coreyharris 0:0bd4103885bf 301 modeConfig.all = 0;
johnGreeneMaxim 11:976c80cc99d5 302 if(rc == 0) {
johnGreeneMaxim 11:976c80cc99d5 303 modeConfig.bits.mode = MAX30101::MultiLedMode; // Sets multiLED mode
coreyharris 0:0bd4103885bf 304 rc = op_sensor.setModeConfiguration(modeConfig);
johnGreeneMaxim 11:976c80cc99d5 305 }
johnGreeneMaxim 11:976c80cc99d5 306
johnGreeneMaxim 11:976c80cc99d5 307
coreyharris 0:0bd4103885bf 308 return rc;
coreyharris 0:0bd4103885bf 309 }
johnGreeneMaxim 8:a1538e8a3fd9 310
johnGreeneMaxim 8:a1538e8a3fd9 311 void pmic_config(I2C & i2c_bus, DigitalOut & pmic_en)
johnGreeneMaxim 8:a1538e8a3fd9 312 {
johnGreeneMaxim 11:976c80cc99d5 313
johnGreeneMaxim 8:a1538e8a3fd9 314 const uint8_t PMIC_ADRS = 0x54;
johnGreeneMaxim 8:a1538e8a3fd9 315 const uint8_t BBB_EXTRA_ADRS = 0x1C;
johnGreeneMaxim 8:a1538e8a3fd9 316 const uint8_t BOOST_VOLTAGE = 0x05;
johnGreeneMaxim 11:976c80cc99d5 317
johnGreeneMaxim 11:976c80cc99d5 318 char data_buff[] = {BBB_EXTRA_ADRS, 0x40}; //BBBExtra register address
johnGreeneMaxim 11:976c80cc99d5 319 //and data to enable passive
johnGreeneMaxim 11:976c80cc99d5 320 //pull down.
johnGreeneMaxim 8:a1538e8a3fd9 321 i2c_bus.write(PMIC_ADRS, data_buff,2); //write to BBBExtra register
johnGreeneMaxim 11:976c80cc99d5 322
johnGreeneMaxim 8:a1538e8a3fd9 323 data_buff[0] = BOOST_VOLTAGE;
johnGreeneMaxim 11:976c80cc99d5 324 data_buff[1] = 0x08; //Boost voltage configuration
johnGreeneMaxim 11:976c80cc99d5 325 //register followed by data
johnGreeneMaxim 11:976c80cc99d5 326 //to set voltage to 4.5V 1f
johnGreeneMaxim 11:976c80cc99d5 327 pmic_en = 0; //disables VLED 08
johnGreeneMaxim 8:a1538e8a3fd9 328 i2c_bus.write(PMIC_ADRS, data_buff,2); //write to BBBExtra register
johnGreeneMaxim 11:976c80cc99d5 329 pmic_en = 1; //enables VLED
johnGreeneMaxim 11:976c80cc99d5 330 }
johnGreeneMaxim 11:976c80cc99d5 331
johnGreeneMaxim 11:976c80cc99d5 332
johnGreeneMaxim 11:976c80cc99d5 333
johnGreeneMaxim 11:976c80cc99d5 334 //
johnGreeneMaxim 11:976c80cc99d5 335 // Heart Rate/SpO2 Monitor function takes sample input 'dinIR' and dinRed.
johnGreeneMaxim 11:976c80cc99d5 336 // Other inputs:
johnGreeneMaxim 11:976c80cc99d5 337 // ns -> Sample Counter, increments with each sample input.
johnGreeneMaxim 11:976c80cc99d5 338 // SampRate -> Input data real-time sample rate.
johnGreeneMaxim 11:976c80cc99d5 339 // dinLShft -> Number of left shifts for data to be 16 bit wide.
johnGreeneMaxim 11:976c80cc99d5 340 // compSpO2 -> If '1' compute SpO2 value,else compute HR only.
johnGreeneMaxim 11:976c80cc99d5 341
johnGreeneMaxim 11:976c80cc99d5 342 //
johnGreeneMaxim 11:976c80cc99d5 343 // Outputs:
johnGreeneMaxim 11:976c80cc99d5 344 // ir_ac_comp -> AC component of the IR signal.
johnGreeneMaxim 11:976c80cc99d5 345 // red_ac_comp -> AC component of the Red signal.
johnGreeneMaxim 11:976c80cc99d5 346 // ir_ac_mag -> Peak to Peak magnitude of the IR signal.
johnGreeneMaxim 11:976c80cc99d5 347 // red_ac_mag -> Peak to Peak magnitude of the Red signal.
johnGreeneMaxim 11:976c80cc99d5 348 // HRbpm -> Heart Rate in beats per minute.
johnGreeneMaxim 11:976c80cc99d5 349 // SpO2 -> SpO2 value as %saturation.
johnGreeneMaxim 11:976c80cc99d5 350 // DRdy -> '1' when new data is available.
johnGreeneMaxim 11:976c80cc99d5 351 //
johnGreeneMaxim 11:976c80cc99d5 352
johnGreeneMaxim 11:976c80cc99d5 353 void HRSpO2Func(uint32_t dinIR, uint32_t dinRed, uint32_t dinGreen, uint32_t ns,uint16_t SampRate,uint16_t compSpO2,
johnGreeneMaxim 11:976c80cc99d5 354 int16_t *ir_ac_comp,int16_t *red_ac_comp, int16_t *green_ac_comp, int16_t *ir_ac_mag,int16_t *red_ac_mag,
johnGreeneMaxim 11:976c80cc99d5 355 int16_t *green_ac_mag, uint16_t *HRbpm2,uint16_t *SpO2B,uint16_t *DRdy )
johnGreeneMaxim 11:976c80cc99d5 356 {
johnGreeneMaxim 11:976c80cc99d5 357 static int32_t ir_avg_reg=0;
johnGreeneMaxim 11:976c80cc99d5 358 static int32_t red_avg_reg=0;
johnGreeneMaxim 11:976c80cc99d5 359 static int32_t green_avg_reg=0;
johnGreeneMaxim 11:976c80cc99d5 360
johnGreeneMaxim 11:976c80cc99d5 361 static int16_t ir_ac_sig_cur=0;
johnGreeneMaxim 11:976c80cc99d5 362 static int16_t ir_ac_sig_pre;
johnGreeneMaxim 11:976c80cc99d5 363 static int16_t ir_ac_sig_min=0;
johnGreeneMaxim 11:976c80cc99d5 364 static int16_t ir_ac_sig_max=0;
johnGreeneMaxim 11:976c80cc99d5 365 static int16_t ir_avg_est;
johnGreeneMaxim 11:976c80cc99d5 366
johnGreeneMaxim 11:976c80cc99d5 367 static int16_t ir_pedge=0, ir_nedge=0;
johnGreeneMaxim 11:976c80cc99d5 368 static int16_t ir_pzxic, ir_pzxip;
johnGreeneMaxim 11:976c80cc99d5 369 static int16_t ir_nzxic;
johnGreeneMaxim 11:976c80cc99d5 370
johnGreeneMaxim 11:976c80cc99d5 371 static int16_t red_ac_sig_cur=0;
johnGreeneMaxim 11:976c80cc99d5 372 static int16_t red_ac_sig_min=0;
johnGreeneMaxim 11:976c80cc99d5 373 static int16_t red_ac_sig_max=0;
johnGreeneMaxim 11:976c80cc99d5 374 static int16_t red_avg_est;
johnGreeneMaxim 11:976c80cc99d5 375
johnGreeneMaxim 11:976c80cc99d5 376 static int16_t green_avg_est;
johnGreeneMaxim 11:976c80cc99d5 377 static int16_t green_ac_sig_cur=0;
johnGreeneMaxim 11:976c80cc99d5 378 //static int16_t green_ac_sig_cur=0;
johnGreeneMaxim 11:976c80cc99d5 379 static int16_t green_ac_sig_pre;
johnGreeneMaxim 11:976c80cc99d5 380 static int16_t green_ac_sig_max ;
johnGreeneMaxim 11:976c80cc99d5 381 static int16_t green_ac_sig_min;
johnGreeneMaxim 11:976c80cc99d5 382 static int16_t green_mac_FIFO[5];
johnGreeneMaxim 11:976c80cc99d5 383 int16_t meanGreenMagFIFO;
johnGreeneMaxim 11:976c80cc99d5 384 int16_t minAmpForHeartBeat ;
johnGreeneMaxim 11:976c80cc99d5 385
johnGreeneMaxim 11:976c80cc99d5 386 uint32_t IRData,RedData, greenData , rnum,rden,rdens;
johnGreeneMaxim 11:976c80cc99d5 387 uint16_t zeros_in_HrQue =0 , posCount=0;
johnGreeneMaxim 11:976c80cc99d5 388 static uint32_t prevPeakLoc = 0 ;
johnGreeneMaxim 11:976c80cc99d5 389 static int16_t IrFIFO[100];
johnGreeneMaxim 11:976c80cc99d5 390 static int16_t HrQue[10], lastKnownGoodHr[10];
johnGreeneMaxim 11:976c80cc99d5 391 static int16_t SPO2Que[5];
johnGreeneMaxim 11:976c80cc99d5 392 int16_t SPO2score[5];
johnGreeneMaxim 11:976c80cc99d5 393 static uint16_t HrQindex=0 ,lengthOfposCountExceeding =0 ;
johnGreeneMaxim 11:976c80cc99d5 394 static uint16_t initHrQueCounter=0 , fingerOff =0;
johnGreeneMaxim 11:976c80cc99d5 395
johnGreeneMaxim 11:976c80cc99d5 396 static int16_t HrQueSmoothing[3];
johnGreeneMaxim 11:976c80cc99d5 397 static int16_t SPO2QueSmoothing[3];
johnGreeneMaxim 11:976c80cc99d5 398
johnGreeneMaxim 11:976c80cc99d5 399 int16_t k, j;
johnGreeneMaxim 11:976c80cc99d5 400 uint32_t peakLoc ;
johnGreeneMaxim 11:976c80cc99d5 401 int16_t bufferIdx1, bufferIdx2;
johnGreeneMaxim 11:976c80cc99d5 402 int16_t maxFIFO ,IdxMaxFIFO ;
johnGreeneMaxim 11:976c80cc99d5 403 int16_t HRperiod2, HRComp2 ,deltaHR;
johnGreeneMaxim 11:976c80cc99d5 404 int16_t cSpO2, SpO2;
johnGreeneMaxim 11:976c80cc99d5 405
johnGreeneMaxim 11:976c80cc99d5 406 int16_t HrCount =0, HrSum =0 ,meanGreenMagFIFOcounter =0;
johnGreeneMaxim 11:976c80cc99d5 407 int16_t SPO2D ,meanHrQ;
johnGreeneMaxim 11:976c80cc99d5 408 int16_t dx[99] ,cumsumX[99];
johnGreeneMaxim 11:976c80cc99d5 409 static int16_t SPO2QueCounter =0 ;//, lastDisplayedHrValue;
johnGreeneMaxim 11:976c80cc99d5 410
johnGreeneMaxim 11:976c80cc99d5 411 int16_t validSPO2Count =0;
johnGreeneMaxim 11:976c80cc99d5 412 int16_t validSPO2Sum =0;
johnGreeneMaxim 11:976c80cc99d5 413 int16_t SPO2scoreAverage= 0;
johnGreeneMaxim 11:976c80cc99d5 414 int16_t SPO2scoreSum =0 ;
johnGreeneMaxim 11:976c80cc99d5 415 // int16_t deltaMeanLastKnownGoodHr=0,meanLastKnownGoodHr =0 ;
johnGreeneMaxim 11:976c80cc99d5 416 // int16_t counterMeanLastKnownGoodHr =0 ;
johnGreeneMaxim 11:976c80cc99d5 417
johnGreeneMaxim 11:976c80cc99d5 418
johnGreeneMaxim 11:976c80cc99d5 419 // clear some vars if fresh new start
johnGreeneMaxim 11:976c80cc99d5 420 if ((ns ==0) || (fingerOff >300)) {
johnGreeneMaxim 11:976c80cc99d5 421 ir_avg_reg=0;
johnGreeneMaxim 11:976c80cc99d5 422 red_avg_reg=0;
johnGreeneMaxim 11:976c80cc99d5 423 green_avg_reg=0;
johnGreeneMaxim 11:976c80cc99d5 424
johnGreeneMaxim 11:976c80cc99d5 425 ir_ac_sig_cur=0;
johnGreeneMaxim 11:976c80cc99d5 426 ir_ac_sig_pre=0;
johnGreeneMaxim 11:976c80cc99d5 427 ir_ac_sig_min=0;
johnGreeneMaxim 11:976c80cc99d5 428 ir_ac_sig_max=0;
johnGreeneMaxim 11:976c80cc99d5 429
johnGreeneMaxim 11:976c80cc99d5 430 ir_avg_est=0;
johnGreeneMaxim 11:976c80cc99d5 431 green_avg_est =0;
johnGreeneMaxim 11:976c80cc99d5 432 red_avg_est =0 ;
johnGreeneMaxim 11:976c80cc99d5 433
johnGreeneMaxim 11:976c80cc99d5 434 ir_pedge=0;
johnGreeneMaxim 11:976c80cc99d5 435 ir_nedge=0;
johnGreeneMaxim 11:976c80cc99d5 436 ir_pzxic=0;
johnGreeneMaxim 11:976c80cc99d5 437 ir_pzxip =0;
johnGreeneMaxim 11:976c80cc99d5 438 ir_nzxic=0 ;
johnGreeneMaxim 11:976c80cc99d5 439 //ir_nzxip =0;
johnGreeneMaxim 11:976c80cc99d5 440 red_ac_sig_cur=0;
johnGreeneMaxim 11:976c80cc99d5 441 red_ac_sig_min=0;
johnGreeneMaxim 11:976c80cc99d5 442 red_ac_sig_max=0;
johnGreeneMaxim 11:976c80cc99d5 443
johnGreeneMaxim 11:976c80cc99d5 444 prevPeakLoc = 0 ;
johnGreeneMaxim 11:976c80cc99d5 445 bufferIdx1=0 ;
johnGreeneMaxim 11:976c80cc99d5 446 bufferIdx2=0;
johnGreeneMaxim 11:976c80cc99d5 447 HrQindex =0;
johnGreeneMaxim 11:976c80cc99d5 448 initHrQueCounter=0;
johnGreeneMaxim 11:976c80cc99d5 449 lengthOfposCountExceeding =0 ;
johnGreeneMaxim 11:976c80cc99d5 450 fingerOff =0;
johnGreeneMaxim 11:976c80cc99d5 451 HRComp2 =0;
johnGreeneMaxim 11:976c80cc99d5 452
johnGreeneMaxim 11:976c80cc99d5 453 for (k=0 ; k<100 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 454 IrFIFO[k]= 0;
johnGreeneMaxim 11:976c80cc99d5 455 }
johnGreeneMaxim 11:976c80cc99d5 456 for (k=0 ; k<10 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 457 HrQue[k]= 0;
johnGreeneMaxim 11:976c80cc99d5 458 lastKnownGoodHr[k]=0;
johnGreeneMaxim 11:976c80cc99d5 459 }
johnGreeneMaxim 11:976c80cc99d5 460 for (k=0 ; k<3 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 461 HrQueSmoothing[k]= 70;
johnGreeneMaxim 11:976c80cc99d5 462 SPO2QueSmoothing[k]=97;
johnGreeneMaxim 11:976c80cc99d5 463 }
johnGreeneMaxim 11:976c80cc99d5 464 for (k=0 ; k<5 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 465 SPO2Que[k] =97;
johnGreeneMaxim 11:976c80cc99d5 466 SPO2score[k] =0;
johnGreeneMaxim 11:976c80cc99d5 467 green_mac_FIFO[k] =0;
johnGreeneMaxim 11:976c80cc99d5 468 }
johnGreeneMaxim 11:976c80cc99d5 469 SPO2QueCounter =0;
johnGreeneMaxim 11:976c80cc99d5 470 *SpO2B =97;
johnGreeneMaxim 11:976c80cc99d5 471 *HRbpm2 = 0;
johnGreeneMaxim 11:976c80cc99d5 472 *DRdy =0 ;
johnGreeneMaxim 11:976c80cc99d5 473
johnGreeneMaxim 11:976c80cc99d5 474 }
johnGreeneMaxim 11:976c80cc99d5 475
johnGreeneMaxim 11:976c80cc99d5 476
johnGreeneMaxim 11:976c80cc99d5 477 // Save current state
johnGreeneMaxim 11:976c80cc99d5 478 green_ac_sig_pre = green_ac_sig_cur;
johnGreeneMaxim 11:976c80cc99d5 479 //
johnGreeneMaxim 11:976c80cc99d5 480 // Process next data sample
johnGreeneMaxim 11:976c80cc99d5 481 minAmpForHeartBeat =0;
johnGreeneMaxim 11:976c80cc99d5 482 IRData = dinIR;
johnGreeneMaxim 11:976c80cc99d5 483 RedData = dinRed;
johnGreeneMaxim 11:976c80cc99d5 484 greenData = dinGreen ;
johnGreeneMaxim 11:976c80cc99d5 485
johnGreeneMaxim 11:976c80cc99d5 486 ir_avg_est = avg_dc_est(&ir_avg_reg,IRData);
johnGreeneMaxim 11:976c80cc99d5 487 red_avg_est = avg_dc_est(&red_avg_reg,RedData);
johnGreeneMaxim 11:976c80cc99d5 488 green_avg_est = avg_dc_est(&green_avg_reg,greenData);
johnGreeneMaxim 11:976c80cc99d5 489
johnGreeneMaxim 11:976c80cc99d5 490 lp_dfir_flt((uint16_t)(IRData - ir_avg_est),(uint16_t)(RedData - red_avg_est),
johnGreeneMaxim 11:976c80cc99d5 491 (uint16_t)(greenData - green_avg_est), &ir_ac_sig_cur,&red_ac_sig_cur,&green_ac_sig_cur);
johnGreeneMaxim 11:976c80cc99d5 492
johnGreeneMaxim 11:976c80cc99d5 493
johnGreeneMaxim 11:976c80cc99d5 494 *ir_ac_comp = ir_ac_sig_cur;
johnGreeneMaxim 11:976c80cc99d5 495 *red_ac_comp = red_ac_sig_cur;
johnGreeneMaxim 11:976c80cc99d5 496 *green_ac_comp = green_ac_sig_cur;
johnGreeneMaxim 11:976c80cc99d5 497
johnGreeneMaxim 11:976c80cc99d5 498 //save to FIFO
johnGreeneMaxim 11:976c80cc99d5 499 for (k=1 ; k<100 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 500 IrFIFO[100 -k]= IrFIFO[99-k];
johnGreeneMaxim 11:976c80cc99d5 501 }
johnGreeneMaxim 11:976c80cc99d5 502 IrFIFO[0] = green_ac_sig_cur ; // invert
johnGreeneMaxim 11:976c80cc99d5 503 for (k=0 ; k<97 ; k++)
johnGreeneMaxim 11:976c80cc99d5 504 dx[k]= IrFIFO[k+2]-IrFIFO[k] ;
johnGreeneMaxim 11:976c80cc99d5 505 dx[97]= dx[96];
johnGreeneMaxim 11:976c80cc99d5 506 dx[98]=dx[96];
johnGreeneMaxim 11:976c80cc99d5 507
johnGreeneMaxim 11:976c80cc99d5 508 for (k=0 ; k<99 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 509 if (dx[k] > 0 )
johnGreeneMaxim 11:976c80cc99d5 510 dx[k]=1;
johnGreeneMaxim 11:976c80cc99d5 511 else
johnGreeneMaxim 11:976c80cc99d5 512 dx[k] = 0;
johnGreeneMaxim 11:976c80cc99d5 513 }
johnGreeneMaxim 11:976c80cc99d5 514
johnGreeneMaxim 11:976c80cc99d5 515 cumsumX[0] =0;
johnGreeneMaxim 11:976c80cc99d5 516 for (k=1; k<99 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 517 if (dx[k]>0 )
johnGreeneMaxim 11:976c80cc99d5 518 cumsumX[k]= cumsumX[k-1] + dx[k] ;
johnGreeneMaxim 11:976c80cc99d5 519 else
johnGreeneMaxim 11:976c80cc99d5 520 cumsumX[k]= 0;
johnGreeneMaxim 11:976c80cc99d5 521 }
johnGreeneMaxim 11:976c80cc99d5 522 // determine noise
johnGreeneMaxim 11:976c80cc99d5 523 // ignore less than 3 conseuctive non-zeros's
johnGreeneMaxim 11:976c80cc99d5 524 // detect # of sign change
johnGreeneMaxim 11:976c80cc99d5 525 posCount=0;
johnGreeneMaxim 11:976c80cc99d5 526 for (k=1; k<99 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 527 if (cumsumX[k]>0 ) {
johnGreeneMaxim 11:976c80cc99d5 528 posCount ++ ;
johnGreeneMaxim 11:976c80cc99d5 529 } else if (cumsumX[k]==0 ) {
johnGreeneMaxim 11:976c80cc99d5 530 if (posCount<4 && k>=4) {
johnGreeneMaxim 11:976c80cc99d5 531 for ( j= k-1; j> k-posCount-1; j--)
johnGreeneMaxim 11:976c80cc99d5 532 cumsumX[j]=0 ;
johnGreeneMaxim 11:976c80cc99d5 533 }
johnGreeneMaxim 11:976c80cc99d5 534 posCount =0;
johnGreeneMaxim 11:976c80cc99d5 535 }
johnGreeneMaxim 11:976c80cc99d5 536 }
johnGreeneMaxim 11:976c80cc99d5 537 // ignore less than 3 conseuctive zeros's
johnGreeneMaxim 11:976c80cc99d5 538
johnGreeneMaxim 11:976c80cc99d5 539 posCount=0;
johnGreeneMaxim 11:976c80cc99d5 540 for (k=1; k<99 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 541 if (cumsumX[k]==0 ) {
johnGreeneMaxim 11:976c80cc99d5 542 posCount ++ ;
johnGreeneMaxim 11:976c80cc99d5 543 } else if (cumsumX[k] > 0 ) {
johnGreeneMaxim 11:976c80cc99d5 544 if (posCount<4 && k>=4) {
johnGreeneMaxim 11:976c80cc99d5 545 for ( j= k-1; j> k-posCount-1; j--)
johnGreeneMaxim 11:976c80cc99d5 546 cumsumX[j]=100 ;
johnGreeneMaxim 11:976c80cc99d5 547 }
johnGreeneMaxim 11:976c80cc99d5 548 posCount =0;
johnGreeneMaxim 11:976c80cc99d5 549 }
johnGreeneMaxim 11:976c80cc99d5 550 }
johnGreeneMaxim 11:976c80cc99d5 551
johnGreeneMaxim 11:976c80cc99d5 552 //// detect # of sign change
johnGreeneMaxim 11:976c80cc99d5 553 posCount=0; // sign change counter
johnGreeneMaxim 11:976c80cc99d5 554 for (k=0; k<98 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 555 if (cumsumX[k]==0 && cumsumX[k+1] > 0 ) {
johnGreeneMaxim 11:976c80cc99d5 556 posCount ++;
johnGreeneMaxim 11:976c80cc99d5 557 }
johnGreeneMaxim 11:976c80cc99d5 558 }
johnGreeneMaxim 11:976c80cc99d5 559 if (posCount>=4) {
johnGreeneMaxim 11:976c80cc99d5 560 lengthOfposCountExceeding ++ ;
johnGreeneMaxim 11:976c80cc99d5 561 // printf("PosCount =%i \n", posCount );
johnGreeneMaxim 11:976c80cc99d5 562 } else
johnGreeneMaxim 11:976c80cc99d5 563 lengthOfposCountExceeding = 0 ;
johnGreeneMaxim 11:976c80cc99d5 564 // Detect IR channel positive zero crossing (rising edge)
johnGreeneMaxim 11:976c80cc99d5 565 if ((green_ac_sig_pre < 0) && (green_ac_sig_cur >= 0) && fingerOff==0 ) {
johnGreeneMaxim 11:976c80cc99d5 566
johnGreeneMaxim 11:976c80cc99d5 567 *ir_ac_mag = ir_ac_sig_max - ir_ac_sig_min;
johnGreeneMaxim 11:976c80cc99d5 568 *red_ac_mag = red_ac_sig_max - red_ac_sig_min;
johnGreeneMaxim 11:976c80cc99d5 569 *green_ac_mag = green_ac_sig_max - green_ac_sig_min;
johnGreeneMaxim 11:976c80cc99d5 570 if ( *green_ac_mag>0 ) {
johnGreeneMaxim 11:976c80cc99d5 571 for (k=0; k<4 ; k++)
johnGreeneMaxim 11:976c80cc99d5 572 green_mac_FIFO[k]=green_mac_FIFO[k+1];
johnGreeneMaxim 11:976c80cc99d5 573 green_mac_FIFO[4] = *green_ac_mag ;
johnGreeneMaxim 11:976c80cc99d5 574 if ( green_mac_FIFO[4] > 1000)
johnGreeneMaxim 11:976c80cc99d5 575 green_mac_FIFO[4] =1000;
johnGreeneMaxim 11:976c80cc99d5 576 }
johnGreeneMaxim 11:976c80cc99d5 577 meanGreenMagFIFO=0;
johnGreeneMaxim 11:976c80cc99d5 578 meanGreenMagFIFOcounter=0;
johnGreeneMaxim 11:976c80cc99d5 579 for (k=0; k<5 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 580 if( green_mac_FIFO[k] >0) {
johnGreeneMaxim 11:976c80cc99d5 581 meanGreenMagFIFO= meanGreenMagFIFO +green_mac_FIFO[k] ;
johnGreeneMaxim 11:976c80cc99d5 582 meanGreenMagFIFOcounter++;
johnGreeneMaxim 11:976c80cc99d5 583 }
johnGreeneMaxim 11:976c80cc99d5 584 }
johnGreeneMaxim 11:976c80cc99d5 585 if (meanGreenMagFIFOcounter>=2 ) {
johnGreeneMaxim 11:976c80cc99d5 586 meanGreenMagFIFO =meanGreenMagFIFO/ meanGreenMagFIFOcounter ;
johnGreeneMaxim 11:976c80cc99d5 587 minAmpForHeartBeat= meanGreenMagFIFO /4 ; //25% of mean of past heart beat
johnGreeneMaxim 11:976c80cc99d5 588 } else
johnGreeneMaxim 11:976c80cc99d5 589 minAmpForHeartBeat = 75;
johnGreeneMaxim 11:976c80cc99d5 590 if (minAmpForHeartBeat <75)
johnGreeneMaxim 11:976c80cc99d5 591 minAmpForHeartBeat =75;
johnGreeneMaxim 11:976c80cc99d5 592 if (minAmpForHeartBeat >400)
johnGreeneMaxim 11:976c80cc99d5 593 minAmpForHeartBeat =400;
johnGreeneMaxim 11:976c80cc99d5 594
johnGreeneMaxim 11:976c80cc99d5 595 ir_pedge = 1;
johnGreeneMaxim 11:976c80cc99d5 596 ir_nedge = 0;
johnGreeneMaxim 11:976c80cc99d5 597 ir_ac_sig_max = 0;
johnGreeneMaxim 11:976c80cc99d5 598 ir_pzxip = ir_pzxic;
johnGreeneMaxim 11:976c80cc99d5 599 ir_pzxic = ns;
johnGreeneMaxim 11:976c80cc99d5 600 bufferIdx1= ir_pzxic- ir_nzxic;
johnGreeneMaxim 11:976c80cc99d5 601 bufferIdx2 = ir_pzxic -ir_pzxip;
johnGreeneMaxim 11:976c80cc99d5 602
johnGreeneMaxim 11:976c80cc99d5 603
johnGreeneMaxim 11:976c80cc99d5 604 if ((*green_ac_mag)> minAmpForHeartBeat && (*green_ac_mag)< 20000 && bufferIdx1>=0
johnGreeneMaxim 11:976c80cc99d5 605 && bufferIdx1 <100 && bufferIdx2>=0 && bufferIdx2 <100 && bufferIdx1< bufferIdx2 ) { // was <5000
johnGreeneMaxim 11:976c80cc99d5 606 maxFIFO = -32766;
johnGreeneMaxim 11:976c80cc99d5 607
johnGreeneMaxim 11:976c80cc99d5 608 IdxMaxFIFO = 0;
johnGreeneMaxim 11:976c80cc99d5 609 for ( j=bufferIdx1; j<= bufferIdx2; j++) { // find max peak
johnGreeneMaxim 11:976c80cc99d5 610 if (IrFIFO[j] > maxFIFO ) {
johnGreeneMaxim 11:976c80cc99d5 611 maxFIFO =IrFIFO[j];
johnGreeneMaxim 11:976c80cc99d5 612 IdxMaxFIFO =j;
johnGreeneMaxim 11:976c80cc99d5 613 }
johnGreeneMaxim 11:976c80cc99d5 614 }
johnGreeneMaxim 11:976c80cc99d5 615 peakLoc= ir_pzxic -IdxMaxFIFO+1 ;
johnGreeneMaxim 11:976c80cc99d5 616
johnGreeneMaxim 11:976c80cc99d5 617 if (prevPeakLoc !=0) {
johnGreeneMaxim 11:976c80cc99d5 618 HRperiod2 =( uint16_t) (peakLoc - prevPeakLoc);
johnGreeneMaxim 11:976c80cc99d5 619 if (HRperiod2>33 && HRperiod2 < 134) {
johnGreeneMaxim 11:976c80cc99d5 620 HRComp2= (6000/HRperiod2);
johnGreeneMaxim 11:976c80cc99d5 621 fingerOff =0 ;
johnGreeneMaxim 11:976c80cc99d5 622 } else
johnGreeneMaxim 11:976c80cc99d5 623 HRComp2=0 ;
johnGreeneMaxim 11:976c80cc99d5 624 } else
johnGreeneMaxim 11:976c80cc99d5 625 HRComp2 = 0 ;
johnGreeneMaxim 11:976c80cc99d5 626
johnGreeneMaxim 11:976c80cc99d5 627 if ( initHrQueCounter<10 && HRComp2 >0 ) {
johnGreeneMaxim 11:976c80cc99d5 628 HrQue[HrQindex] =HRComp2;
johnGreeneMaxim 11:976c80cc99d5 629 HrQindex++;
johnGreeneMaxim 11:976c80cc99d5 630 initHrQueCounter ++;
johnGreeneMaxim 11:976c80cc99d5 631 if (HrQindex== 10)
johnGreeneMaxim 11:976c80cc99d5 632 HrQindex =0;
johnGreeneMaxim 11:976c80cc99d5 633 }
johnGreeneMaxim 11:976c80cc99d5 634
johnGreeneMaxim 11:976c80cc99d5 635 if (initHrQueCounter > 7 && lengthOfposCountExceeding<=3) {
johnGreeneMaxim 11:976c80cc99d5 636 if ( HRComp2 > 0) {
johnGreeneMaxim 11:976c80cc99d5 637
johnGreeneMaxim 11:976c80cc99d5 638 HrCount =0;
johnGreeneMaxim 11:976c80cc99d5 639 HrSum =0;
johnGreeneMaxim 11:976c80cc99d5 640 zeros_in_HrQue=0;
johnGreeneMaxim 11:976c80cc99d5 641 for (k=1 ; k<initHrQueCounter ; k++) {
johnGreeneMaxim 11:976c80cc99d5 642 if (HrQue[k] >0) {
johnGreeneMaxim 11:976c80cc99d5 643 HrSum +=HrQue[k];
johnGreeneMaxim 11:976c80cc99d5 644 HrCount ++;
johnGreeneMaxim 11:976c80cc99d5 645 } else
johnGreeneMaxim 11:976c80cc99d5 646 zeros_in_HrQue ++;
johnGreeneMaxim 11:976c80cc99d5 647 }
johnGreeneMaxim 11:976c80cc99d5 648 meanHrQ = HrSum/HrCount ;
johnGreeneMaxim 11:976c80cc99d5 649 deltaHR= lastKnownGoodHr[0]/10;
johnGreeneMaxim 11:976c80cc99d5 650
johnGreeneMaxim 11:976c80cc99d5 651 if ( HRComp2 > lastKnownGoodHr[0] -deltaHR && HRComp2 <lastKnownGoodHr[0] +deltaHR ) {
johnGreeneMaxim 11:976c80cc99d5 652 for (k=1 ; k<10 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 653 HrQue[10 -k]= HrQue[9-k];
johnGreeneMaxim 11:976c80cc99d5 654 }
johnGreeneMaxim 11:976c80cc99d5 655 HrQue[0] =HRComp2;
johnGreeneMaxim 11:976c80cc99d5 656 } // HR smmothing using FIFO queue -
johnGreeneMaxim 11:976c80cc99d5 657
johnGreeneMaxim 11:976c80cc99d5 658 if (zeros_in_HrQue<=2) {
johnGreeneMaxim 11:976c80cc99d5 659 for (k=1 ; k<3 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 660 HrQueSmoothing[3 -k]= HrQueSmoothing[2-k];
johnGreeneMaxim 11:976c80cc99d5 661 }
johnGreeneMaxim 11:976c80cc99d5 662 HrQueSmoothing[0] = meanHrQ ;
johnGreeneMaxim 11:976c80cc99d5 663 HRComp2 = ( (HrQueSmoothing[0]<<2) + (HrQueSmoothing[1]<<1) + (HrQueSmoothing[2] <<1) ) >>3;
johnGreeneMaxim 11:976c80cc99d5 664 *HRbpm2 =HRComp2 ;
johnGreeneMaxim 11:976c80cc99d5 665
johnGreeneMaxim 11:976c80cc99d5 666 for (k=1 ; k<10 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 667 lastKnownGoodHr[10 -k]= lastKnownGoodHr[9-k];
johnGreeneMaxim 11:976c80cc99d5 668 }
johnGreeneMaxim 11:976c80cc99d5 669 lastKnownGoodHr[0] =HRComp2;
johnGreeneMaxim 11:976c80cc99d5 670 }
johnGreeneMaxim 11:976c80cc99d5 671 }
johnGreeneMaxim 11:976c80cc99d5 672
johnGreeneMaxim 11:976c80cc99d5 673 }
johnGreeneMaxim 11:976c80cc99d5 674 ///// if (initHrQueCounter > 7 && lengthOfposCountExceeding<5)
johnGreeneMaxim 11:976c80cc99d5 675 else if (initHrQueCounter < 7) { // before que is filled up, display whatever it got.
johnGreeneMaxim 11:976c80cc99d5 676 *HRbpm2 = HRComp2;
johnGreeneMaxim 11:976c80cc99d5 677
johnGreeneMaxim 11:976c80cc99d5 678 } else {
johnGreeneMaxim 11:976c80cc99d5 679 // *HRbpm2 = 0 ;
johnGreeneMaxim 11:976c80cc99d5 680 HrCount =0;
johnGreeneMaxim 11:976c80cc99d5 681 HrSum =0;
johnGreeneMaxim 11:976c80cc99d5 682 for (k=0 ; k<10 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 683 if (lastKnownGoodHr[k] >0) {
johnGreeneMaxim 11:976c80cc99d5 684 HrSum =HrSum + lastKnownGoodHr[k];
johnGreeneMaxim 11:976c80cc99d5 685 HrCount++;
johnGreeneMaxim 11:976c80cc99d5 686 }
johnGreeneMaxim 11:976c80cc99d5 687 }
johnGreeneMaxim 11:976c80cc99d5 688 if (HrCount>0)
johnGreeneMaxim 11:976c80cc99d5 689 *HRbpm2 = HrSum/HrCount;
johnGreeneMaxim 11:976c80cc99d5 690 else
johnGreeneMaxim 11:976c80cc99d5 691 *HRbpm2 = 0;
johnGreeneMaxim 11:976c80cc99d5 692
johnGreeneMaxim 11:976c80cc99d5 693
johnGreeneMaxim 11:976c80cc99d5 694
johnGreeneMaxim 11:976c80cc99d5 695 }
johnGreeneMaxim 11:976c80cc99d5 696 prevPeakLoc = peakLoc ; // save peakLoc into Static var
johnGreeneMaxim 11:976c80cc99d5 697
johnGreeneMaxim 11:976c80cc99d5 698
johnGreeneMaxim 11:976c80cc99d5 699 if (compSpO2) {
johnGreeneMaxim 11:976c80cc99d5 700 rnum = (ir_avg_reg >> 20)*(*red_ac_mag);
johnGreeneMaxim 11:976c80cc99d5 701 rden = (red_avg_reg >> 20)*(*ir_ac_mag);
johnGreeneMaxim 11:976c80cc99d5 702 rdens = (rden>>15);
johnGreeneMaxim 11:976c80cc99d5 703 if (rdens>0) cSpO2 = 110- (((25*rnum)/(rdens))>>15);
johnGreeneMaxim 11:976c80cc99d5 704
johnGreeneMaxim 11:976c80cc99d5 705 if (cSpO2 >=100) SpO2 = 100;
johnGreeneMaxim 11:976c80cc99d5 706 else if (cSpO2 <= 70) SpO2 = 70;
johnGreeneMaxim 11:976c80cc99d5 707 else SpO2 = cSpO2;
johnGreeneMaxim 11:976c80cc99d5 708
johnGreeneMaxim 11:976c80cc99d5 709 SPO2Que[SPO2QueCounter ] = SpO2;
johnGreeneMaxim 11:976c80cc99d5 710
johnGreeneMaxim 11:976c80cc99d5 711 for (k=0 ; k<5 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 712 SPO2score[k] =0;
johnGreeneMaxim 11:976c80cc99d5 713 for (j=0 ; j< 5 ; j++)
johnGreeneMaxim 11:976c80cc99d5 714 if( abs( SPO2Que[k] - SPO2Que[j] )>5)
johnGreeneMaxim 11:976c80cc99d5 715 SPO2score[k] ++;
johnGreeneMaxim 11:976c80cc99d5 716 }
johnGreeneMaxim 11:976c80cc99d5 717
johnGreeneMaxim 11:976c80cc99d5 718
johnGreeneMaxim 11:976c80cc99d5 719 SPO2scoreSum= 0;
johnGreeneMaxim 11:976c80cc99d5 720 for (k=0 ; k<5 ; k++)
johnGreeneMaxim 11:976c80cc99d5 721 SPO2scoreSum += SPO2score[k] ;
johnGreeneMaxim 11:976c80cc99d5 722 SPO2scoreAverage= SPO2scoreSum / 5;
johnGreeneMaxim 11:976c80cc99d5 723 for (k=1 ; k<5 ; k++)
johnGreeneMaxim 11:976c80cc99d5 724 SPO2score[k] = SPO2score[k] -SPO2scoreAverage;
johnGreeneMaxim 11:976c80cc99d5 725
johnGreeneMaxim 11:976c80cc99d5 726 validSPO2Count =0;
johnGreeneMaxim 11:976c80cc99d5 727 validSPO2Sum =0;
johnGreeneMaxim 11:976c80cc99d5 728 for (k=1 ; k<5 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 729 if (SPO2score[k]<=0 ) { // add for HR to report
johnGreeneMaxim 11:976c80cc99d5 730 validSPO2Sum +=SPO2Que[k];
johnGreeneMaxim 11:976c80cc99d5 731 validSPO2Count ++;
johnGreeneMaxim 11:976c80cc99d5 732 }
johnGreeneMaxim 11:976c80cc99d5 733 }
johnGreeneMaxim 11:976c80cc99d5 734 if ( validSPO2Count>0)
johnGreeneMaxim 11:976c80cc99d5 735 SPO2D = (validSPO2Sum /validSPO2Count)-1;
johnGreeneMaxim 11:976c80cc99d5 736 if ( SPO2D >100)
johnGreeneMaxim 11:976c80cc99d5 737 SPO2D = 100;
johnGreeneMaxim 11:976c80cc99d5 738
johnGreeneMaxim 11:976c80cc99d5 739 SPO2QueCounter ++;
johnGreeneMaxim 11:976c80cc99d5 740 if (SPO2QueCounter ==5) SPO2QueCounter = 0;
johnGreeneMaxim 11:976c80cc99d5 741
johnGreeneMaxim 11:976c80cc99d5 742 for (k=1 ; k<3 ; k++) {
johnGreeneMaxim 11:976c80cc99d5 743 SPO2QueSmoothing[3 -k]= SPO2QueSmoothing[2-k];
johnGreeneMaxim 11:976c80cc99d5 744 }
johnGreeneMaxim 11:976c80cc99d5 745 SPO2QueSmoothing[0] = SPO2D;
johnGreeneMaxim 11:976c80cc99d5 746 *SpO2B = ( (SPO2QueSmoothing[0]<<2) + (SPO2QueSmoothing[1]<<1) + (SPO2QueSmoothing[2] <<1) ) >>3;
johnGreeneMaxim 11:976c80cc99d5 747
johnGreeneMaxim 11:976c80cc99d5 748 if (*SpO2B> 100) *SpO2B = 100 ;
johnGreeneMaxim 11:976c80cc99d5 749
johnGreeneMaxim 11:976c80cc99d5 750 } else {
johnGreeneMaxim 11:976c80cc99d5 751 SpO2 = 0;
johnGreeneMaxim 11:976c80cc99d5 752 *SpO2B = 0;
johnGreeneMaxim 11:976c80cc99d5 753 }
johnGreeneMaxim 11:976c80cc99d5 754 *DRdy = 1;
johnGreeneMaxim 11:976c80cc99d5 755
johnGreeneMaxim 11:976c80cc99d5 756 }
johnGreeneMaxim 11:976c80cc99d5 757 }
johnGreeneMaxim 11:976c80cc99d5 758 // Detect IR channel negative zero crossing (falling edge)
johnGreeneMaxim 11:976c80cc99d5 759 if ((green_ac_sig_pre > 0) && (green_ac_sig_cur <= 0)) {
johnGreeneMaxim 11:976c80cc99d5 760 ir_pedge = 0;
johnGreeneMaxim 11:976c80cc99d5 761 ir_nedge = 1;
johnGreeneMaxim 11:976c80cc99d5 762 ir_ac_sig_min = 0;
johnGreeneMaxim 11:976c80cc99d5 763 ir_nzxic = ns;
johnGreeneMaxim 11:976c80cc99d5 764 }
johnGreeneMaxim 11:976c80cc99d5 765 // Find Maximum IR & Red values in positive cycle
johnGreeneMaxim 11:976c80cc99d5 766 if (ir_pedge && (green_ac_sig_cur > green_ac_sig_pre)) {
johnGreeneMaxim 11:976c80cc99d5 767 ir_ac_sig_max = ir_ac_sig_cur;
johnGreeneMaxim 11:976c80cc99d5 768 red_ac_sig_max = red_ac_sig_cur;
johnGreeneMaxim 11:976c80cc99d5 769 green_ac_sig_max = green_ac_sig_cur;
johnGreeneMaxim 11:976c80cc99d5 770 }
johnGreeneMaxim 11:976c80cc99d5 771
johnGreeneMaxim 11:976c80cc99d5 772 // Find minimum IR & Red values in negative cycle
johnGreeneMaxim 11:976c80cc99d5 773 if (ir_nedge && (green_ac_sig_cur < green_ac_sig_pre)) {
johnGreeneMaxim 11:976c80cc99d5 774 ir_ac_sig_min = ir_ac_sig_cur;
johnGreeneMaxim 11:976c80cc99d5 775 red_ac_sig_min = red_ac_sig_cur;
johnGreeneMaxim 11:976c80cc99d5 776 green_ac_sig_min = green_ac_sig_cur;
johnGreeneMaxim 11:976c80cc99d5 777 }
johnGreeneMaxim 11:976c80cc99d5 778 if (IRData <50000 )
johnGreeneMaxim 11:976c80cc99d5 779 // finger-off
johnGreeneMaxim 11:976c80cc99d5 780 {
johnGreeneMaxim 11:976c80cc99d5 781 fingerOff++;
johnGreeneMaxim 11:976c80cc99d5 782 *DRdy = 0;
johnGreeneMaxim 11:976c80cc99d5 783 } else
johnGreeneMaxim 11:976c80cc99d5 784 fingerOff = 0 ;
johnGreeneMaxim 11:976c80cc99d5 785 /*if (IRData <50000 && fingerOff>10 )
johnGreeneMaxim 11:976c80cc99d5 786 fingerOff = 0; */
johnGreeneMaxim 11:976c80cc99d5 787 if ( *SpO2B == 0 || *HRbpm2 ==0)
johnGreeneMaxim 11:976c80cc99d5 788 *DRdy = 0;
johnGreeneMaxim 11:976c80cc99d5 789
johnGreeneMaxim 11:976c80cc99d5 790 /*if (ns > 2000 )
johnGreeneMaxim 11:976c80cc99d5 791 {
johnGreeneMaxim 11:976c80cc99d5 792 if (abs(lastDisplayedHrValue - *HRbpm2)>5)
johnGreeneMaxim 11:976c80cc99d5 793 *HRbpm2 =lastDisplayedHrValue ;
johnGreeneMaxim 11:976c80cc99d5 794 else
johnGreeneMaxim 11:976c80cc99d5 795 lastDisplayedHrValue = *HRbpm2;
johnGreeneMaxim 11:976c80cc99d5 796 }*/
johnGreeneMaxim 11:976c80cc99d5 797 // *DRdy = minAmpForHeartBeat;
johnGreeneMaxim 11:976c80cc99d5 798
johnGreeneMaxim 11:976c80cc99d5 799 }
johnGreeneMaxim 11:976c80cc99d5 800
johnGreeneMaxim 11:976c80cc99d5 801 // Average DC Estimator
johnGreeneMaxim 11:976c80cc99d5 802 uint16_t avg_dc_est(int32_t *p, uint16_t x)
johnGreeneMaxim 11:976c80cc99d5 803 {
johnGreeneMaxim 11:976c80cc99d5 804 *p += ((((int32_t) x << 15) - *p)>>4);
johnGreeneMaxim 11:976c80cc99d5 805 return (*p >> 15);
johnGreeneMaxim 11:976c80cc99d5 806 }
johnGreeneMaxim 11:976c80cc99d5 807
johnGreeneMaxim 11:976c80cc99d5 808 // Symmetric Dual Low Pass FIR Filter
johnGreeneMaxim 11:976c80cc99d5 809 void lp_dfir_flt(int16_t din0,int16_t din1,int16_t din2, int16_t *dout0,int16_t *dout1,int16_t *dout2)
johnGreeneMaxim 11:976c80cc99d5 810 {
johnGreeneMaxim 11:976c80cc99d5 811 static const uint16_t FIRCoeffs[12] = {688,1283,2316,3709,5439,7431,
johnGreeneMaxim 11:976c80cc99d5 812 9561,11666,13563,15074,16047,16384
johnGreeneMaxim 11:976c80cc99d5 813 };
johnGreeneMaxim 11:976c80cc99d5 814
johnGreeneMaxim 11:976c80cc99d5 815 static int16_t cbuf0[32],cbuf1[32] , cbuf2[32];
johnGreeneMaxim 11:976c80cc99d5 816 static int16_t offset = 0;
johnGreeneMaxim 11:976c80cc99d5 817 int32_t y0,y1, y2;
johnGreeneMaxim 11:976c80cc99d5 818 int16_t i;
johnGreeneMaxim 11:976c80cc99d5 819
johnGreeneMaxim 11:976c80cc99d5 820 cbuf0[offset] = din0;
johnGreeneMaxim 11:976c80cc99d5 821 cbuf1[offset] = din1;
johnGreeneMaxim 11:976c80cc99d5 822 cbuf2[offset] = din2;
johnGreeneMaxim 11:976c80cc99d5 823
johnGreeneMaxim 11:976c80cc99d5 824 y0 = mul16(FIRCoeffs[11], cbuf0[(offset - 11)&0x1F]);
johnGreeneMaxim 11:976c80cc99d5 825 y1 = mul16(FIRCoeffs[11], cbuf1[(offset - 11)&0x1F]);
johnGreeneMaxim 11:976c80cc99d5 826 y2 = mul16(FIRCoeffs[11], cbuf2[(offset - 11)&0x1F]);
johnGreeneMaxim 11:976c80cc99d5 827
johnGreeneMaxim 11:976c80cc99d5 828
johnGreeneMaxim 11:976c80cc99d5 829 for (i=0; i<11; i++) {
johnGreeneMaxim 11:976c80cc99d5 830 y0 += mul16(FIRCoeffs[i], cbuf0[(offset-i)&0x1F] + cbuf0[(offset-22+i)&0x1F]);
johnGreeneMaxim 11:976c80cc99d5 831 y1 += mul16(FIRCoeffs[i], cbuf1[(offset-i)&0x1F] + cbuf1[(offset-22+i)&0x1F]);
johnGreeneMaxim 11:976c80cc99d5 832 y2 += mul16(FIRCoeffs[i], cbuf2[(offset-i)&0x1F] + cbuf2[(offset-22+i)&0x1F]);
johnGreeneMaxim 11:976c80cc99d5 833 }
johnGreeneMaxim 11:976c80cc99d5 834 offset = (offset + 1)&0x1F;
johnGreeneMaxim 11:976c80cc99d5 835
johnGreeneMaxim 11:976c80cc99d5 836 *dout0 = (y0>>15);
johnGreeneMaxim 11:976c80cc99d5 837 *dout1 = (y1>>15);
johnGreeneMaxim 11:976c80cc99d5 838 *dout2 = (y2>>15);
johnGreeneMaxim 11:976c80cc99d5 839 }
johnGreeneMaxim 11:976c80cc99d5 840
johnGreeneMaxim 11:976c80cc99d5 841 // Integer multiplier
johnGreeneMaxim 11:976c80cc99d5 842 int32_t mul16(int16_t x, int16_t y)
johnGreeneMaxim 11:976c80cc99d5 843 {
johnGreeneMaxim 11:976c80cc99d5 844 return (int32_t)(x* y);
johnGreeneMaxim 8:a1538e8a3fd9 845 }