John Scharf / Mbed 2 deprecated Hat_Board_v5

accel now workin

Dependencies:   mbed-dsp mbed

Fork of Hat_Board_Test by Jim Cooke

main.cpp@4:e9df42113893, 2014-04-03 (annotated)

Committer:
jjes144
Date:
Thu Apr 03 07:31:24 2014 +0000
Revision:
4:e9df42113893
Parent:
3:8334f137c151 
accel now workin

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jjes144 4:e9df42113893 1 //Hat Board v5 by Cooke Scharf 4/2/2014
drnow 0:34bad5aca893 2
drnow 0:34bad5aca893 3 #include "mbed.h"
drnow 0:34bad5aca893 4 #include "SI_LIS.h"
drnow 0:34bad5aca893 5 #include <time.h>
drnow 0:34bad5aca893 6
jjes144 4:e9df42113893 7 #include "arm_math.h"
jjes144 4:e9df42113893 8 #include "math_helper.h"
jjes144 4:e9df42113893 9 #include "dsp.h"
jjes144 4:e9df42113893 10 #include "arm_const_structs.h"
jjes144 4:e9df42113893 11
drnow 0:34bad5aca893 12 DigitalOut myled3(LED3);
drnow 0:34bad5aca893 13 DigitalOut myled4(LED4);
drnow 0:34bad5aca893 14 Serial pc(USBTX,USBRX);
drnow 0:34bad5aca893 15 DigitalIn int_pin(p8);
drnow 0:34bad5aca893 16 Timer t;
drnow 0:34bad5aca893 17
drnow 0:34bad5aca893 18 int reading_IR,reading_660,LSB,MSB;
drnow 0:34bad5aca893 19 char rx_data[4];
drnow 1:2efeed26d93a 20 char accel_data[6];
drnow 0:34bad5aca893 21 char temp_val;
drnow 3:8334f137c151 22 short int dataX; // short int: 16 bits. This allows easy negative results
drnow 3:8334f137c151 23 short int dataY; // short int: 16 bits. This allows easy negative results
drnow 3:8334f137c151 24 short int dataZ; // short int: 16 bits. This allows easy negative results
drnow 2:3a8cd127b72a 25 float t_msec;
drnow 3:8334f137c151 26 float dataX_fl;
drnow 3:8334f137c151 27 float dataY_fl;
drnow 3:8334f137c151 28 float dataZ_fl;
jjes144 4:e9df42113893 29 //float accel_fl;
jjes144 4:e9df42113893 30
jjes144 4:e9df42113893 31 ////////////////////////////////////////////
jjes144 4:e9df42113893 32 ///////////////////////////////////////////
jjes144 4:e9df42113893 33 #define MAX_BLOCKSIZE 4096
jjes144 4:e9df42113893 34 uint32_t fftsize = MAX_BLOCKSIZE/2;
jjes144 4:e9df42113893 35 const static arm_cfft_instance_f32 *S;
jjes144 4:e9df42113893 36
jjes144 4:e9df42113893 37 ///////////////////////////////////////////
jjes144 4:e9df42113893 38 long int BAUD_RATE = 230400;
jjes144 4:e9df42113893 39 void baud(int baudrate)
jjes144 4:e9df42113893 40 {
jjes144 4:e9df42113893 41 Serial s(USBTX,USBRX);
jjes144 4:e9df42113893 42 s.baud(baudrate);
jjes144 4:e9df42113893 43 }
jjes144 4:e9df42113893 44
jjes144 4:e9df42113893 45 ///////////////////////////////////////////
jjes144 4:e9df42113893 46 unsigned short i,j,k, m;
jjes144 4:e9df42113893 47 float32_t temp;
jjes144 4:e9df42113893 48 float32_t temp2;
jjes144 4:e9df42113893 49 float sclr;
jjes144 4:e9df42113893 50
jjes144 4:e9df42113893 51 float32_t v[MAX_BLOCKSIZE], w[MAX_BLOCKSIZE], acc[MAX_BLOCKSIZE];
jjes144 4:e9df42113893 52 float32_t testoutput[MAX_BLOCKSIZE/2];
jjes144 4:e9df42113893 53 float32_t testoutputb[26], testoutputc[26], outputacc[26];
jjes144 4:e9df42113893 54
jjes144 4:e9df42113893 55 ////////////////////////////////////////////
drnow 0:34bad5aca893 56
drnow 0:34bad5aca893 57 int main()
drnow 0:34bad5aca893 58 {
jjes144 4:e9df42113893 59 myled4 = 0;
drnow 3:8334f137c151 60 pc.baud(460800);
drnow 2:3a8cd127b72a 61 Init_Accel(); // starts LIS3DH
drnow 2:3a8cd127b72a 62 restart(); // starts Si1142
drnow 2:3a8cd127b72a 63 wait_ms(30);
drnow 2:3a8cd127b72a 64 command (PS_AUTO); //start measuring
drnow 2:3a8cd127b72a 65 wait (0.5);
drnow 0:34bad5aca893 66
jjes144 4:e9df42113893 67 j = 0;
jjes144 4:e9df42113893 68
drnow 2:3a8cd127b72a 69 while(1) {
drnow 2:3a8cd127b72a 70 if(!int_pin) {
jjes144 4:e9df42113893 71 //t.reset();
jjes144 4:e9df42113893 72 //t.start();
drnow 1:2efeed26d93a 73
drnow 2:3a8cd127b72a 74 write_reg(IRQ_STATUS,0x04); // clear the interrupt.
drnow 1:2efeed26d93a 75
drnow 2:3a8cd127b72a 76 read_reg2(PS1_DATA0);
drnow 3:8334f137c151 77 reading_IR = rx_data[1] << 8 | rx_data[0];
drnow 3:8334f137c151 78 reading_660 = rx_data[3] << 8 | rx_data[1];
drnow 1:2efeed26d93a 79
drnow 2:3a8cd127b72a 80 Get_Accel_Reg_6 (0x28);
drnow 0:34bad5aca893 81
drnow 3:8334f137c151 82 dataX = accel_data[1] << 8 | accel_data[0];
jjes144 4:e9df42113893 83 dataY = accel_data[3] << 8 | accel_data[2];
jjes144 4:e9df42113893 84 dataZ = accel_data[5] << 8 | accel_data[4];
jjes144 4:e9df42113893 85
jjes144 4:e9df42113893 86 k = (j/2)*2;
jjes144 4:e9df42113893 87 if (j==k) {
jjes144 4:e9df42113893 88 v[j] = (float32_t)reading_IR;
jjes144 4:e9df42113893 89 v[j+1] = 0;
jjes144 4:e9df42113893 90 w[j] = (float32_t)reading_660;
jjes144 4:e9df42113893 91 w[j+1] = 0;
jjes144 4:e9df42113893 92
drnow 3:8334f137c151 93 dataX_fl = (float) dataX;
drnow 3:8334f137c151 94 dataY_fl = (float) dataY;
drnow 3:8334f137c151 95 dataZ_fl = (float) dataZ;
jjes144 4:e9df42113893 96
jjes144 4:e9df42113893 97 acc[j] = sqrt( (dataX_fl*dataX_fl) + (dataY_fl*dataY_fl) + (dataZ_fl*dataZ_fl) );
jjes144 4:e9df42113893 98 acc[j+1] = 0;
jjes144 4:e9df42113893 99
jjes144 4:e9df42113893 100 }
jjes144 4:e9df42113893 101 pc.printf ("%4d %5d %5d %5d %5d %5d\n\r", j, reading_IR, reading_660, dataX, dataY, dataZ);
jjes144 4:e9df42113893 102
jjes144 4:e9df42113893 103 j++;
jjes144 4:e9df42113893 104
jjes144 4:e9df42113893 105 if(j==MAX_BLOCKSIZE)
jjes144 4:e9df42113893 106 {
jjes144 4:e9df42113893 107
jjes144 4:e9df42113893 108 switch (fftsize) {
jjes144 4:e9df42113893 109 case 64:
jjes144 4:e9df42113893 110 S = & arm_cfft_sR_f32_len64;
jjes144 4:e9df42113893 111 break;
jjes144 4:e9df42113893 112 case 128:
jjes144 4:e9df42113893 113 S = & arm_cfft_sR_f32_len128;
jjes144 4:e9df42113893 114 break;
jjes144 4:e9df42113893 115 case 256:
jjes144 4:e9df42113893 116 S = & arm_cfft_sR_f32_len256;
jjes144 4:e9df42113893 117 break;
jjes144 4:e9df42113893 118 case 512:
jjes144 4:e9df42113893 119 S = & arm_cfft_sR_f32_len512;
jjes144 4:e9df42113893 120 break;
jjes144 4:e9df42113893 121 case 1024:
jjes144 4:e9df42113893 122 S = & arm_cfft_sR_f32_len1024;
jjes144 4:e9df42113893 123 break;
jjes144 4:e9df42113893 124 case 2048:
jjes144 4:e9df42113893 125 S = & arm_cfft_sR_f32_len2048;
jjes144 4:e9df42113893 126 break;
jjes144 4:e9df42113893 127 case 4096:
jjes144 4:e9df42113893 128 S = & arm_cfft_sR_f32_len4096;
jjes144 4:e9df42113893 129 break;
jjes144 4:e9df42113893 130 }
drnow 0:34bad5aca893 131
jjes144 4:e9df42113893 132 /////////////////////////////////////////////////////////////////////
jjes144 4:e9df42113893 133
jjes144 4:e9df42113893 134 /////////////////////////////////////////////////////////////////////
jjes144 4:e9df42113893 135 float32_t maxvaluea, maxvalueb, maxacc, rvalue, spo2;
jjes144 4:e9df42113893 136 uint32_t testindexa, testindexb, indexacc;
jjes144 4:e9df42113893 137
jjes144 4:e9df42113893 138 for (i = 0; i < 26; i++) {
jjes144 4:e9df42113893 139 pc.printf ("\n\r%4d %10.2f %10.2f %10.2f\n\r", i, v[i], w[i], acc[i]);
jjes144 4:e9df42113893 140 }
jjes144 4:e9df42113893 141
jjes144 4:e9df42113893 142 /////////////////////////////////////////////////////////////////////
jjes144 4:e9df42113893 143 arm_cfft_f32(S, v, 0, 1);
jjes144 4:e9df42113893 144 arm_cmplx_mag_squared_f32(v, testoutput, fftsize);
jjes144 4:e9df42113893 145 sclr = 1000000/testoutput[0];
jjes144 4:e9df42113893 146 arm_scale_f32(testoutput, sclr, testoutput, fftsize);
jjes144 4:e9df42113893 147
jjes144 4:e9df42113893 148 for (i = 0; i < 26; i++) {
jjes144 4:e9df42113893 149 testoutputb[i] = testoutput[i];
jjes144 4:e9df42113893 150 if (i<4) {
jjes144 4:e9df42113893 151 testoutputb[i] = 0;
jjes144 4:e9df42113893 152 }
jjes144 4:e9df42113893 153 pc.printf ("\n\r%4d %10.2f\n\r", i, testoutputb[i]);
jjes144 4:e9df42113893 154 }
jjes144 4:e9df42113893 155 // 805 nm
jjes144 4:e9df42113893 156 arm_max_f32(testoutputb, 26, &maxvalueb, &testindexb); // 720 hz sampling / 4096 fft = 0.1758 hz bin spacing = 360 hz sampling / 2048 fft
jjes144 4:e9df42113893 157 // 1 beat-sec / 0.1758 = about 6 bins (HR = 60 bpm)
jjes144 4:e9df42113893 158 // 3.6667 beats-sec / 0.1758 hz bin spacing = about 21 bins (HR = 220 bpm)
jjes144 4:e9df42113893 159 pc.printf ("\n\r\n\r%4d %10.3f\n\r\n\r", testindexb, maxvalueb);
jjes144 4:e9df42113893 160
jjes144 4:e9df42113893 161 /////////////////////////////////////////////////////////////////////
jjes144 4:e9df42113893 162 arm_cfft_f32(S, w, 0, 1);
jjes144 4:e9df42113893 163 arm_cmplx_mag_squared_f32(w, testoutput, fftsize);
jjes144 4:e9df42113893 164 sclr = 1000000/testoutput[0];
jjes144 4:e9df42113893 165 arm_scale_f32(testoutput, sclr, testoutput, fftsize);
jjes144 4:e9df42113893 166
jjes144 4:e9df42113893 167 for (i = 0; i < 26; i++) {
jjes144 4:e9df42113893 168 testoutputc[i] = testoutput[i];
jjes144 4:e9df42113893 169 if (i<4) {
jjes144 4:e9df42113893 170 testoutputc[i] = 0;
jjes144 4:e9df42113893 171 }
jjes144 4:e9df42113893 172 }
jjes144 4:e9df42113893 173 // 660 nm
jjes144 4:e9df42113893 174 arm_max_f32(testoutputc, 26, &maxvaluea, &testindexa); // 720 hz sampling / 4096 fft = 0.1758 hz bin spacing = 360 hz sampling / 2048 fft
jjes144 4:e9df42113893 175 // 1 beat-sec / 0.1758 = about 6 bins (HR = 60 bpm)
jjes144 4:e9df42113893 176 // 3.6667 beats-sec / 0.1758 hz bin spacing = about 21 bins (HR = 220 bpm)
jjes144 4:e9df42113893 177 pc.printf ("\n\r\n\r%4d %10.3f\n\r\n\r", testindexa, maxvaluea);
jjes144 4:e9df42113893 178
jjes144 4:e9df42113893 179 rvalue = maxvaluea/maxvalueb;
jjes144 4:e9df42113893 180 spo2 = -22.6 * rvalue + 108;
jjes144 4:e9df42113893 181
jjes144 4:e9df42113893 182 pc.printf ("\n\r\n\r%5.2f %5.2f\n\r\n\r", rvalue, spo2);
jjes144 4:e9df42113893 183
jjes144 4:e9df42113893 184 /////////////////////////////////////////////////////////////////////
jjes144 4:e9df42113893 185 arm_cfft_f32(S, acc, 0, 1);
jjes144 4:e9df42113893 186 arm_cmplx_mag_squared_f32(acc, testoutput, fftsize);
jjes144 4:e9df42113893 187 sclr = 1000000/testoutput[0];
jjes144 4:e9df42113893 188 arm_scale_f32(testoutput, sclr, testoutput, fftsize);
jjes144 4:e9df42113893 189
jjes144 4:e9df42113893 190 for (i = 0; i < 26; i++) {
jjes144 4:e9df42113893 191 outputacc[i] = testoutput[i];
jjes144 4:e9df42113893 192 if (i<4) {
jjes144 4:e9df42113893 193 outputacc[i] = 0;
jjes144 4:e9df42113893 194 }
jjes144 4:e9df42113893 195 pc.printf ("\n\r%4d %10.2f\n\r", i, outputacc[i]);
jjes144 4:e9df42113893 196 }
jjes144 4:e9df42113893 197 // accelerometer
jjes144 4:e9df42113893 198 arm_max_f32(outputacc, 26, &maxacc, &indexacc); // 720 hz sampling / 4096 fft = 0.1758 hz bin spacing = 360 hz sampling / 2048 fft
jjes144 4:e9df42113893 199 // 1 beat-sec / 0.1758 = about 6 bins (HR = 60 bpm)
jjes144 4:e9df42113893 200 // 3.6667 beats-sec / 0.1758 hz bin spacing = about 21 bins (HR = 220 bpm)
jjes144 4:e9df42113893 201 pc.printf ("\n\r\n\r%4d %10.3f\n\r\n\r", indexacc, maxacc);
jjes144 4:e9df42113893 202
jjes144 4:e9df42113893 203 /////////////////////////////////////////////////////////////////////
jjes144 4:e9df42113893 204
jjes144 4:e9df42113893 205 j = 0;
jjes144 4:e9df42113893 206
jjes144 4:e9df42113893 207 /////////////////////////////////////////////////////////////////////
jjes144 4:e9df42113893 208
jjes144 4:e9df42113893 209 }
jjes144 4:e9df42113893 210
jjes144 4:e9df42113893 211 //----------------------------------------------------------
drnow 2:3a8cd127b72a 212 }
drnow 2:3a8cd127b72a 213 }
drnow 0:34bad5aca893 214 }
drnow 0:34bad5aca893 215