Diff: main.cpp

Revision:

7:88d71c228407

Parent:

6:0398d0fcc8cc

Child:

8:2d43385e7784

--- a/main.cpp	Wed Sep 16 16:28:29 2015 +0000
+++ b/main.cpp	Wed Dec 01 00:20:32 2021 +0000
@@ -1,35 +1,228 @@
 #include "mbed.h"
+#include "rtos.h"
+#include "LSM9DS1.h"
+#include "stepcounter.h"
+#define PI 3.14159
+// Earth's magnetic field varies by location. Add or subtract
+// a declination to get a more accurate heading. Calculate
+// your's here:
+// http://www.ngdc.noaa.gov/geomag-web/#declination
+#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
+LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+Timer timer;
+
+//
+filter_avg_t acc_data;
+axis_info_t acc_sample;
+peak_value_t acc_peak;
+slid_reg_t acc_slid;
+uint8_t step_cnt = 0;
+//
+AnalogIn skinTemp(p19);
 
 RawSerial  pc(USBTX, USBRX);
-RawSerial  dev(p28,p27);
+RawSerial  dev(p9,p10);
 DigitalOut led1(LED1);
 DigitalOut led4(LED4);
 
-void dev_recv()
-{
-    led1 = !led1;
-    while(dev.readable()) {
-        pc.putc(dev.getc());
+struct gyro {
+    float x;
+    float y;
+    float z;
+};
+
+struct acce {
+    float x;
+    float y;
+    float z;
+};
+
+char imu_data[24];
+
+
+char c[4];
+void aa(void const *arg){
+    char d1 = '0';
+    while(1){
+        c[1] = d1;
+        d1++;
+        if(d1 >= '9')
+            d1 = '0';
+        Thread::wait(100);
+    }
+}
+    
+void bb(void const *arg){
+    char d2 = 'a';
+    while(1){
+        c[2] = d2;
+        d2++;
+        if(d2 >= 'z')
+            d2 = 'a';
+        Thread::wait(100);
+    }
+}
+
+void out(void const *arg){
+    while(1){
+        //int i = 1;
+        //dev.putc(c[1]);
+        //dev.putc(c[2]);
+        //dev.putc(10);
+        //pc.printf("step: %d\n", step_cnt);
+        Thread::wait(100);
+    }
+}
+
+void store_imu_data(float d, char* sp) {
+    int id = *(int*)&d;
+    for(int i = 0; i < 4; i++) {
+        *(sp+i) = id % 0xff;
+        id = id >> 8;
+    }
+}
+
+void update_IMU_data(void const *arg){
+    struct gyro gy;
+    struct acce ac;
+    while(1){
+        while(!IMU.accelAvailable());
+        IMU.readAccel();
+        while(!IMU.gyroAvailable());
+        IMU.readGyro();
+        
+        gy.x = IMU.calcGyro(IMU.gx);
+        gy.y = IMU.calcGyro(IMU.gy);
+        gy.z = IMU.calcGyro(IMU.gz);
+        ac.x = IMU.calcAccel(IMU.ax);
+        ac.y = IMU.calcAccel(IMU.ay);
+        ac.z = IMU.calcAccel(IMU.az);
+        
+        store_imu_data(gy.x, &imu_data[0]);
+        store_imu_data(gy.y, &imu_data[4]);
+        store_imu_data(gy.z, &imu_data[8]);
+        store_imu_data(ac.x, &imu_data[12]);
+        store_imu_data(ac.y, &imu_data[16]);
+        store_imu_data(ac.z, &imu_data[20]);
+        
+        Thread::wait(500);  
     }
 }
 
-void pc_recv()
-{
-    led4 = !led4;
-    while(pc.readable()) {
-        dev.putc(pc.getc());
+void step_counter(void const *arg) {
+    
+    peak_value_init(&acc_peak);
+    struct acce ac2;
+
+    while (1)
+    {
+        timer.reset();
+        timer.start();
+        uint16_t i = 0;
+        float temp = 0;
+
+        for (i = 0; i < FILTER_CNT; i++)
+        {
+            while(!IMU.accelAvailable());
+            IMU.readAccel();
+            ac2.x = IMU.calcAccel(IMU.ax);
+            ac2.y = IMU.calcAccel(IMU.ay);
+            ac2.z = IMU.calcAccel(IMU.az);
+            
+            temp = ac2.x * DATA_FACTOR;
+            acc_data.info[i].x = (short)(temp);
+
+            temp = ac2.y * DATA_FACTOR;
+            acc_data.info[i].y = (short)temp;
+
+            temp = ac2.z * DATA_FACTOR;
+            acc_data.info[i].z = (short)temp;
+            
+            Thread::wait(5);
+        }
+
+        filter_calculate(&acc_data, &acc_sample);
+
+        peak_update(&acc_peak, &acc_sample);
+
+        slid_update(&acc_slid, &acc_sample);
+
+        detect_step(&acc_peak, &acc_slid, &acc_sample);
+        
+        timer.stop();
+        if(timer.read_ms() <= 20)
+            Thread::wait(20 - timer.read_ms());
     }
+    
 }
 
+void skin_temp(void const *arg) {
+     float R1 = 12457; //thermistor resistance at 20C
+    float R2 = 8052.1; //thermistor resistance at 30C
+    float R3 = 5334.0; //thermistor resistance at 40C
+    
+    float T1 = 293.15; //20C
+    float T2 = 303.15; //30C
+    float T3 = 313.15; //40C
+
+    float L1 = log(R1);
+    float L2 = log(R2);
+    float L3 = log(R3);
+    float Y1 = 1/T1;
+    float Y2 = 1/T2;
+    float Y3 = 1/T3;
+
+    float g2 = (Y2-Y1)/(L2-L1);
+    float g3 = (Y3-Y1)/(L3-L1);
+
+    float C = (g3-g2)/(L3-L2)*(1/(L1+L2+L3));
+    float B = g2 - C*(L1*L1 + L1*L2 + L2*L2);
+    float A = Y1 - L1*(B + L1*L1*C);
+    
+    float Vt;
+    while(1) {
+        Vt = skinTemp;
+        //myled = skinTemp;
+        float R = 9900*(1/Vt - 1); //9900 is the resistance of R1 in voltage divider
+
+        float T = 1/(A + B*log(R) + C*log(R)*log(R)*log(R));
+        //pc.printf("Vt: %f\n\r", 1 - Vt);
+        //pc.printf("R: %f\n\r", R);
+              
+        //pc.printf("Skin temp: %f\n\r", T-273.15);
+        Thread::wait(1000);      
+    }
+    
+}
+
+
 int main()
 {
+    
     pc.baud(9600);
     dev.baud(9600);
+    
+    IMU.begin();
+    if (!IMU.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
+    IMU.calibrate(1);
 
-    pc.attach(&pc_recv, Serial::RxIrq);
-    dev.attach(&dev_recv, Serial::RxIrq);
+   
+    Thread t1(step_counter);
+    Thread t2(skin_temp);
+    //Thread t2(bb);
+    //Thread t3(out);
 
+    
     while(1) {
-        sleep();
+        
+        dev.putc(0xff);
+        dev.putc('a');
+        dev.putc('b');
+        dev.putc('\n')
+        dev.putc(0xff);
+        Thread::wait(10000);
+        
     }
 }