Diff: main.cpp

Revision:

13:df67ca499051

Parent:

11:2d5fcf102778

Child:

14:2707af31e02f

--- a/main.cpp	Mon Aug 30 08:47:17 2021 +0000
+++ b/main.cpp	Wed Sep 01 11:37:07 2021 +0000
@@ -29,14 +29,24 @@
 //DigitalIn hx_dt(PB_6);
 //I2C               i2cacc(p_sda, p_scl)
 
-#include "H3LIS331DL.h"
+//#include "H3LIS331DL.h"
+
+//#define TIMEINTERVAL
 
-#define TIMEINTERVAL
+//キャリブレーション　仮値
+#define OFFSET_X   0
+#define OFFSET_Y   0
+#define OFFSET_Z   0
 
-H3LIS331DL  h3dacc(PB_7,PB_6);
+HAL_StatusTypeDef writeEEPROMByte(uint32_t address, uint8_t data);
+uint8_t readEEPROMByte(uint32_t address);
+
+
+//H3LIS331DL  h3dacc(PB_7,PB_6);
+I2C i2c(PB_7,PB_6);                          //NUCLEO pin assign
 
 RawSerial         pc(PA_9, PA_10,115200); //console UART
-LowPowerTicker    interrupt;
+//LowPowerTicker    interrupt;
 //Ticker    interrupt;
 SPI               STSPI(PB_15, PB_14, PB_13); //mosi,miso,clk
 DigitalOut        STSPICS(PB_12);
@@ -46,126 +56,103 @@
 DigitalOut        led(PB_5);
 int initLIS3DH();
 int read3axes(short *tx,short *ty,short *tz);
+int initLIS331();
+int read3axes331(short *tx,short *ty,short *tz);
+int int_sqrt(unsigned int x);
 //int readTemp(short *tmp);
 FILE     *fp1,*fp2;//fp1:H3LIS331DL, fp2:LIS3DH
-int timecount = 0;
-//static uint8_t buffer[512] = {};
-//char buffer1[512] = {};
-//char buffer2[512] = {};
-//char stracc[32] = {};
-//bool write1 = false,write2 = false;
-//double xyz[3] = {};
-
-/*
-void timer(void)
-{
-    short x=0,y=0,z=0;
-    float lis3dh_acc = 0;
-    double xyz[3]= {},h3lis331dl_acc=0;
-    //char stracc[32] = {};
-
-//H3LIS331DL
-    h3dacc.getAcceleration(xyz);
-    //pc.printf("x:%f,y:%f,z:%f\r\n",(float)xyz[0],(float)xyz[1],(float)xyz[2]); //!!!If you read here, you cannot get acceleration!!!
-    h3lis331dl_acc = sqrt(xyz[0]*xyz[0] + xyz[1]*xyz[1] + xyz[2]*xyz[2]);
-    sprintf(stracc1,"%ld,%lf\r\n",timecount*200,h3lis331dl_acc);
-    pc.printf("h3lis331dl:%s",stracc1);
-    if(strlen(buffer1)+strlen(stracc1) > 512) {
-        //pc.printf("buffer over. write file2\r\n");
-        write1 = true;
-    } else {
-        sprintf(buffer1,"%s%s",buffer1,stracc1);
-    }
-
-    if(timecount == 10) {
-        //pc.printf("calc lis3dh acc\r\n");
-        //LIS3DH
-        read3axes(&x,&y,&z);
-        lis3dh_acc = sqrt( (float)x/1024.0f * (float)x/1024 + (float)y/1024.0f * (float)y/1024 + (float)z/1024.0f * (float)z/1024);
-        sprintf(stracc2,"%ld,%lf\r\n",time(NULL),lis3dh_acc);
-        pc.printf("lis3dh:%s",stracc2);
-        if(strlen(buffer1)+ strlen(stracc1)> 512) {
-            write2 = true;
-        } else {
-            sprintf(buffer2,"%s%s",buffer2,stracc2);
-        }
-        timecount = 0;
-    }
+unsigned long timecount = 0;
+int oldcount = 0;
+//uint8_t maxacc = 0;
+long max_g=0;
+long now_g;
+int  update;
+char buffer1[512] = {};
+char buffer2[512] = {};
+char stracc1[32] = {}, stracc2[32] = {};
+ int lognum1 = 0, lognum2 = 0;
+char filename1[32]= {};
+char filename2[32]= {};
+int ret = 0;
 
-    timecount++;
-}
-*/
-
-int main()
-{
-    int lognum1 = 0, lognum2 = 0;
-    char filename1[32]= {};
-    char filename2[32]= {};
-    int ret = 0;
-    short x=0,y=0,z=0;
-    double xyz[3] = {}, h3lis331dl_acc = 0;
-    float lis3dh_acc = 0;
-    int count = 0;
-    char buffer1[512] = {};
-    char buffer2[512] = {};
-    char stracc1[32] = {}, stracc2[32] = {};
-
-    initLIS3DH();
-
-    h3dacc.init(H3LIS331DL_ODR_50Hz, H3LIS331DL_NORMAL,H3LIS331DL_FULLSCALE_8);//これで初期化している FULLSCALE_8=400G
-    h3dacc.setHPFMode(H3LIS331DL_HPM_NORMAL_MODE_RES);//High Pass Filter ON
-    //h3dacc.setHPFCutOFF(H3LIS331DL_HPFCF_1);
-
-    sprintf(filename1,"/sd/lis3dh_%d",lognum1);
-    sprintf(filename2,"/sd/h3lis_%d",lognum2);
-
-    fp1 = fopen(filename1,"a");
-    if(!fp1) {
-        pc.printf("fp1(%s) open failed\r\n",filename1);
+void timer(){
+    short tx=0,ty=0,tz=0;
+    long scr=0;
+    //static long ax,ay,az,as;
+    //static  int cnt;
+    
+    read3axes331(&tx,&ty,&tz);
+    //キャリブレーションの補正
+    tx += OFFSET_X;
+    ty += OFFSET_Y;
+    tz += OFFSET_Z;
+    
+    //スカラー値変換
+    scr = int_sqrt( tx*tx  + ty*ty + tz*tz);
+    now_g = scr;
+    
+    //ax+= tx;
+    //ay+= ty;
+    //az+= tz;
+    //as+= scr;
+    //cnt++;
+    pc.printf("new x:%d y:%d,z:%d scaler = %d  max g=%2.2fG \r\n",tx,ty,tz,scr,  (float)max_g/11.0f);
+    if ( max_g < scr) {
+        max_g = scr;
+        update=1;
+        //pc.printf("x:%d y:%d,z:%d scaler = %d  max g=%2.2fG \r\n",tx,ty,tz,scr,  (float)scr/11.0f);
+        //pc.printf("new x:%d y:%d,z:%d scaler = %d  max g=%2.2fG \r\n",tx,ty,tz,scr,  (float)max_g/11.0f);
+        //Save EEPROM
+        writeEEPROMByte(0, max_g);
     }
-    fp2 = fopen(filename2,"a");
-    if(!fp2) {
-        pc.printf("fp2(%s) open failed\r\n",filename2);
-    }
-
-    //interrupt.attach_us(&timer,200000);//200ms
-
-    while(1) {
-        //H3LIS331DL
-        h3dacc.getAcceleration(xyz);
-        //pc.printf("x:%f,y:%f,z:%f\r\n",(float)xyz[0],(float)xyz[1],(float)xyz[2]); //!!!If you read here, you cannot get acceleration!!!
-        h3lis331dl_acc = sqrt(xyz[0]*xyz[0] + xyz[1]*xyz[1] + xyz[2]*xyz[2]);
-        sprintf(stracc1,"%ld,%lf\r\n",count*20,h3lis331dl_acc);
-        //pc.printf("h3lis331dl:%s",stracc1);
-        if(strlen(buffer1)+strlen(stracc1) > 512) {
+    
+    sprintf(stracc1,"%d,%lf\r\n",timecount,scr);
+    //pc.printf("h3lis331dl:%s",stracc1);
+    if(strlen(buffer1)+strlen(stracc1) > 512) {
             //pc.printf("buffer over. write file2\r\n");
             ret = fprintf(fp1,"%s",buffer1);
+             if(ret == 0){
+                pc.printf("fp1 write failed. Reboot!!!\r\n");
+                NVIC_SystemReset();
+            }
             memset(buffer1,0,sizeof(buffer1));
             sprintf(buffer1,"%s",stracc1);
-            //write1 = false;
         
         } else {
             sprintf(buffer1,"%s%s",buffer1,stracc1);
         }
-        
-        if(count == 10) {
+    
+    /* 
+    //16回平均
+    if ((cnt & 0x0f)== 0) {
+         float asc = (float)as / 11.0f /16.0f;
+         pc.printf("avarage x:%d\ty:%d\tz:%d\tscaler=%d\t%2.2fG \r\n",ax/16,ay/16,az/16,as/16,  asc );
+        as=ax=ay=az=0;
+    }
+    */
+    
+    if(oldcount == 10){
         //LIS3DH
-        read3axes(&x,&y,&z);
-        lis3dh_acc = sqrt( (float)x/1024.0f * (float)x/1024 + (float)y/1024.0f * (float)y/1024 + (float)z/1024.0f * (float)z/1024);
-        sprintf(stracc2,"%ld,%lf\r\n",timecount*20*10,lis3dh_acc);
+        read3axes(&tx,&ty,&tz);
+        scr = int_sqrt( tx*tx  + ty*ty + tz*tz);
+        pc.printf("old x:%d y:%d,z:%d scaler = %d  max g=%2.2fG \r\n",tx,ty,tz,scr,  (float)max_g/11.0f);
+        sprintf(stracc2,"%d,%lf\r\n",timecount,scr);
         //pc.printf("lis3dh:%s",stracc2);
         if(strlen(buffer2)+ strlen(stracc2)> 512) {
             ret = fprintf(fp2,"%s",buffer2);
-            //pc.printf("write buffer:%d\r\n",ret);
+            if(ret == 0){
+                pc.printf("fp2 write failed. Reboot!!!\r\n");
+                NVIC_SystemReset();
+            }
             memset(buffer2,0,sizeof(buffer2));
             sprintf(buffer2,"%s",stracc2);
         } else {
             sprintf(buffer2,"%s%s",buffer2,stracc2);
         }
-        count = 0;
+        oldcount = 0;
     }
-        
-        if(time(NULL) > 86400){
+    
+    if(time(NULL) > 86400){
             //Create New File
             set_time(NULL);
             timecount = 0;
@@ -173,26 +160,59 @@
             fclose(fp2);
             lognum1++;
             lognum2++;
-            sprintf(filename1,"/sd/lis3dh_%d",lognum1);
-            sprintf(filename2,"/sd/h3lis331dl_%d",lognum2);
+            sprintf(filename1,"/sd/new_%d",lognum1);
+            sprintf(filename2,"/sd/old_%d",lognum2);
             fp1 = fopen(filename1,"a");
             if(!fp1){
                 pc.printf("fp1 create file failed\r\n");
+                NVIC_SystemReset();
                 }
             fp2 = fopen(filename2,"a");
             if(!fp2){
                 pc.printf("fp2 create file failed\r\n");
+                NVIC_SystemReset();
             }
         }
-        
-        led = !led;
+}
+
+int main()
+{
+    //h3dacc.init(H3LIS331DL_ODR_50Hz, H3LIS331DL_NORMAL,H3LIS331DL_FULLSCALE_8);//これで初期化している FULLSCALE_8=400G
+    //h3dacc.setHPFMode(H3LIS331DL_HPM_NORMAL_MODE_RES);//High Pass Filter ON
+    //h3dacc.setHPFCutOFF(H3LIS331DL_HPFCF_1);
+
+    sprintf(filename1,"/sd/new_%d",lognum1);
+    sprintf(filename2,"/sd/old_%d",lognum2);
+
+    fp1 = fopen(filename1,"a");
+    if(!fp1) {
+        pc.printf("fp1(%s) open failed\r\n",filename1);
+        //NVIC_SystemReset();
+    }
+    fp2 = fopen(filename2,"a");
+    if(!fp2) {
+        pc.printf("fp2(%s) open failed\r\n",filename2);
+        //NVIC_SystemReset();
+    }
+
+    //interrupt.attach_us(&timer,200000);//200ms
+
+    while(ret) {
+      ret = initLIS3DH();
+      ret = initLIS331();
+      pc.printf(" init acc sensor %d\r\n",ret);
+      wait_ms(100);
+    }
+    while(1)
+    {
+        timer();
         timecount++;
-        count++;
+        oldcount++;
         wait_ms(20);
     }
+    
 }
 /*********** porting **************/
-
 void spiFormat(int b,int m)
 {
     STSPI.format(b,m);  /* 8bit */
@@ -210,7 +230,45 @@
     return ( STSPI.write(wd));
 }
 
-void error(const char* format, ...)
+/*******************************************************
+  EEPROM WRITE
+********************************************************/  
+HAL_StatusTypeDef writeEEPROMByte(uint32_t address, uint8_t data)
+ {
+    HAL_StatusTypeDef  status;
+    address = address + 0x08080000;
+    HAL_FLASHEx_DATAEEPROM_Unlock();  //Unprotect the EEPROM to allow writing
+    status = HAL_FLASHEx_DATAEEPROM_Program(TYPEPROGRAMDATA_BYTE, address, data);
+    HAL_FLASHEx_DATAEEPROM_Lock();  // Reprotect the EEPROM
+    return status;
+}
+
+/*******************************************************
+  EEPROM READ 
+********************************************************/  
+uint8_t readEEPROMByte(uint32_t address) {
+    volatile uint8_t tmp ;
+    address = address + 0x08080000;
+    tmp = *(__IO uint8_t*)address;
+    return tmp;
+}
+
+/***************************
+  integer sqrt
+    整数 sqrt √演算を整数で　ライブラリより速い
+****************************/
+int int_sqrt(unsigned int x)
 {
-    return;
+  register int a = 0, c = 0, y = 0, i = 0, t = x;
+  while(t >>= 1){
+    ++i;
+  }
+  for(i += i & 1; i >= 0; i -= 2){
+    c = (y << 1 | 1) <= x >> i;
+    a = a << 1 | c;
+    y = y << 1 | c;
+    x -= c * y << i;
+    y += c;
+  }
+  return a;
 }
\ No newline at end of file