Revision 5:100310ea8fba, committed 2016-08-23

Comitter:

herenvarno

Date:

Tue Aug 23 15:59:59 2016 +0000

Parent:

4:3fecfc9eeadd

Child:

6:fb1b3ce7738d

Commit message:

final

Changed in this revision

--- a/func/Persistor.cpp	Mon Dec 14 18:54:42 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,50 +0,0 @@
-#include "Persistor.h"
-/*
- 
-struct Sector {
-    uint32_t address, sector;
-};
-Sector FlashMem[ 7 ] = {
-    { 0x08000000, FLASH_SECTOR_0 }, // 16 KB
-    { 0x08004000, FLASH_SECTOR_1 }, // 16 KB
-    { 0x08008000, FLASH_SECTOR_2 }, // 16 KB
-    { 0x0800C000, FLASH_SECTOR_3 }, // 16 KB
-    { 0x08010000, FLASH_SECTOR_4 }, // 64 KB
-    { 0x08020000, FLASH_SECTOR_5 }  // 128 KB
-};
-const int USE_FLASH_SECTOR = 4;
- 
- 
-unsigned int PersistentStorage::storage[ StorageSize ] __attribute__((aligned(32)));
-                
-                
-void PersistentStorage::readStorageFromFlash() {
-    HAL_FLASH_Unlock();
-    memcpy( (uint32_t*)storage, (uint32_t*)FlashMem[ USE_FLASH_SECTOR ].address, StorageSize );
-    HAL_FLASH_Lock();
-}
- 
- 
-bool PersistentStorage::writeStorageToFlash() {
-    HAL_FLASH_Unlock();
-    __HAL_FLASH_CLEAR_FLAG( FLASH_FLAG_EOP | FLASH_FLAG_OPERR |FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR );
-    FLASH_EraseInitTypeDef eraser;
-    eraser.TypeErase = TYPEERASE_SECTORS;
-    eraser.Banks = FLASH_BANK_1;
-    eraser.Sector = FlashMem[ USE_FLASH_SECTOR ].sector;
-    eraser.NbSectors = 1;
-    eraser.VoltageRange = VOLTAGE_RANGE_3;
-    uint32_t sectorerr = 0;
-    if( HAL_OK != HAL_FLASHEx_Erase( &eraser, &sectorerr ) || sectorerr != 0xFFFFFFFF ) {
-        HAL_FLASH_Lock();
-        return false;
-    }
-    for( int i = 0; i < StorageSize; ++i ) {
-        if( HAL_OK != HAL_FLASH_Program( TYPEPROGRAM_WORD, FlashMem[ USE_FLASH_SECTOR ].address + i * sizeof( storage[ i ] ), storage[ i ] ) ) {
-                HAL_FLASH_Lock();
-                return false;
-        }
-    }
-    HAL_FLASH_Lock();
-    return true;
-}*/
\ No newline at end of file

--- a/func/Persistor.h	Mon Dec 14 18:54:42 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,23 +0,0 @@
-#include "mbed.h"
-#include "stm32l4xx.h"
-#include "stm32l4xx_hal_flash.h"
-
-#ifndef __PERSISTOR_H__
-#define __PERSISTOR_H__
-
-class PersistentStorage {
-public:
-    static const int StorageSize = 64;
-    static unsigned int storage[ StorageSize ] __attribute__((aligned(32)));
- 
-    enum StorageOffsets { RandomSeed = 0 };
- 
-    static void readStorageFromFlash();
-    static bool writeStorageToFlash();
- 
-    static inline unsigned int get( int offs )      { return offs >= StorageSize ? 0x77777777 : storage[ offs ]; }
-    static inline void set( int offs, unsigned int x )  { offs < StorageSize ? storage[ offs ] = x : true; }
-private:
-    PersistentStorage();
-};
-#endif // __PERSISTOR_H__
\ No newline at end of file

--- a/func/RawData.cpp	Mon Dec 14 18:54:42 2015 +0000
+++ b/func/RawData.cpp	Tue Aug 23 15:59:59 2016 +0000
@@ -1,8 +1,26 @@
 #include "RawData.h"
 
 char buffer2[32];
-uint32_t pressure[11]={1000, 950, 850, 700, 500, 400, 300, 250, 200, 150 , 100};
-uint32_t altitude[11]={0, 700, 1500, 3000, 5500, 7000, 9000, 10000, 12000, 14000, 16000};
+const uint32_t __pressure_table[16]={1013, 995, 976, 959, 942, 925, 908, 892, 875, 859, 843, 812, 782, 752, 724, 696};
+const uint32_t __altitude_table[16]={0, 500, 1000, 1500, 2000, 2500, 3000, 3500, 4000, 4500, 5000, 6000, 7000, 8000, 9000, 10000};
+const uint8_t __dir_table[5] = {'E', 'N', 'W', 'S', 'E'}; 
+
+RawData::RawData()
+{
+    this->__temp_offset=0;
+    this->__humi_offset=0;
+    this->__pressure_offset=0;
+    this->__altitude_offset=0;
+    this->__acc_offset[0]=0;
+    this->__acc_offset[1]=0;
+    this->__acc_offset[2]=0;
+    this->__gyr_offset[0]=0;
+    this->__gyr_offset[1]=0;
+    this->__gyr_offset[2]=0;
+    this->__mag_offset[0]=0;
+    this->__mag_offset[1]=0;
+    this->__mag_offset[2]=0;
+}
 
 void RawData::str_date(char *buffer)
 {
@@ -16,52 +34,29 @@
 }
 void RawData::str_temperature(char *buffer)
 {
-    if(this->__temp>=0)
-        snprintf(buffer, 16, "T: %s", this->printDouble(buffer2, (this->__temp)));
+    if(this->calc_temperature()>=0)
+        snprintf(buffer, 16, "T: %sC", this->printDouble(buffer2, (this->calc_temperature())));
     else
-        snprintf(buffer, 16, "T:-%s", this->printDouble(buffer2, -1*(this->__temp)));
+        snprintf(buffer, 16, "T:-%sC", this->printDouble(buffer2, -1*(this->calc_temperature())));
 }
 void RawData::str_humidity(char *buffer)
 {
-    if(this->__humi<100)
-        snprintf(buffer, 16, "H: %s", this->printDouble(buffer2, (this->__humi)));
+    if(this->calc_humidity()<100)
+        snprintf(buffer, 16, "H: %sPer", this->printDouble(buffer2, (this->calc_humidity())));
     else
-        snprintf(buffer, 16, "H: 00.00");
-}
-void RawData::str_pressure(char *buffer)
-{
-    snprintf(buffer, 16, "P:%sK", this->printDouble(buffer2, (this->__pressure/10),1));
+        snprintf(buffer, 16, "H: 00.0Per");
 }
 void RawData::str_altitude(char *buffer)
 {
-    uint32_t p=this->__pressure;
-    int i=0;
-    while(p<pressure[i])
-    {
-        i++;
-        if(i>=11)
-            break;
-    }
-    uint32_t a;
-    if(i==0)
-        a=altitude[0];
-    else if(i>=11)
-        a=altitude[10];
-    else
-        a=(altitude[i]-altitude[i-1])/float(pressure[i]-pressure[i-1])*(p-pressure[i-1])+altitude[i-1];
-    snprintf(buffer, 16, "A:%6u", a);
-}
-void RawData::str_magnetic(char *buffer)
-{
-    float m=sqrt(float(this->__mag[0]*this->__mag[0]+this->__mag[1]*this->__mag[1]+this->__mag[2]*this->__mag[2]))/1000;
-    snprintf(buffer, 16, "M: %s", this->printDouble(buffer2, m, 3));
+    snprintf(buffer, 16, "A: %4uM", this->calc_altitude());
 }
 void RawData::str_direction(char *buffer)
 {
-    snprintf(buffer, 16, "D:100");
+    uint32_t dir = this->calc_cpassdir();
+    snprintf(buffer, 16, "D: %c%3uDeg", __dir_table[(dir+45)/90], dir);
 }
 
-char* RawData::printDouble(char* str, double v, int decimalDigits)
+char* RawData::printDouble(char* str, double v, int integerDigits, int decimalDigits)
 {
   int i = 1;
   int intPart, fractPart;
@@ -72,7 +67,14 @@
   for (;decimalDigits!=0; i*=10, decimalDigits--);
 
   /* calculate integer & fractinal parts */
-  intPart = (int)v;
+  int powval=1;
+  for(int xx=0; xx<integerDigits;xx++)
+    powval = powval*10;
+    
+  if(integerDigits>0)
+    intPart = (int)(v) % powval;
+  else
+    intPart = (int)(v);
   fractPart = (int)((v-(double)(int)v)*i);
 
   /* fill in integer part */
@@ -94,6 +96,62 @@
   return str;
 }
 
+
+float RawData::calc_temperature()
+{
+    return this->__temp+this->__temp_offset;
+}
+float RawData::calc_humidity()
+{
+    return this->__humi+this->__humi_offset;
+}
+int32_t RawData::calc_altitude()
+{
+    uint32_t altitude=0;
+    uint32_t bound_h=0, bound_l=16;
+    float pressure = (this->__prss+this->__pressure_offset)/10;
+    for(uint32_t i=0; i<16; i++)
+    {
+        if(__pressure_table[i]<= pressure)
+        {
+            bound_h=i;
+            break;
+        }
+        bound_l=i;
+    }
+    if(bound_h==bound_l){
+        this->__pressure_offset = __pressure_table[bound_l]-this->__prss;
+        altitude = __altitude_table[bound_l];
+    }else{
+        altitude = __altitude_table[bound_l]+(__altitude_table[bound_h]-__altitude_table[bound_l])/double(__pressure_table[bound_l]-__pressure_table[bound_h])*(__pressure_table[bound_l]-pressure);
+    }
+    int32_t a=int32_t(altitude+__altitude_offset);
+    return a;
+}
+int32_t RawData::calc_cpassdir()
+{
+    int32_t dir =0;
+    float x = this->__mag[0]+this->__mag_offset[0];
+    float y = this->__mag[1]+this->__mag_offset[1];
+    if(y==0){
+        if(x>=0)
+            dir = 0;
+        else
+            dir = 180;
+    }else if(y>0){
+        dir = uint32_t(90-(atan(x/y))*180/3.14);
+    }else{
+        dir = uint32_t(270-(atan(x/y))*180/3.14);
+    }
+    return dir;
+}
+
+
+
+
+
+
+
 void RawData::add_year_10()
 {
     struct tm *timeinfo = localtime (&(this->__time));
@@ -103,6 +161,7 @@
         year=timeinfo->tm_year-90;
     timeinfo->tm_year=year;
     this->__time=mktime(timeinfo);
+    set_time(this->__time);
 }
 void RawData::add_year_1()
 {
@@ -111,6 +170,7 @@
     if(timeinfo->tm_year%10==0)
         timeinfo->tm_year-=10;
     this->__time=mktime(timeinfo);
+    set_time(this->__time);
 }
 void RawData::add_month()
 {
@@ -121,6 +181,7 @@
         mon=0;
     timeinfo->tm_mon=mon;
     this->__time=mktime(timeinfo);
+    set_time(this->__time);
 }
 void RawData::add_day()
 {
@@ -154,6 +215,7 @@
     }
     timeinfo->tm_mday=day;
     this->__time=mktime(timeinfo);
+    set_time(this->__time);
 }
 void RawData::add_hour()
 {
@@ -164,6 +226,7 @@
         hour=0;
     timeinfo->tm_hour=hour;
     this->__time=mktime(timeinfo);
+    set_time(this->__time);
 }
 void RawData::add_min_10()
 {
@@ -174,6 +237,7 @@
         min-=60;
     timeinfo->tm_min=min;
     this->__time=mktime(timeinfo);
+    set_time(this->__time);
 }
 void RawData::add_min_1()
 {
@@ -184,6 +248,7 @@
         min-=10;
     timeinfo->tm_min=min;
     this->__time=mktime(timeinfo);
+    set_time(this->__time);
 }
 void RawData::add_sec_10()
 {
@@ -194,6 +259,7 @@
         sec-=60;
     timeinfo->tm_sec=sec;
     this->__time=mktime(timeinfo);
+    set_time(this->__time);
 }
 void RawData::add_sec_1()
 {
@@ -204,92 +270,100 @@
        sec-=10;
     timeinfo->tm_sec=sec;
     this->__time=mktime(timeinfo);
+    set_time(this->__time);
 }
 void RawData::add_temp_sign()
 {
-    this->__temp*=-1;
+    this->__temp_offset= -1*(this->calc_temperature())-this->__temp;
 }
 void RawData::add_temp_10()
 {
-    int val=int(this->__temp);
-    if(val>=0)
+    float val = this->__temp_offset;
+    if(this->__temp+val>=0)
         val+=10;
     else
         val-=10;
-    if(val>=100)
+    if(this->__temp+val>=100)
         val-=100;
-    else if(val<=-100)
+    else if(this->__temp+val<=-100)
         val+=100;
-    this->__temp=this->__temp+(val-int(this->__temp));
+    this->__temp_offset=val;
 }
 void RawData::add_temp_1()
 {
-    int val=int(this->__temp);
-    if(val>=0)
+    float val= this->__temp_offset;
+    if(this->__temp+val>=0)
         val+=1;
     else
         val-=1;
-    if(val>=0 && val%10==0)
+    if((this->__temp+val)>=0 && (int)(this->__temp+val)%10==0)
         val-=10;
-    else if(val<0 && val%10==0)
+    else if((this->__temp+val)<0 && (int)(this->__temp+val)%10==0)
         val+=10;
-    this->__temp=this->__temp+(val-int(this->__temp));
+    this->__temp_offset=val;
 }
 void RawData::add_temp_1_10()
 {
-    int val=(this->__temp*10);
-    if(val>=0)
+    float val=(this->__temp_offset*10);
+    if(this->__temp*10+val>=0)
         val+=1;
     else
         val-=1;
-    if(val>=0 && val%10==0)
+    if(this->__temp*10+val>=0 && (int)(this->__temp*10+val)%10==0)
         val-=10;
-    else if(val<0 && val%10==0)
+    else if(this->__temp*10+val<0 && (int)(this->__temp*10+val)%10==0)
         val+=10;
-    this->__temp=this->__temp+(val-int(this->__temp*10))/10.0;
-}
-void RawData::add_temp_1_100()
-{
-    int val=(this->__temp*100);
-    if(val>=0)
-        val+=1;
-    else
-        val-=1;
-    if(val>=0 && val%10==0)
-        val-=10;
-    else if(val<0 && val%10==0)
-        val+=10;
-    this->__temp=this->__temp+(val-int(this->__temp*100))/100.0;
+    this->__temp_offset=val/10.0;
 }
 void RawData::add_humi_10()
 {
-    int val=int(this->__humi);
+    float val=this->__humi_offset;
     val+=10;
-    if(val>=100)
+    if((int)(this->__humi+val)>=100)
         val-=100;
-    this->__humi=this->__humi+(val-int(this->__humi));
+    this->__humi_offset=val;
 }
 void RawData::add_humi_1()
 {
-    int val=int(this->__humi);
-    val+=1;
-    if(val%10==0)
-        val-=10;
-    this->__humi=this->__humi+(val-int(this->__humi));
+    float val=this->__humi_offset;
+    int delta = (int)(this->__humi_offset);
+    val = val-delta;
+    delta+=1;
+    if((int)(this->__humi+delta)%10==0)
+        delta-=10;
+    this->__humi_offset=val+delta;
 }
 void RawData::add_humi_1_10()
 {
-    int val=(this->__humi*10);
-    val+=1;
-    if(val%10==0)
-        val-=10;
-    this->__humi=this->__humi+(val-int(this->__humi*10))/10.0;
+    int delta=(int)(this->__humi_offset*10);
+    delta+=1;
+    if((int)(this->__humi*10+delta)%10==0)
+        delta-=10;
+    this->__humi_offset=delta/10.0;
+}
+
+// ALTITUDE
+void RawData::add_altitude_1000()
+{
+    this->__altitude_offset+=1000;
+    if(this->calc_altitude()>=10000)
+        this->__altitude_offset-=10000;
 }
-void RawData::add_humi_1_100()
+void RawData::add_altitude_100()
+{
+    this->__altitude_offset+=100;
+    if((this->calc_altitude()/100)%10==0)
+        this->__altitude_offset-=1000;
+}
+void RawData::add_altitude_10()
 {
-    int val=(this->__humi*100);
-    val+=1;
-    if(val%10==0)
-        val-=10;
-    this->__humi=this->__humi+(val-int(this->__humi*100))/100.0;
+    this->__altitude_offset+=10;
+    if((this->calc_altitude()/10)%10==0)
+       this->__altitude_offset-=100;
+}
+void RawData::add_altitude_1()
+{
+    this->__altitude_offset+=1;
+    if(this->calc_altitude()%10==0)
+       this->__altitude_offset-=10;
 }
\ No newline at end of file

--- a/func/RawData.h	Mon Dec 14 18:54:42 2015 +0000
+++ b/func/RawData.h	Tue Aug 23 15:59:59 2016 +0000
@@ -1,20 +1,28 @@
 #include "mbed.h"
 
-#ifndef __RAW_DATA_H__
-#define __RAW_DATA_H__
+#ifndef __MAS_RAW_DATA_H__
+#define __MAS_RAW_DATA_H__
 
 class RawData
 {
 public:
     time_t __time;
-    float __temp;
-    float __humi;
-    float __pressure;
-    int32_t __acc[3];
-    int32_t __gyr[3];
-    int32_t __mag[3];
+    float __temp;               // unit : Degree Celsius
+    float __humi;               // unit : %
+    float __prss;           // unit : kPa
+    int32_t __acc[3];           // unit : 
+    int32_t __gyr[3];           // unit :
+    int32_t __mag[3];           // unit :
+    float __temp_offset;        // unit : Degree Celsius
+    float __humi_offset;        // unit : %
+    float __pressure_offset;
+    float __altitude_offset;    // unit : m
+    int32_t __acc_offset[3];    // unit :
+    int32_t __gyr_offset[3];    // unit :
+    int32_t __mag_offset[3];    // unit :
+    
 public:
-    void str_date(char *buffer);
+    RawData();
     void add_year_10();
     void add_year_1();
     void add_month();
@@ -28,11 +36,19 @@
     void add_temp_10();
     void add_temp_1();
     void add_temp_1_10();
-    void add_temp_1_100();
     void add_humi_10();
     void add_humi_1();
     void add_humi_1_10();
-    void add_humi_1_100();
+    void add_altitude_1();
+    void add_altitude_10();
+    void add_altitude_100();
+    void add_altitude_1000();
+    time_t calc_datetime();
+    float calc_temperature();
+    float calc_humidity();
+    int32_t calc_altitude();
+    int32_t calc_cpassdir();
+    void str_date(char *buffer);
     void str_time(char *buffer);
     void str_temperature(char *buffer);
     void str_humidity(char *buffer);
@@ -40,9 +56,10 @@
     void str_altitude(char *buffer);
     void str_magnetic(char *buffer);
     void str_direction(char *buffer);
+    
 private:
-    char* printDouble(char* str, double v,int decimalDigits=2);
+    char* printDouble(char* str, double v,int integerDigits=2, int decimalDigits=1);
 };
 
 
-#endif // __RAW_DATA_H__
\ No newline at end of file
+#endif // __MAS_RAW_DATA_H__
\ No newline at end of file

--- a/main.cpp	Mon Dec 14 18:54:42 2015 +0000
+++ b/main.cpp	Tue Aug 23 15:59:59 2016 +0000
@@ -111,15 +111,15 @@
         {
             if(mybutton==1 and fbtn==BTN_RELEASED)
             {
-                if(fbtn_counter>0 and fbtn_counter<=10)
+                if(fbtn_counter>0 and fbtn_counter<=20)
                 {
                     fbtn=BTN_PRESSED;
                 }
-                else if(fbtn_counter>10 and fbtn_counter<=30)
+                else if(fbtn_counter>20 and fbtn_counter<=50)
                 {
                     fbtn=BTN_PRESSED_LONG;
                 }
-                else if(fbtn_counter>30)
+                else if(fbtn_counter>50)
                 {
                     fbtn=BTN_PRESSED_VERY_LONG;
                 }
@@ -196,13 +196,6 @@
 }
 printf("\n");
 */
-
-
-
-
-
-
-
   
 /*  
   printf("\r\n--- Starting new run ---\r\n");
@@ -216,7 +209,7 @@
   gyroscope->ReadID(&id);
   printf("LSM6DS0 accelerometer & gyroscope = 0x%X\r\n", id);
 */
-
+float pressure=0;
 while(1)
 {
     // MEASURE AND DISPLAY
@@ -236,23 +229,19 @@
             myrawdata.str_temperature(buffer0);
             myrawdata.str_humidity(buffer1);
         }
-        else if(func==FUNC_PRESSURE)
+        else if(func==FUNC_ALTICDIR)
         {
-            pressure_sensor->GetPressure(&myrawdata.__pressure);
-            myrawdata.str_pressure(buffer0);
-            myrawdata.str_altitude(buffer1);
-        }
-        else if(func==FUNC_CPASSDIR)
-        {
+            pressure_sensor->GetPressure(&pressure);
+            myrawdata.__prss = pressure * 10;
             magnetometer->Get_M_Axes(myrawdata.__mag);
             accelerometer->Get_X_Axes(myrawdata.__acc);
             gyroscope->Get_G_Axes(myrawdata.__gyr);
-            myrawdata.str_magnetic(buffer0);
+            myrawdata.str_altitude(buffer0);
             myrawdata.str_direction(buffer1);
         }
         mydisplay.show(buffer0, buffer1);
     }
-    else
+    else if(mode==MODE_CONFIG)
     {
         myled=1;
         if(func==FUNC_DATETIME)
@@ -326,42 +315,41 @@
                 }
                 else if(config_index==4)
                 {
-                    buffer0[7]=0xff;
+                    buffer1[3]=0xff;
                 }
                 else if(config_index==5)
                 {
-                    buffer1[3]=0xff;
+                    buffer1[4]=0xff;
                 }
                 else if(config_index==6)
                 {
-                    buffer1[4]=0xff;
-                }
-                else if(config_index==7)
-                {
                     buffer1[6]=0xff;
                 }
-                else if(config_index==8)
-                {
-                    buffer1[7]=0xff;
-                }
             }
         }
-        else if(func==FUNC_CPASSDIR)
+        else if(func==FUNC_ALTICDIR)
         {
-            mydisplay.show("CALIBRATION", "START IN 3s ...");
-            wait(1);
-            mydisplay.show("CALIBRATION", "START IN 3s ...");
-            wait(1);
-            mydisplay.show("CALIBRATION", "START IN 3s ...");
-            wait(1);
-            mydisplay.show("CALIBRATION", "PHASE 1 ...");
-            wait(3);
-            mydisplay.show("CALIBRATION", "PHASE 2 ...");
-            wait(3);
-            
-            mode=MODE_NORMAL;
-            myrawdata.str_magnetic(buffer0);
+            myrawdata.str_altitude(buffer0);
             myrawdata.str_direction(buffer1);
+            if(flag_flash)
+            {
+                if(config_index==0)
+                {
+                    buffer0[3]=0xff;
+                }
+                else if(config_index==1)
+                {
+                    buffer0[4]=0xff;
+                }
+                else if(config_index==2)
+                {
+                    buffer0[5]=0xff;
+                }
+                else if(config_index==3)
+                {
+                    buffer0[6]=0xff;
+                }
+            }
         }
         
         if(flag_flash)
@@ -374,22 +362,76 @@
         }
         mydisplay.show(buffer0, buffer1);
     }
+    else
+    {
+        mydisplay.show("CALIBRATION", "START IN 3s ...");
+        wait(1);
+        mydisplay.show("CALIBRATION", "START IN 2s ...");
+        wait(1);
+        mydisplay.show("CALIBRATION", "START IN 1s ...");
+        wait(1);
+        mydisplay.show("CALIBRATION", "PHASE 1 ...");
+        int32_t acc[3]={0}, gyr[3]={0};
+        for(int i=0;i<100;i++)
+        {
+            int32_t acc_tmp[3]={0}, gyr_tmp[3]={0};
+            accelerometer->Get_X_Axes(acc_tmp);
+            gyroscope->Get_G_Axes(gyr_tmp);
+            acc[0] += acc_tmp[0];
+            acc[1] += acc_tmp[1];
+            acc[2] += acc_tmp[2];
+            gyr[0] += gyr_tmp[0];
+            gyr[1] += gyr_tmp[1];
+            gyr[2] += gyr_tmp[2];
+            wait(0.02);
+        }
+        myrawdata.__gyr_offset[0]=-gyr[0]/100;
+        myrawdata.__gyr_offset[1]=-gyr[1]/100;
+        myrawdata.__gyr_offset[2]=-gyr[2]/100;
+        myrawdata.__acc_offset[0]=-acc[0]/100;
+        myrawdata.__acc_offset[1]=-acc[1]/100;
+        myrawdata.__acc_offset[2]=1-acc[2]/100;  // gravaty
+        
+        mydisplay.show("CALIBRATION", "PHASE 2 ...");
+        int32_t mag_max[3]={-10000}, mag_min[3]={10000};
+        for(int i=0;i<500;i++)
+        {
+            int32_t mag_tmp[3]={0};
+            magnetometer->Get_M_Axes(mag_tmp);
+            mag_max[0]=mag_tmp[0]>mag_max[0]?mag_tmp[0]:mag_max[0];
+            mag_max[1]=mag_tmp[1]>mag_max[1]?mag_tmp[1]:mag_max[1];
+            mag_max[2]=mag_tmp[2]>mag_max[2]?mag_tmp[2]:mag_max[2];
+            mag_min[0]=mag_tmp[0]<mag_min[0]?mag_tmp[0]:mag_min[0];
+            mag_min[1]=mag_tmp[1]<mag_min[1]?mag_tmp[1]:mag_min[1];
+            mag_min[2]=mag_tmp[2]<mag_min[2]?mag_tmp[2]:mag_min[2];
+            wait(0.02);
+        }
+        myrawdata.__mag_offset[0]=-((mag_max[0]+mag_min[0])/2);
+        myrawdata.__mag_offset[1]=-((mag_max[1]+mag_min[1])/2);
+        myrawdata.__mag_offset[2]=-((mag_max[2]+mag_min[2])/2);
+        
+        mode=MODE_NORMAL;
+    }
     
     // NEXT STATE
     if(mode==MODE_NORMAL)
     {
-        if(fbtn==BTN_PRESSED_LONG || fbtn==BTN_PRESSED_VERY_LONG)
+        if(fbtn==BTN_PRESSED_LONG)
         {
             mode=MODE_CONFIG;
         }
         else if(fbtn==BTN_PRESSED)
         {
             func=func+1;
-            if(func>FUNC_CPASSDIR)
+            if(func>FUNC_ALTICDIR)
                 func=FUNC_DATETIME;
         }
+        else if(fbtn==BTN_PRESSED_VERY_LONG)
+        {
+            mode=MODE_CONFIG_AUTO;
+        }
     }
-    else
+    else if(mode==MODE_CONFIG)
     {
         if(fbtn==BTN_PRESSED_VERY_LONG)
         {
@@ -399,9 +441,21 @@
         else if(fbtn==BTN_PRESSED_LONG)
         {
             config_index+=1;
-            if(config_index>8)
-            {
-                config_index=0;
+            if(func==FUNC_DATETIME){
+                if(config_index>8)
+                {
+                    config_index=0;
+                }
+            } else if(func== FUNC_TEMPHUMI){
+                if(config_index>6)
+                {
+                    config_index=0;
+                }
+            } else if(func==FUNC_ALTICDIR){
+                if(config_index>3)
+                {
+                    config_index=0;
+                }
             }
         }
         else if(fbtn==BTN_PRESSED)
@@ -445,7 +499,7 @@
                     myrawdata.add_sec_1();
                 }
             }
-            if(func==FUNC_TEMPHUMI)
+            else if(func==FUNC_TEMPHUMI)
             {
                 if(config_index==0)
                 {
@@ -465,31 +519,47 @@
                 }
                 else if(config_index==4)
                 {
-                    myrawdata.add_temp_1_100();
+                    myrawdata.add_humi_10();
                 }
                 else if(config_index==5)
                 {
-                    myrawdata.add_humi_10();
+                    myrawdata.add_humi_1();
                 }
                 else if(config_index==6)
                 {
-                    myrawdata.add_humi_1();
-                }
-                else if(config_index==7)
-                {
                     myrawdata.add_humi_1_10();
                 }
-                else if(config_index==8)
+            }
+            else if(func==FUNC_ALTICDIR)
+            {
+                if(config_index==0)
+                {
+                    myrawdata.add_altitude_1000();
+                }
+                else if(config_index==1)
                 {
-                    myrawdata.add_humi_1_100();
+                    myrawdata.add_altitude_100();
+                }
+                else if(config_index==2)
+                {
+                    myrawdata.add_altitude_10();
+                }
+                else if(config_index==3)
+                {
+                    myrawdata.add_altitude_1();
                 }
             }
         }
     }
+    else
+    {
+        ;
+    }
     
     wait_action();
 }
-    
+
+/*  
 raise_error(ERROR_CONFIG);
 
   printf("START CALIBRATION\n");
@@ -576,5 +646,6 @@
     printf("LSM6DS0 [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
     }
     wait(1.5);
-  }
+  }*/
+  
 }

--- a/main.h	Mon Dec 14 18:54:42 2015 +0000
+++ b/main.h	Tue Aug 23 15:59:59 2016 +0000
@@ -2,7 +2,6 @@
 #include "x_nucleo_iks01a1.h"
 #include "func/Error.h"
 #include "func/Config.h"
-#include "func/Persistor.h"
 #include "func/Display.h"
 #include "func/RawData.h"
 
@@ -18,14 +17,14 @@
 
 enum{
     MODE_NORMAL,
-    MODE_CONFIG
+    MODE_CONFIG,
+    MODE_CONFIG_AUTO
 };
 
 enum{
     FUNC_DATETIME,
     FUNC_TEMPHUMI,
-    FUNC_PRESSURE,
-    FUNC_CPASSDIR
+    FUNC_ALTICDIR
 };