Diff: main.cpp

Revision:

23:9f1afcbd26f5

Parent:

22:bbdb7b928ccc

diff -r bbdb7b928ccc -r 9f1afcbd26f5 main.cpp
--- a/main.cpp	Mon May 27 11:50:00 2019 +0000
+++ b/main.cpp	Mon Jun 10 11:01:04 2019 +0000
@@ -43,6 +43,25 @@
 #define sampleFreqDef   10.0f
 
 Serial pc(USBTX, USBRX, 115200);
+uint8_t id;
+float value1, value2;
+char buffer1[32], buffer2[32];
+char* nmea;
+int32_t axes[3];
+int32_t mag[3];
+int32_t gyr[3];
+int32_t ac[3];
+float gyrf[3];
+float pitch, roll, yaw;
+uint32_t counter = 0;
+int32_t mag_max[3];
+int32_t mag_min[3]; // zmienne do przechwoywania "offsetu" magnetometru
+bool initialized = false;
+
+//enable a ticker that allows to  setup a recurring interrupt to repeatedly call a function at a specified rate.
+Ticker flipper; 
+
+Madgwick madgwick_filter;
 
 /* Instantiate the expansion board */
 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
@@ -90,10 +109,10 @@
   return str;
 }
 
+
 // NMEA conversion
-
 void float_to_char(float my_float, char* tab){
-        tab[0] = (char)( (int)(my_float/100.0) + 48);
+    tab[0] = (char)( (int)(my_float/100.0) + 48);
     tab[1] = (char)( (int)((my_float - (tab[0]- '0')*100)/10.0) + 48);
     tab[2] = (char)( (int)((my_float - (tab[0]- '0')*100 - (tab[1]- '0')*10 )) + 48);
     tab[4] = (char)( (int)((my_float - (tab[0]- '0')*100 - (tab[1]- '0')*10  - (tab[2]- '0'))*10.0) + 48);
@@ -101,7 +120,7 @@
 }
 
 char * get_nmea_sentence(float roll, float pitch, float yaw){
-    static char sentence[] = "$PASHR,hhmmss.sss,hhh.hh,T,rrr.rr,ppp.pp,xxx.xx,a.aaa,b.bbb,c.ccc,d,e*hh\n";
+    static char sentence[] = "$PASHR,hhmmss.sss,hhh.hh,T,rrr.rr,ppp.pp,xxx.xx,a.aaa,b.bbb,c.ccc,d,e*hh\n\r\n";
     //float roll_deg =    roll *  360/(2*3.14);
    // float pitch_deg =   pitch * 360/(2*3.14);
    // float yaw_deg =     yaw *  360/(2*3.14);
@@ -114,53 +133,39 @@
 }
 
 
-/* Simple main function */
-int main() {
-  uint8_t id;
-  float value1, value2;
-  char buffer1[32], buffer2[32];
-  char* nmea;
-  int32_t axes[3];
-  int32_t mag[3];
-  int32_t gyr[3];
-  int32_t ac[3];
-  float gyrf[3];
-  float pitch, roll, yaw;
-  // pc.baud(115200);
-  
-  
-  /* Enable all sensors */
-  hum_temp->enable();
-  press_temp->enable();
-  magnetometer->enable();
-  accelerometer->enable();
-  acc_gyro->enable_x();
-  acc_gyro->enable_g();
-  
-  //printf("\r\n--- Starting new run ---\r\n");
-
-  magnetometer->read_id(&id);
- // printf("LSM303AGR magnetometer            = 0x%X\r\n", id);
-  accelerometer->read_id(&id);
-//  printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
-  acc_gyro->read_id(&id);
- // printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
- 
- 
- Madgwick madgwick_filter;
- madgwick_filter.begin(sampleFreqDef); 
- 
- 
-  while(1) {
-    pc.printf("\r\n");
+void update_filter(){
+    magnetometer->get_m_axes(axes);
+    mag[0] = axes[0] + 15;
+    mag[1] = axes[1] + 180;
+    mag[2] = axes[2] + 51;
+    
+    /* 
+    if(initialized == false){
+        mag_max[0] = mag[0];
+        mag_max[1] = mag[1];
+        mag_max[2] = mag[2];
+        mag_min[0] = mag[0];
+        mag_min[1] = mag[1];
+        mag_min[2] = mag[2];
+        initialized = true;
+    }
     
-    magnetometer->get_m_axes(axes);
-    mag[0] = axes[0];
-    mag[1] = axes[1];
-    mag[2] = axes[2];
-  //  printf("LSM303AGR [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", mag[0], mag[1], mag[2]);
+      
+    for(int i=0; i<3; i++){
+        if(mag[i] > mag_max[i]){
+            mag_max[i] = mag[i];
+            }
+        if(mag[i] < mag_min[i]){
+            mag_min[i] = mag[i];
+            }
+        }
+    */
+        
     
-  //  printf("mag1 is at 0x%08X\r\n", &mag[0]); it's how to get an adress for use in stm studio
+    //pc.printf("LSM303AGR maximum axes values [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", mag_max[0], mag_max[1], mag_max[2]);
+    //pc.printf("LSM303AGR minimum axes values [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", mag_min[0], mag_min[1], mag_min[2]);
+    
+    //pc.printf("mag1 is at 0x%08X\r\n", &mag[0]); it's how to get an adress for use in stm studio
   
     accelerometer->get_x_axes(axes);
     ac[0] = axes[0];
@@ -189,11 +194,34 @@
     pitch = madgwick_filter.getPitch();
     roll = madgwick_filter.getRoll();
     yaw = madgwick_filter.getYaw();
+}
+
+
+int main() {
+  
+  /* Enable all sensors */
+  hum_temp->enable();
+  press_temp->enable();
+  magnetometer->enable();
+  accelerometer->enable();
+  acc_gyro->enable_x();
+  acc_gyro->enable_g();
+  
+
+  magnetometer->read_id(&id);
+ // printf("LSM303AGR magnetometer            = 0x%X\r\n", id);
+  accelerometer->read_id(&id);
+ // printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
+  acc_gyro->read_id(&id);
+ // printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
     
-   // printf(" Roll: %.2f Pitch: %.2f , Yaw: %.2f\n\r", roll, pitch, yaw);
-    nmea = get_nmea_sentence(roll,pitch,yaw);
-    pc.printf("%s",nmea);
-   // printf("Hehehehe\n\r");
-    wait(0.1);
-  }
+flipper.attach(&update_filter, 0.1);
+    
+    while(1){      
+    //nmea = get_nmea_sentence(roll,pitch,yaw);
+    //pc.printf("%s",nmea);
+    pc.printf("no elo");
+    wait_ms(500);
+    }
 }
+