Zhihan Zhang / Mbed 2 deprecated 4180_FinalProject

ECE 4180 Final Project

Dependencies:   mbed PulseSensor mbed-rtos LSM9DS1_Library_cal

main.cpp@9:dcbd546412ea, 2021-12-02 (annotated)

Committer:
zhihanzhang
Date:
Thu Dec 02 18:46:01 2021 +0000
Revision:
9:dcbd546412ea
Parent:
8:2d43385e7784
Child:
10:63d223104806
12/2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedAustin 0:59bec1fd956e 1 #include "mbed.h"
yutation 7:88d71c228407 2 #include "rtos.h"
yutation 7:88d71c228407 3 #include "LSM9DS1.h"
yutation 7:88d71c228407 4 #include "stepcounter.h"
yutation 8:2d43385e7784 5 #include "PulseSensor.h"
yutation 7:88d71c228407 6 #define PI 3.14159
yutation 7:88d71c228407 7 // Earth's magnetic field varies by location. Add or subtract
yutation 7:88d71c228407 8 // a declination to get a more accurate heading. Calculate
yutation 7:88d71c228407 9 // your's here:
yutation 7:88d71c228407 10 // http://www.ngdc.noaa.gov/geomag-web/#declination
zhihanzhang 9:dcbd546412ea 11 #define DECLINATION -4.94 // Declination (degrees) in Atlanta, GA.
yutation 7:88d71c228407 12 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
yutation 7:88d71c228407 13 Timer timer;
yutation 7:88d71c228407 14
yutation 8:2d43385e7784 15 /*
zhihanzhang 9:dcbd546412ea 16 Skin temp
zhihanzhang 9:dcbd546412ea 17 Heart beat
zhihanzhang 9:dcbd546412ea 18 Human resistance
zhihanzhang 9:dcbd546412ea 19 Step count
yutation 8:2d43385e7784 20 */
yutation 8:2d43385e7784 21
yutation 8:2d43385e7784 22 // IMU
yutation 7:88d71c228407 23 filter_avg_t acc_data;
yutation 7:88d71c228407 24 axis_info_t acc_sample;
yutation 7:88d71c228407 25 peak_value_t acc_peak;
yutation 7:88d71c228407 26 slid_reg_t acc_slid;
yutation 8:2d43385e7784 27
yutation 8:2d43385e7784 28 // skin temp
yutation 7:88d71c228407 29 AnalogIn skinTemp(p19);
zhihanzhang 9:dcbd546412ea 30 uint8_t temp_skin = 25;
mbedAustin 2:a8dcb07a1d00 31
zhihanzhang 9:dcbd546412ea 32 //Heart beat
zhihanzhang 9:dcbd546412ea 33 uint8_t bpm;
yutation 8:2d43385e7784 34
zhihanzhang 9:dcbd546412ea 35 //Human resistance
yutation 8:2d43385e7784 36 AnalogIn gsr(p17);
yutation 8:2d43385e7784 37 AnalogIn sig(p18);
yutation 8:2d43385e7784 38
zhihanzhang 9:dcbd546412ea 39 uint8_t Human_Resistance = 128;
yutation 8:2d43385e7784 40 float gsrValue = 0;
yutation 8:2d43385e7784 41 float phasic = 0;
yutation 8:2d43385e7784 42 float baseline = 0;
yutation 8:2d43385e7784 43 int on = 1, off = 0;
yutation 8:2d43385e7784 44
yutation 8:2d43385e7784 45 Serial pc(USBTX, USBRX);
yutation 7:88d71c228407 46 RawSerial dev(p9,p10);
sam_grove 5:96cb82af9996 47 DigitalOut led1(LED1);
sam_grove 5:96cb82af9996 48 DigitalOut led4(LED4);
mbedAustin 2:a8dcb07a1d00 49
yutation 7:88d71c228407 50 struct gyro {
yutation 7:88d71c228407 51 float x;
yutation 7:88d71c228407 52 float y;
yutation 7:88d71c228407 53 float z;
yutation 7:88d71c228407 54 };
yutation 7:88d71c228407 55
yutation 7:88d71c228407 56 struct acce {
yutation 7:88d71c228407 57 float x;
yutation 7:88d71c228407 58 float y;
yutation 7:88d71c228407 59 float z;
yutation 7:88d71c228407 60 };
yutation 7:88d71c228407 61
yutation 7:88d71c228407 62 char imu_data[24];
yutation 7:88d71c228407 63
zhihanzhang 9:dcbd546412ea 64 void sendDataToProcessing(int data)
yutation 8:2d43385e7784 65 {
zhihanzhang 9:dcbd546412ea 66 pc.printf("%d\r\n", data);
zhihanzhang 9:dcbd546412ea 67 bpm = data;
yutation 7:88d71c228407 68 }
yutation 7:88d71c228407 69
yutation 7:88d71c228407 70
yutation 7:88d71c228407 71 void store_imu_data(float d, char* sp) {
yutation 7:88d71c228407 72 int id = *(int*)&d;
yutation 7:88d71c228407 73 for(int i = 0; i < 4; i++) {
yutation 7:88d71c228407 74 *(sp+i) = id % 0xff;
yutation 7:88d71c228407 75 id = id >> 8;
yutation 7:88d71c228407 76 }
yutation 7:88d71c228407 77 }
yutation 7:88d71c228407 78
yutation 7:88d71c228407 79 void update_IMU_data(void const *arg){
yutation 7:88d71c228407 80 struct gyro gy;
yutation 7:88d71c228407 81 struct acce ac;
yutation 7:88d71c228407 82 while(1){
yutation 7:88d71c228407 83 while(!IMU.accelAvailable());
yutation 7:88d71c228407 84 IMU.readAccel();
yutation 7:88d71c228407 85 while(!IMU.gyroAvailable());
yutation 7:88d71c228407 86 IMU.readGyro();
yutation 7:88d71c228407 87
yutation 7:88d71c228407 88 gy.x = IMU.calcGyro(IMU.gx);
yutation 7:88d71c228407 89 gy.y = IMU.calcGyro(IMU.gy);
yutation 7:88d71c228407 90 gy.z = IMU.calcGyro(IMU.gz);
yutation 7:88d71c228407 91 ac.x = IMU.calcAccel(IMU.ax);
yutation 7:88d71c228407 92 ac.y = IMU.calcAccel(IMU.ay);
yutation 7:88d71c228407 93 ac.z = IMU.calcAccel(IMU.az);
yutation 7:88d71c228407 94
yutation 7:88d71c228407 95 store_imu_data(gy.x, &imu_data[0]);
yutation 7:88d71c228407 96 store_imu_data(gy.y, &imu_data[4]);
yutation 7:88d71c228407 97 store_imu_data(gy.z, &imu_data[8]);
yutation 7:88d71c228407 98 store_imu_data(ac.x, &imu_data[12]);
yutation 7:88d71c228407 99 store_imu_data(ac.y, &imu_data[16]);
yutation 7:88d71c228407 100 store_imu_data(ac.z, &imu_data[20]);
yutation 7:88d71c228407 101
yutation 7:88d71c228407 102 Thread::wait(500);
sam_grove 5:96cb82af9996 103 }
sam_grove 5:96cb82af9996 104 }
sam_grove 5:96cb82af9996 105
yutation 7:88d71c228407 106 void step_counter(void const *arg) {
yutation 7:88d71c228407 107
yutation 7:88d71c228407 108 peak_value_init(&acc_peak);
yutation 7:88d71c228407 109 struct acce ac2;
yutation 7:88d71c228407 110
zhihanzhang 9:dcbd546412ea 111 while (1) {
yutation 7:88d71c228407 112 uint16_t i = 0;
yutation 7:88d71c228407 113 float temp = 0;
yutation 7:88d71c228407 114
yutation 7:88d71c228407 115 for (i = 0; i < FILTER_CNT; i++)
yutation 7:88d71c228407 116 {
yutation 7:88d71c228407 117 while(!IMU.accelAvailable());
yutation 8:2d43385e7784 118 //pc.printf("11\n");
yutation 7:88d71c228407 119 IMU.readAccel();
yutation 7:88d71c228407 120 ac2.x = IMU.calcAccel(IMU.ax);
yutation 7:88d71c228407 121 ac2.y = IMU.calcAccel(IMU.ay);
yutation 7:88d71c228407 122 ac2.z = IMU.calcAccel(IMU.az);
yutation 7:88d71c228407 123
yutation 7:88d71c228407 124 temp = ac2.x * DATA_FACTOR;
yutation 7:88d71c228407 125 acc_data.info[i].x = (short)(temp);
yutation 7:88d71c228407 126
yutation 7:88d71c228407 127 temp = ac2.y * DATA_FACTOR;
yutation 7:88d71c228407 128 acc_data.info[i].y = (short)temp;
yutation 7:88d71c228407 129
yutation 7:88d71c228407 130 temp = ac2.z * DATA_FACTOR;
yutation 7:88d71c228407 131 acc_data.info[i].z = (short)temp;
yutation 7:88d71c228407 132
yutation 7:88d71c228407 133 Thread::wait(5);
yutation 7:88d71c228407 134 }
yutation 7:88d71c228407 135
yutation 7:88d71c228407 136 filter_calculate(&acc_data, &acc_sample);
yutation 7:88d71c228407 137
yutation 7:88d71c228407 138 peak_update(&acc_peak, &acc_sample);
yutation 7:88d71c228407 139
yutation 7:88d71c228407 140 slid_update(&acc_slid, &acc_sample);
yutation 7:88d71c228407 141
yutation 7:88d71c228407 142 detect_step(&acc_peak, &acc_slid, &acc_sample);
yutation 7:88d71c228407 143
yutation 7:88d71c228407 144 timer.stop();
yutation 7:88d71c228407 145 if(timer.read_ms() <= 20)
yutation 7:88d71c228407 146 Thread::wait(20 - timer.read_ms());
yutation 8:2d43385e7784 147 //Thread::wait(5);
yutation 8:2d43385e7784 148
mbedAustin 0:59bec1fd956e 149 }
yutation 7:88d71c228407 150
mbedAustin 0:59bec1fd956e 151 }
mbedAustin 4:ba9100d52e48 152
yutation 7:88d71c228407 153 void skin_temp(void const *arg) {
yutation 8:2d43385e7784 154 float R1 = 4700*1.4773; //thermistor resistance at 15C
yutation 8:2d43385e7784 155 float R2 = 4700; //thermistor resistance at 25C
yutation 8:2d43385e7784 156 float R3 = 4700*0.69105; //thermistor resistance at 35C
yutation 8:2d43385e7784 157
yutation 8:2d43385e7784 158 float T1 = 288.15; //15C
yutation 8:2d43385e7784 159 float T2 = 298.15; //25C
yutation 8:2d43385e7784 160 float T3 = 308.15; //35C
yutation 7:88d71c228407 161
yutation 7:88d71c228407 162 float L1 = log(R1);
yutation 7:88d71c228407 163 float L2 = log(R2);
yutation 7:88d71c228407 164 float L3 = log(R3);
yutation 7:88d71c228407 165 float Y1 = 1/T1;
yutation 7:88d71c228407 166 float Y2 = 1/T2;
yutation 7:88d71c228407 167 float Y3 = 1/T3;
yutation 7:88d71c228407 168
yutation 7:88d71c228407 169 float g2 = (Y2-Y1)/(L2-L1);
yutation 7:88d71c228407 170 float g3 = (Y3-Y1)/(L3-L1);
yutation 7:88d71c228407 171
yutation 7:88d71c228407 172 float C = (g3-g2)/(L3-L2)*(1/(L1+L2+L3));
yutation 7:88d71c228407 173 float B = g2 - C*(L1*L1 + L1*L2 + L2*L2);
yutation 7:88d71c228407 174 float A = Y1 - L1*(B + L1*L1*C);
yutation 7:88d71c228407 175
yutation 7:88d71c228407 176 float Vt;
yutation 7:88d71c228407 177 while(1) {
yutation 7:88d71c228407 178 Vt = skinTemp;
yutation 8:2d43385e7784 179 //float R = 9900*(1/Vt - 1); //9900 is the resistance of R1 in voltage divider
yutation 8:2d43385e7784 180 float R = 4900 * Vt / (1 - Vt);
yutation 7:88d71c228407 181
yutation 7:88d71c228407 182 float T = 1/(A + B*log(R) + C*log(R)*log(R)*log(R));
yutation 8:2d43385e7784 183 //pc.printf("Vt: %f\n\r", Vt);
yutation 7:88d71c228407 184 //pc.printf("R: %f\n\r", R);
yutation 8:2d43385e7784 185
yutation 8:2d43385e7784 186 temp_skin=(uint8_t)(T-273.15);
yutation 8:2d43385e7784 187 //pc.printf("temp: %d\n", temp_skin);
yutation 8:2d43385e7784 188 //pc.printf("Skin temp: %f C\n\r", T-273.15);
yutation 8:2d43385e7784 189 Thread::wait(100);
yutation 7:88d71c228407 190 }
yutation 7:88d71c228407 191
yutation 7:88d71c228407 192 }
yutation 8:2d43385e7784 193
yutation 8:2d43385e7784 194 // calculate baseline to compare against
yutation 8:2d43385e7784 195 void Get_Baseline(void)
yutation 8:2d43385e7784 196 {
yutation 8:2d43385e7784 197 double sum = 0;
yutation 8:2d43385e7784 198 wait(1);
yutation 8:2d43385e7784 199 for(int i=0; i<500; i++) {
yutation 8:2d43385e7784 200 gsrValue = sig;
yutation 8:2d43385e7784 201 sum += gsrValue ;
yutation 8:2d43385e7784 202 wait(0.005);
yutation 8:2d43385e7784 203 }
yutation 8:2d43385e7784 204 baseline = sum/500;
yutation 8:2d43385e7784 205 //printf("baseline = %f\n\r", baseline);
yutation 8:2d43385e7784 206 }
yutation 8:2d43385e7784 207
yutation 8:2d43385e7784 208 // main loop, compare against baseline
yutation 8:2d43385e7784 209 // sound buzzer if a >5% change happens
yutation 8:2d43385e7784 210 void hum_R(void const *arg)
yutation 8:2d43385e7784 211 {
yutation 8:2d43385e7784 212 float delta;
yutation 8:2d43385e7784 213 float Serial_Port_Reading;
zhihanzhang 9:dcbd546412ea 214 int hr;
yutation 8:2d43385e7784 215 Get_Baseline();
yutation 8:2d43385e7784 216
yutation 8:2d43385e7784 217 while(1) {
yutation 8:2d43385e7784 218 gsrValue = gsr;
yutation 8:2d43385e7784 219 phasic = sig;
yutation 8:2d43385e7784 220 delta = gsrValue - phasic;
yutation 8:2d43385e7784 221 if(abs(delta) > 0.05) {
yutation 8:2d43385e7784 222 gsrValue = gsr;
yutation 8:2d43385e7784 223 delta = baseline - gsrValue;
yutation 8:2d43385e7784 224 }
zhihanzhang 9:dcbd546412ea 225 hr = 254* (phasic);
yutation 8:2d43385e7784 226 //pc.printf("HR!!: %f\n", phasic);
yutation 8:2d43385e7784 227 //Human_Resistance = ((1024+2*Serial_Port_Reading)*10000)/(512-Serial_Port_Reading);
zhihanzhang 9:dcbd546412ea 228 Human_Resistance = hr;
yutation 8:2d43385e7784 229 Thread::wait(100);
yutation 8:2d43385e7784 230 }
yutation 8:2d43385e7784 231 }
yutation 8:2d43385e7784 232
yutation 8:2d43385e7784 233
mbedAustin 4:ba9100d52e48 234 int main()
mbedAustin 4:ba9100d52e48 235 {
yutation 7:88d71c228407 236
yutation 8:2d43385e7784 237 //pc.baud(9600);
yutation 8:2d43385e7784 238 //dev.baud(9600);
yutation 7:88d71c228407 239
yutation 7:88d71c228407 240 IMU.begin();
yutation 7:88d71c228407 241 if (!IMU.begin()) {
yutation 7:88d71c228407 242 pc.printf("Failed to communicate with LSM9DS1.\n");
yutation 7:88d71c228407 243 }
yutation 7:88d71c228407 244 IMU.calibrate(1);
yutation 8:2d43385e7784 245 pc.printf("IMU end\n");
yutation 8:2d43385e7784 246
yutation 8:2d43385e7784 247 PulseSensor sensor(p15, sendDataToProcessing);
zhihanzhang 9:dcbd546412ea 248 sensor.start();
mbedAustin 4:ba9100d52e48 249
yutation 7:88d71c228407 250
yutation 7:88d71c228407 251 Thread t1(step_counter);
yutation 7:88d71c228407 252 Thread t2(skin_temp);
yutation 8:2d43385e7784 253 Thread t3(hum_R);
zhihanzhang 9:dcbd546412ea 254 bpm = 60;
yutation 7:88d71c228407 255
yutation 8:2d43385e7784 256 pc.printf("Main Loop\n");
mbedAustin 4:ba9100d52e48 257 while(1) {
yutation 7:88d71c228407 258
yutation 7:88d71c228407 259 dev.putc(0xff);
yutation 8:2d43385e7784 260 dev.putc(temp_skin);
zhihanzhang 9:dcbd546412ea 261 dev.putc(sensor.get_bpm());
zhihanzhang 9:dcbd546412ea 262 dev.putc(Human_Resistance);
yutation 8:2d43385e7784 263 dev.putc(get_step());
yutation 8:2d43385e7784 264
yutation 7:88d71c228407 265 dev.putc(0xff);
yutation 8:2d43385e7784 266 Thread::wait(200);
yutation 7:88d71c228407 267
mbedAustin 4:ba9100d52e48 268 }
mbedAustin 4:ba9100d52e48 269 }