main.cpp

00001 #include "mbed.h"
00002 #include "SDFileSystem.h"
00003 #include <errno.h>
00004 #include "watchdog.h"
00005 
00006 /*
00007    This basic example just shows how to read the ADC internal channels raw values.
00008    Please look in the corresponding device reference manual for a complete
00009    description of how to make a temperature sensor, VBat or Vref measurement.
00010 */
00011 
00012 AnalogIn adc_temp(ADC_TEMP);
00013 AnalogIn adc_vref(ADC_VREF);
00014 /* UD-GS2 H/W define
00015  PIO_SWin    PB_4
00016  PIO_wkup    PA_4
00017  PIO_enable  PB_0
00018  PIO_intout1 PB_2
00019  PIO_led     PB_5
00020  PIO_v20v    PC_13
00021  PIO_v18v    PH_0
00022  PIO_intout2 PH_1
00023  PIO_spics   PB_12
00024  PIO_battryMonEn   PA_5
00025  PB_7 SDA
00026  PB_6 SCL
00027 
00028 */
00029 //DigitalOut hx_clk(PB_7);
00030 //DigitalIn hx_dt(PB_6);
00031 //I2C               i2cacc(p_sda, p_scl)
00032 
00033 //#include "H3LIS331DL.h"
00034 
00035 //#define TIMEINTERVAL
00036 
00037 //キャリブレーション　仮値
00038 //No.1
00039 #define OFFSET_X   220
00040 #define OFFSET_Y   220 
00041 #define OFFSET_Z   -40 
00042 //No.2
00043 //#define OFFSET_X   375 
00044 //#define OFFSET_Y   340 
00045 //#define OFFSET_Z   -200
00046 //#define OFFSET_X   0
00047 //#define OFFSET_Y   0 
00048 //#define OFFSET_Z   0
00049 
00050 #define DATA_SIZE 512
00051 #define RETRY_COUNT 5
00052 
00053 HAL_StatusTypeDef writeEEPROMByte(uint32_t address, uint8_t data);
00054 uint8_t readEEPROMByte(uint32_t address);
00055 
00056 
00057 //H3LIS331DL  h3dacc(PB_7,PB_6);
00058 I2C i2c(PB_7,PB_6);                          //NUCLEO pin assign
00059 
00060 RawSerial         pc(PA_9, PA_10,115200); //console UART
00061 //LowPowerTicker    interrupt;
00062 //Ticker    interrupt;
00063 SPI               STSPI(PB_15, PB_14, PB_13); //mosi,miso,clk
00064 DigitalOut        STSPICS(PB_12);
00065 
00066 SDFileSystem *sd = new SDFileSystem(PA_12, PA_11, PB_3, PA_15, "sd"); // mosi, miso, sclk, cs, name
00067 
00068 DigitalOut        myled(PB_5);
00069 DigitalIn         button(PB_4);
00070 int               btn_flag = 0;
00071 
00072 int initLIS3DH();
00073 int read3axes(short *tx,short *ty,short *tz);
00074 int initLIS331();
00075 int read3axes331(short *tx,short *ty,short *tz);
00076 int int_sqrt(unsigned int x);
00077 //int readTemp(short *tmp);
00078 FILE     *fp1,*fp2;//fp1:H3LIS331DL, fp2:LIS3DH
00079 unsigned long timecount = 0;
00080 short oldcount = 0;
00081 short newcount = 0;
00082 long max_g=0;
00083 long boot_count = 0;
00084 long reboot_count = 0;
00085 char buffer1[DATA_SIZE] = {};
00086 char buffer2[DATA_SIZE] = {};
00087 char stracc1[32] = {}, stracc2[32] = {};
00088  int lognum1 = 0, lognum2 = 0;
00089 char filename1[16]= {};
00090 char filename2[16]= {};
00091 int init=1;
00092 short clear_flag = 0;
00093 short writed_flag = 0;
00094 short error_flag = 0;
00095 //short reboot_flag = 0;
00096 short ledcount_write = 0;
00097 short ledcount_error = 0;
00098 
00099 Watchdog wdt;
00100 
00101 void LED_Control_Write(short cnt){
00102     //Blinks every 0.5 seconds. 3 times.
00103     if(cnt == 0){
00104         myled = 0;
00105     }
00106     else if((cnt % 200) == 0 ){
00107         if(myled == 0){
00108             myled = 1;
00109         }
00110         else if(myled == 1){
00111             myled = 0;
00112         }    
00113     }
00114     
00115     if(cnt > 1000){
00116         myled = 1;
00117         writed_flag = 0;
00118     }
00119 }
00120 
00121 void LED_Control_Error(){
00122     //Blinks every 0.25 seconds. 
00123     short cnt = 0;
00124     while(cnt < 20){
00125         myled = 0;
00126         wait(0.25);
00127         myled = 1;
00128         wait(0.25);
00129         cnt++;
00130     }
00131     myled = 1;
00132 }
00133 
00134 void sub(){
00135     static short tx=0,ty=0,tz=0;
00136     static short tx2=0,ty2=0,tz2=0;
00137     static long scr = 0;
00138     static long scr2 = 0;
00139     static long ax,ay,az,as;
00140     static long ax2,ay2,az2,as2;
00141     static long tmp_max = 0, tmp_max2 = 0;
00142     short retry = 0;
00143     short ret = 0;
00144     
00145     read3axes331(&tx,&ty,&tz);
00146     //キャリブレーションの補正
00147     tx += OFFSET_X;
00148     ty += OFFSET_Y;
00149     tz += OFFSET_Z;
00150     
00151     //スカラー値変換
00152     scr = int_sqrt( tx*tx  + ty*ty + tz*tz);// 195mg/digit
00153     
00154     ax+= tx;
00155     ay+= ty;
00156     az+= tz;
00157     as+= scr;
00158     //pc.printf("%d,%d,x:%2.2f y:%2.2f,z:%2.2f scaler = %2.2f \r\n",timecount,time(NULL),(float)tx*0.005,(float)ty*0.005,(float)tz*0.005,(float)scr*0.005);
00159     //pc.printf("new x:%d y:%d,z:%d scr=%d, max=%d\r\n",tx,ty,tz,scr,tmp_max);
00160     
00161     //Total Max G
00162     if ( max_g < scr) {
00163         max_g = scr;
00164         //update=1;
00165         //pc.printf("%max g=%d \r\n",max_g);
00166         //Save EEPROM
00167         for(int i = 0; i < 4; i++){
00168             //pc.printf("writerom 0x%x\r\n",(max_g >> 8*i) & 0xFF);
00169             writeEEPROMByte(i, (max_g >> 8*i) & 0xFF);
00170         }
00171     }
00172     
00173     //MAX G per cycle
00174     if(tmp_max < scr){
00175         //pc.printf("tmp_max:%d,%d",tmp_max,scr);
00176         tmp_max = scr;
00177     }
00178     
00179     //平均(/s)
00180     if(newcount == 500){
00181         float asc = (float)as / 500.0f;
00182         pc.printf("avarage(new) x:%2.2f,y:%2.2f,z:%2.2f,scaler=%2.2f,max:%2.2f\r\n",
00183             (float)ax/500.0f*0.005,(float)ay/500.0f*0.005,(float)az/500.0f*0.005,(float)as/500.0f*0.005, (float)tmp_max*0.005);
00184         sprintf(stracc1,"%d,%d,%2.2f,%2.2f\r\n",timecount,time(NULL),asc*0.005,(float)tmp_max*0.005);
00185         
00186         if((strlen(buffer1)+strlen(stracc1)) > DATA_SIZE) {
00187             pc.printf("try write fp1:%d,%d\r\n",strlen(buffer1),strlen(stracc1));
00188             //ret = fprintf(fp1,"%s",buffer1);
00189             //ret = fwrite(buffer1,sizeof(char),strlen(buffer1),fp1);
00190             while((fwrite(buffer1,sizeof(char),strlen(buffer1),fp1) != strlen(buffer1)) && (retry < RETRY_COUNT)){
00191                 if(retry < RETRY_COUNT -1) {
00192                     pc.printf("fp1 fwrite failed:%d. retry=%d\r\n",ret,retry);
00193                 }
00194                 else if(retry == RETRY_COUNT -1){
00195                     pc.printf("fp1 fwrite failed:%d. Reboot!\r\n",ret);
00196                     reboot_count++;
00197                     for(int i = 0; i < 4; i++){
00198                         writeEEPROMByte(i+8, 0x00);
00199                         writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00200                     }
00201                     LED_Control_Error();
00202                     NVIC_SystemReset();
00203                 }
00204                 retry++;
00205                 wait_ms(1);
00206             }
00207             pc.printf("fp1 fwrite success:%d\r\n",ret);
00208             retry = 0;
00209             writed_flag = 1;
00210             ledcount_write = 0;
00211             
00212             memset(buffer1,0,sizeof(buffer1));
00213             sprintf(buffer1,"%s",stracc1);
00214         } else {
00215             sprintf(buffer1,"%s%s",buffer1,stracc1);
00216         }
00217     
00218         as=ax=ay=az=0;
00219         newcount = 0;
00220         tmp_max = 0;
00221     }
00222     
00223     //LED Control
00224     if(writed_flag){
00225         LED_Control_Write(ledcount_write);
00226         ledcount_write++;
00227     }
00228     
00229     if( (oldcount % 10) == 0){
00230         //LIS3DH
00231         read3axes(&tx2,&ty2,&tz2);
00232         scr2 = int_sqrt( tx2*tx2  + ty2*ty2 + tz2*tz2);
00233         ax2+= tx2;
00234         ay2+= ty2;
00235         az2+= tz2;
00236         as2+= scr2;
00237         if(tmp_max2 < scr2){
00238             tmp_max2 = scr2;
00239         }
00240          //pc.printf("old %d,%d,x:%2.2f y:%2.2f,z:%2.2f scaler = %2.2f \r\n",timecount,time(NULL),(float)tx2*0.005,(float)ty2*0.005,(float)tz2*0.005,(float)scr2*0.005);
00241         //pc.printf("%d,%d,old x:%d,y:%d,z:%d scaler = %d\r\n",timecount,time(NULL),tx,ty,tz,scr);
00242         //sprintf(stracc2,"%d,%d,%d\r\n",timecount,time(NULL),scr);
00243         //pc.printf("lis3dh:%s",stracc2);
00244         if(oldcount == 5000){
00245             float asc2 = (float)as2 / 500.0f;
00246             //pc.printf("avarage(old) x:%2.2f,y:%2.2f,z:%2.2f,scaler=%2.2f,max:%2.2f\r\n",
00247             //(float)ax2/500.0f*0.005,(float)ay2/500.0f*0.005,(float)az2/500.0f*0.005,(float)as2/500.0f*0.005, (float)tmp_max2*0.005);
00248             sprintf(stracc2,"%d,%d,%2.2f,%2.2f\r\n",timecount,time(NULL),asc2*0.005,(float)tmp_max2*0.005);
00249             
00250             if( (strlen(buffer2)+ strlen(stracc2)) > DATA_SIZE) {
00251                 pc.printf("try write fp2:%d,%d\r\n",strlen(buffer2),strlen(stracc2));   
00252                 while( (fwrite(buffer2,sizeof(char),strlen(buffer2),fp2) != strlen(buffer2)) && (retry < RETRY_COUNT)){
00253                     if(retry < RETRY_COUNT -1) {
00254                         pc.printf("fp2 fwrite failed:%d. retry=%d\r\n",ret,retry);
00255                     }
00256                     else if(retry == RETRY_COUNT -1){
00257                         pc.printf("fp2 fwrite failed:%d. Reboot!\r\n",ret);
00258                         reboot_count++;
00259                         for(int i = 0; i < 4; i++){
00260                             writeEEPROMByte(i+8,0x00);
00261                             writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00262                         }
00263                         LED_Control_Error();
00264                         NVIC_SystemReset();
00265                     }
00266                     retry++;
00267                     wait_ms(1);
00268                 }
00269                 pc.printf("fp2 fwrite success:%d\r\n",ret);
00270                 retry = 0;
00271                 memset(buffer2,0,sizeof(buffer2));
00272                 sprintf(buffer2,"%s",stracc2);
00273             } else {
00274                 sprintf(buffer2,"%s%s",buffer2,stracc2);
00275             }
00276         
00277             oldcount = 0;
00278             as2=ax2=ay2=az2=0;
00279             tmp_max2 = 0;
00280         }
00281     }
00282     
00283     if((time(NULL) % 300) == 0){
00284         //5 minutes passed. Save File.
00285         //pc.printf("5minutes passed. Save File.\r\n");
00286         if(fp1){
00287             while(fclose(fp1) != 0 && retry < RETRY_COUNT){
00288                 if(retry < RETRY_COUNT -1){
00289                     pc.printf("fp1 fclose failed:%d. retry=%d\r\n",ret,retry);
00290                 }
00291                 else if(retry == RETRY_COUNT -1){
00292                     pc.printf("fp1 fclose failed:%d. Reboot!\r\n",ret);
00293                     reboot_count++;
00294                     for(int i = 0; i < 4; i++){
00295                         writeEEPROMByte(i+8, 0x00);
00296                         writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00297                     }
00298                     LED_Control_Error();
00299                     NVIC_SystemReset();
00300                 }
00301                 retry++;
00302                 wait_ms(1);
00303             }
00304             retry = 0;
00305             fp1 = NULL;
00306         }
00307         if(fp2){
00308             while(fclose(fp2) != 0 && retry < RETRY_COUNT){
00309                 if(retry < RETRY_COUNT -1){
00310                     pc.printf("fp2 fclose failed:%d. retry=%d\r\n",ret,retry);
00311                 }
00312                 else if(retry == RETRY_COUNT -1){
00313                     pc.printf("fp2 fclose failed:%d. Reboot!\r\n",ret);
00314                         reboot_count++;
00315                         for(int i = 0; i < 4; i++){
00316                             writeEEPROMByte(i+8, 0x00);
00317                             writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00318                         }
00319                         LED_Control_Error();
00320                         NVIC_SystemReset();
00321                 }
00322                 retry++;
00323                 wait_ms(1);
00324             }
00325             retry = 0;
00326             fp2 = NULL;
00327         }
00328         
00329         fp1 = fopen(filename1,"ab");
00330         if(!fp1){
00331             while(!fp1 && retry < RETRY_COUNT){
00332                 pc.printf("fp1 fopen failed:%d\r\n",errno);
00333                 wait_ms(1);
00334                 fp1 = fopen(filename1,"ab");
00335                 if(fp1){
00336                     pc.printf("fp1 fopen success.\r\n");
00337                     retry = 0;
00338                     break;
00339                 }
00340                 else if(retry < RETRY_COUNT -1){
00341                     pc.printf("fp1 fopen failed. retyr=%d\r\n",retry);
00342                 }
00343                 else if(retry == RETRY_COUNT -1){
00344                     pc.printf("fp1 fopen failed. Reboot! %d\r\n",reboot_count+1);
00345                     reboot_count++;
00346                     
00347                     for(int i = 0; i < 4; i++){
00348                         pc.printf("writeEEPROM(reboot) 0x%x\r\n",(reboot_count >> 8*i) & 0xFF);
00349                         writeEEPROMByte(i+8, 0x00);
00350                         HAL_StatusTypeDef status = writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00351                         pc.printf("writed:0x%x,%d\r\n",readEEPROMByte(i+8),status);
00352                     }
00353                     LED_Control_Error();
00354                     NVIC_SystemReset();
00355                 }
00356                 retry++;
00357             }
00358         }
00359         fp2 = fopen(filename2,"ab");
00360         if(!fp2){
00361             while(!fp2 && retry < RETRY_COUNT){
00362                 pc.printf("fp2 fopen failed:%d\r\n",errno);
00363                 wait_ms(1);
00364                 fp2 = fopen(filename2,"ab");
00365                 if(fp2){
00366                     pc.printf("fp2 fopen success.\r\n");
00367                     retry = 0;
00368                     break;
00369                 }
00370                 else if(retry < RETRY_COUNT -1){
00371                     pc.printf("fp1 fopen failed. retyr=%d\r\n",retry);
00372                 }
00373                 else if(retry == RETRY_COUNT -1){
00374                     pc.printf("fp1 fopen failed. Reboot!\r\n");
00375                     reboot_count++;
00376                     for(int i = 0; i < 4; i++){
00377                         writeEEPROMByte(i+8, 0x00);
00378                         writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00379                     }
00380                     LED_Control_Error();
00381                     NVIC_SystemReset();
00382                 }
00383                 retry++;
00384             }
00385         }
00386     }
00387 
00388     if(time(NULL) > 86400){
00389             //Create New File
00390             set_time(NULL);
00391             timecount = 0;
00392             if(fp1){
00393                 while(fclose(fp1) != 0 && retry < RETRY_COUNT){
00394                     if(retry < RETRY_COUNT -1){
00395                         pc.printf("fp1 fclose failed:%d. retry=%d\r\n",ret,retry);
00396                     }
00397                     else if(retry == RETRY_COUNT -1){
00398                         pc.printf("fp1 fclose failed:%d. Reboot!\r\n",ret);
00399                         reboot_count++;
00400                         for(int i = 0; i < 4; i++){
00401                             writeEEPROMByte(i+8, 0x00);
00402                             writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00403                         }
00404                         LED_Control_Error();
00405                         NVIC_SystemReset();
00406                     }
00407                     retry++;
00408                     wait_ms(1);
00409                 }
00410                 retry = 0;
00411                 fp1 = NULL;
00412             }
00413             if(fp2){
00414                 while(fclose(fp2) != 0 && retry < RETRY_COUNT){
00415                     if(retry < RETRY_COUNT -1){
00416                         pc.printf("fp12 fclose failed:%d. retry=%d\r\n",ret,retry);
00417                     }
00418                     else if(retry == RETRY_COUNT -1){
00419                         pc.printf("fp2 fclose failed:%d. Reboot!\r\n",ret);
00420                         reboot_count++;
00421                         for(int i = 0; i < 4; i++){
00422                             writeEEPROMByte(i+8, 0x00);
00423                             writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00424                         }
00425                         LED_Control_Error();
00426                         NVIC_SystemReset();
00427                     }
00428                     retry++;
00429                     wait_ms(1);
00430                 }
00431                 retry = 0;
00432                 fp2 = NULL;
00433             }
00434             lognum1++;
00435             lognum2++;
00436             sprintf(filename1,"/sd/new_%d",lognum1);
00437             sprintf(filename2,"/sd/old_%d",lognum2);
00438             fp1 = fopen(filename1,"ab");
00439             while(!fp1 && retry < RETRY_COUNT){
00440                 pc.printf("fp1 fopen failed:%d\r\n",errno);
00441                 wait_ms(1);
00442                 fp1 = fopen(filename1,"ab");
00443                 if(fp1){
00444                     pc.printf("fp1 fopen success.\r\n");
00445                     retry = 0;
00446                     break;
00447                 }
00448                 else if(retry < RETRY_COUNT -1){
00449                     pc.printf("fp1 fopen failed. retyr=%d\r\n",retry);
00450                 }
00451                 else if(retry == RETRY_COUNT -1){
00452                     pc.printf("fp1 fopen failed. Reboot!\r\n");
00453                     reboot_count++;
00454                     for(int i = 0; i < 4; i++){
00455                         writeEEPROMByte(i+8, 0x00);
00456                         writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00457                     }
00458                     LED_Control_Error();
00459                     NVIC_SystemReset();
00460                 }
00461                 retry++;
00462             }
00463             fp2 = fopen(filename2,"ab");
00464             while(!fp2 && retry < RETRY_COUNT){
00465                 pc.printf("fp2 fopen failed:%d\r\n",errno);
00466                 wait_ms(1);
00467                 fp1 = fopen(filename2,"ab");
00468                 if(fp2){
00469                     pc.printf("fp2 fopen success.\r\n");
00470                     retry = 0;
00471                     break;
00472                 }
00473                 else if(retry < RETRY_COUNT -1){
00474                     pc.printf("fp2 fopen failed. retyr=%d\r\n",retry);
00475                 }
00476                 else if(retry == RETRY_COUNT -1){
00477                     pc.printf("fp2 fopen failed. Reboot!\r\n");
00478                     reboot_count++;
00479                     for(int i = 0; i < 4; i++){
00480                         writeEEPROMByte(i+8, 0x00);
00481                         writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00482                     }
00483                     LED_Control_Error();
00484                     NVIC_SystemReset();
00485                 }
00486                 retry++;
00487             }
00488         
00489             pc.printf("Create New File:%d,%d",timecount,time(NULL));
00490         }
00491         
00492         //Button Check
00493         if(button.read() == 1){
00494             btn_flag++;
00495             //pc.printf("Button pressed:%d\r\n",btn_flag);
00496         }
00497         else{
00498             btn_flag = 0;
00499         }
00500         
00501         if(btn_flag > 1000 && !clear_flag){
00502             //ROM Crear
00503             pc.printf("Button pressed. ROM clear\r\n");
00504             myled = 0;
00505             for(int i = 0; i < 16; i++){
00506                 writeEEPROMByte(i, 0x00);
00507             }
00508             
00509             if(fp1){
00510                 while(fclose(fp1) != 0 && retry < RETRY_COUNT){
00511                     if(retry < RETRY_COUNT -1){
00512                         pc.printf("fp1 fclose failed:%d. retry=%d\r\n",ret,retry);
00513                     }
00514                     else if(retry == RETRY_COUNT -1){
00515                         pc.printf("fp1 fclose failed:%d. Reboot!\r\n",ret);
00516                         reboot_count++;
00517                         for(int i = 0; i < 4; i++){
00518                             writeEEPROMByte(i+8, 0x00);
00519                             writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00520                         }
00521                         LED_Control_Error();
00522                         NVIC_SystemReset();
00523                     }
00524                     retry++;
00525                     wait_ms(1);
00526                 }
00527                 retry = 0;
00528                 fp1 = NULL;
00529             }
00530             if(fp2){
00531                 while(fclose(fp2) != 0 && retry < RETRY_COUNT){
00532                     if(retry < RETRY_COUNT -1){
00533                         pc.printf("fp2 fclose failed:%d. retry=%d\r\n",ret,retry);
00534                     }
00535                     else if(retry == RETRY_COUNT -1){
00536                         pc.printf("fp2 fclose failed:%d. Reboot!\r\n",ret);
00537                         reboot_count++;
00538                         for(int i = 0; i < 4; i++){
00539                             writeEEPROMByte(i+8, 0x00);
00540                             writeEEPROMByte(i+8, (reboot_count >> 8*i) & 0xFF);
00541                         }
00542                         LED_Control_Error();
00543                         NVIC_SystemReset();
00544                     }
00545                     retry++;
00546                     wait_ms(1);
00547                 }
00548                 retry = 0;
00549                 fp2 = NULL;
00550             }
00551             
00552             btn_flag = 0;
00553             clear_flag = 1;
00554             boot_count = 0;
00555             reboot_count = 0;
00556             
00557             wait(3);
00558             myled = 1;
00559         }
00560 }
00561 
00562 int main()
00563 {
00564     short retry = 0;
00565     pc.printf("UD-GS5 Start acceleration measurement\r\n");
00566     set_time(NULL);
00567     
00568     myled = 0;
00569     wait(3);
00570     myled = 1;
00571     
00572     //Read Boot/Reboot count
00573     for(int i = 0; i < 4; i++){
00574         boot_count |= readEEPROMByte(i+4) << 8*i;
00575     }
00576     boot_count++;
00577     for(int i = 0; i < 4; i++){
00578         writeEEPROMByte(i+4, (boot_count >> 8*i) & 0xFF);
00579     }
00580     for(int i = 0; i < 4; i++){
00581         //pc.printf("reboot_count:0x%x\r\n",readEEPROMByte(i+8));
00582         reboot_count |= readEEPROMByte(i+8) << 8*i;
00583     }
00584     pc.printf("boot=%d, reboot=%d\r\n",boot_count,reboot_count);
00585     
00586     //Read Max G
00587     for(int i = 0; i < 4; i++){
00588             //pc.printf("readEEPROM:0x%x\r\n",readEEPROMByte(i));
00589             max_g |=  readEEPROMByte(i) << 8*i;
00590     }
00591     pc.printf("read max_g from eeprom:%2.2f\r\n",(float)max_g*0.005);
00592     
00593     i2c.frequency(400000);
00594 
00595     sprintf(filename1,"/sd/new_%d",lognum1);
00596     sprintf(filename2,"/sd/old_%d",lognum2);
00597     
00598     if (wdt.WatchdogCausedReset())
00599         pc.printf("WatchDog Reset.\r\n");
00600     
00601     wdt.Configure(23);
00602 
00603     while(!fp1 && retry < 5){
00604         fp1 = fopen(filename1,"ab");
00605         if(!fp1) {
00606             pc.printf("fp1(%s) fopen failed errno:%d\r\n",filename1,errno);
00607             //NVIC_SystemReset();
00608         }
00609         else{
00610             pc.printf("fp1 fopen success\r\n");
00611         }
00612         retry++;
00613         wait_ms(100);
00614     }
00615     retry = 0;
00616     
00617     while(!fp2 && retry < 5){
00618         fp2 = fopen(filename2,"ab");
00619         if(!fp2) {
00620             pc.printf("fp2(%s) fopen failed errno:%d\r\n",filename2,errno);
00621             //NVIC_SystemReset();
00622         }
00623         else{
00624             pc.printf("fp2 fopen success\r\n");
00625         }
00626         retry++;
00627         wait_ms(100);
00628     }
00629     retry = 0;
00630     
00631     while(init) {
00632         init = initLIS331();
00633         initLIS3DH();
00634         pc.printf(" init acc sensor %d\r\n",init);
00635         wait_ms(100);
00636     }
00637     //interrupt.attach_us(&timer,20000);//20ms
00638     
00639     while(1)
00640     {
00641         wdt.Service();
00642         timecount++;
00643         oldcount++;
00644         newcount++;
00645         sub();
00646         //wait_us(2500);
00647     }
00648 
00649     //pc.printf("Program End.\r\n");
00650     return 0;
00651 }
00652 /*********** porting **************/
00653 void spiFormat(int b,int m)
00654 {
00655     STSPI.format(b,m);  /* 8bit */
00656 }
00657 void spiFrequency(int f)
00658 {
00659     STSPI.frequency(f); /* 1Mbps */
00660 }
00661 void spiWriteCS(int cs)
00662 {
00663     STSPICS=cs;
00664 }
00665 int spiWrite(int wd)
00666 {
00667     return ( STSPI.write(wd));
00668 }
00669 
00670 /*******************************************************
00671   EEPROM WRITE
00672 ********************************************************/  
00673 HAL_StatusTypeDef writeEEPROMByte(uint32_t address, uint8_t data)
00674  {
00675     HAL_StatusTypeDef  status;
00676     address = address + 0x08080000;
00677     
00678     if (!IS_FLASH_DATA_ADDRESS(address)) {
00679         pc.printf("writeEEPROMByte address outofrange\r\n");
00680         return HAL_ERROR;
00681     }
00682     
00683     HAL_FLASHEx_DATAEEPROM_Unlock();
00684     //pc.printf("writeEEPROMByte HAL_FLASH_DATAEEPROM_Unlock:%d\r\n",status);
00685     status = HAL_FLASHEx_DATAEEPROM_Program(TYPEPROGRAMDATA_BYTE, address, data);
00686     //pc.printf("writeEEPROMByte HAL_FLASH_DATAEEPROM_Program:%d\r\n",status);
00687     HAL_FLASHEx_DATAEEPROM_Lock();
00688     //pc.printf("writeEEPROMByte HAL_FLASH_DATAEEPROM_lock:%d\r\n",status);
00689     
00690     return status;
00691 }
00692 
00693 /*******************************************************
00694   EEPROM READ 
00695 ********************************************************/  
00696 uint8_t readEEPROMByte(uint32_t address) {
00697     volatile uint8_t tmp ;
00698     address = address + 0x08080000;
00699     tmp = *(__IO uint8_t*)address;
00700     return tmp;
00701 }
00702 
00703 /***************************
00704   integer sqrt
00705     整数 sqrt √演算を整数で　ライブラリより速い
00706 ****************************/
00707 int int_sqrt(unsigned int x)
00708 {
00709   register int a = 0, c = 0, y = 0, i = 0, t = x;
00710   while(t >>= 1){
00711     ++i;
00712   }
00713   for(i += i & 1; i >= 0; i -= 2){
00714     c = (y << 1 | 1) <= x >> i;
00715     a = a << 1 | c;
00716     y = y << 1 | c;
00717     x -= c * y << i;
00718     y += c;
00719   }
00720   return a;
00721 }