Diff: main.cpp

Revision:

0:8a2c63ece2a9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 10 11:17:09 2014 +0000
@@ -0,0 +1,304 @@
+#include "mbed.h"
+#include "math.h"
+#include "MODSERIAL.h"                           // MBED BUFFERED SERIAL HEADER
+#include "UM6_usart.h"                           // UM6 USART HEADER
+#include "UM6_config.h"                          // UM6 CONFIG HEADER
+
+#define PI 3.1415926
+#define acc_acu 8
+#define mag_acu 8
+
+
+
+//Serial ios(p28, p27);                            // Serielle Verbi. mit XBee über Pin: tx-28, rx-27
+Serial ios(USBTX, USBRX);                      // Serielle Verbi. über USB Port vom PC
+
+DigitalOut rst(p11);                             // Digital Reset für the XBee, 200ns für reset
+PwmOut x_kreisel(p21);                           // Pin21-PwmOut ist für Drehung um X-Achse
+PwmOut y_kreisel(p23);                           // Pin23-PwmOut ist für Drehung um Y-Achse
+PwmOut z_kreisel(p22);                           // Pin22-PwmOut ist für Drehung um Z-Achse
+
+DigitalOut uart_activity(LED1);                  // LED1 = UM6 SERIAL für Kommunikation
+
+//acc struct
+struct {
+        float x;
+        float y;
+        float z;
+       }raw_acc;
+
+struct {
+        float g;
+        float xy;
+        float zx;
+        float zy;
+        float x;
+        float y;
+        float z;
+       }norm_acc;
+
+struct {
+        float x;
+        float y;
+        float z;
+       }euler_acc_raw;
+
+struct {
+        double x;
+        double y;
+        double z;
+        char  x_acu;
+        char  z_acu;
+        char  y_acu;
+       }euler_acc;
+       
+struct {
+        double x;
+        double y;
+        double z;
+    
+       }euler_m;
+
+
+//mag struct
+struct {
+        float x;
+        float y;
+        float z;
+       }raw_mag;
+
+struct {
+        float g;
+        float xy;
+        float zx;
+        float zy;
+        float x;
+        float y;
+        float z;
+       }norm_mag;
+
+struct {
+        float x;
+        float y;
+        float z;
+       }euler_mag_raw;
+
+struct {
+        float x;
+        char  x_acu;
+        float y;
+        char  y_acu;
+        float z;
+        char  z_acu;
+        float bias_x;
+        float bias_y;
+        float bias_z;
+       }euler_mag;
+
+bool recive;
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// rxCallback // FUNKTION FÜR INTERRUPT
+
+void rxCallback(MODSERIAL_IRQ_INFO *q) 
+{
+    if (um6_uart.rxBufferGetCount() >=  MAX_PACKET_DATA) 
+    {
+        uart_activity = !uart_activity;          // LED leuchtet wenn RxBuff hat > 40 Bytes
+        Process_um6_packet();      
+    }
+}
+
+
+float deg_diff(float start_angle, float end_angle)
+{
+ float left;
+ 
+ left = end_angle - start_angle;
+
+ if (left >  180) {left = ((end_angle - 360)-start_angle);}
+ if (left < -180) {left = ((end_angle + 360)-start_angle);}
+ return left;
+}
+
+float rad_to_deg(float rad)
+{ return((rad) * (180.0 / PI));}
+
+// Acc Sensor
+
+int sgn(double x)
+{
+  if (x > 0.0L)
+    return 1.0L;
+  else if (x < 0.0L)  
+    return -1.0L;
+  else  
+    return 0.0L;
+}
+
+void make_acc_norm()
+{
+ norm_acc.g     = sqrt((data.Accel_Proc_X * data.Accel_Proc_X) + (data.Accel_Proc_Y * data.Accel_Proc_Y) + (data.Accel_Proc_Z * data.Accel_Proc_Z));   
+ norm_acc.xy    = sqrt((data.Accel_Proc_X * data.Accel_Proc_X) + (data.Accel_Proc_Y * data.Accel_Proc_Y));
+ norm_acc.zx    = sqrt((data.Accel_Proc_Z * data.Accel_Proc_Z) + (data.Accel_Proc_X * data.Accel_Proc_X));
+ norm_acc.zy    = sqrt((data.Accel_Proc_Z * data.Accel_Proc_Z) + (data.Accel_Proc_Y * data.Accel_Proc_Y));
+ 
+ norm_acc.x     = data.Accel_Proc_X / norm_acc.g;
+ norm_acc.y     = data.Accel_Proc_Y / norm_acc.g;
+ norm_acc.z     = data.Accel_Proc_Z / norm_acc.g;
+}
+
+char nicken_y_acc_raw()
+{   
+ if(norm_acc.z != 0)
+   {euler_acc_raw.y = atan(norm_acc.x / norm_acc.z);}
+  else {return (0);} 
+   
+ return (acc_acu * norm_acc.zx/norm_acc.g);
+}
+
+char rollen_x_acc_raw()
+{
+ if(norm_acc.z != 0)
+   {euler_acc_raw.x = atan(norm_acc.y / norm_acc.z);}
+  else {return (0);} 
+  
+ return (acc_acu * norm_acc.zy/norm_acc.g);
+}
+ 
+char drehen_z_acc_raw()
+{
+ if((norm_acc.y != 0)&&(norm_acc.xy != 0))
+   {euler_acc_raw.z = asin(norm_acc.y / norm_acc.xy);
+    if (sgn(norm_acc.x) < 0) { if (euler_acc_raw.z > 0) {euler_acc_raw.z = PI - euler_acc_raw.z ;} else {euler_acc_raw.z = -PI - euler_acc_raw.z ;} }
+   }
+  else {return (0);} 
+ 
+ if (euler_acc_raw.z < 0) {euler_acc_raw.z = 2 * PI + euler_acc_raw.z;}
+ 
+ return (acc_acu * norm_acc.xy/norm_acc.g);  
+}
+  
+void position_acc ()
+{
+ 
+ make_acc_norm();
+ 
+ euler_acc.x_acu = rollen_x_acc_raw();
+ euler_acc.y_acu = nicken_y_acc_raw();
+ euler_acc.z_acu = drehen_z_acc_raw();
+ 
+ euler_acc.x     = rad_to_deg(euler_acc_raw.x);
+ euler_acc.y     = rad_to_deg(euler_acc_raw.y);
+ euler_acc.z     = rad_to_deg(euler_acc_raw.z);
+}
+
+
+
+// Magnetometer
+
+void make_mag_norm()
+{
+ norm_mag.g     = sqrt((data.Mag_Proc_X * data.Mag_Proc_X) + (data.Mag_Proc_Y * data.Mag_Proc_Y) + (data.Mag_Proc_Z * data.Mag_Proc_Z));   
+ norm_mag.xy    = sqrt((data.Mag_Proc_X * data.Mag_Proc_X) + (data.Mag_Proc_Y * data.Mag_Proc_Y));
+ norm_mag.zx    = sqrt((data.Mag_Proc_Z * data.Mag_Proc_Z) + (data.Mag_Proc_X * data.Mag_Proc_X));
+ norm_mag.zy    = sqrt((data.Mag_Proc_Z * data.Mag_Proc_Z) + (data.Mag_Proc_Y * data.Mag_Proc_Y));
+ 
+ norm_mag.x     = data.Mag_Proc_X / norm_mag.g;
+ norm_mag.y     = data.Mag_Proc_Y / norm_mag.g;
+ norm_mag.z     = data.Mag_Proc_Z / norm_mag.g;
+}
+
+char nicken_y_mag_raw()
+{   
+ if(norm_mag.z != 0)
+   {euler_mag_raw.y = atan(norm_mag.x / norm_mag.z);}
+  else {return (0);} 
+   
+ return (acc_acu * norm_mag.zx/norm_mag.g);
+}
+
+char rollen_x_mag_raw()
+{
+ if(norm_mag.z != 0)
+   {euler_mag_raw.x = atan(norm_mag.y / norm_mag.z);}
+  else {return (0);} 
+  
+ return (acc_acu *norm_mag.zy/norm_mag.g);
+}
+ 
+char drehen_z_mag_raw()
+{
+ if((norm_mag.y != 0)&&(norm_mag.xy != 0))
+   {euler_mag_raw.z = asin(norm_mag.x / norm_mag.xy) * (norm_mag.y / abs(norm_mag.y));}
+  else {return (0);} 
+ 
+ if (euler_mag_raw.z < 0) {euler_mag_raw.z = 6.28 + euler_mag_raw.z;}
+ 
+ return (acc_acu *norm_mag.xy/norm_mag.g);  
+}
+  
+void position_mag ()
+{
+ 
+ make_mag_norm();
+ 
+ euler_mag.x_acu = rollen_x_mag_raw();
+ euler_mag.y_acu = nicken_y_mag_raw();
+ euler_mag.z_acu = drehen_z_mag_raw();
+ 
+ euler_mag.x     = euler_mag_raw.x - euler_mag.bias_x;
+ euler_mag.y     = euler_mag_raw.y - euler_mag.bias_y; 
+ euler_mag.z     = euler_mag_raw.z;
+}
+
+void print_data()
+{
+ios.printf("\n\r Data:   Acc          Mag");
+ios.printf("\n\r       x: %5.1f  %d  x: %5.1f  %d",euler_m.x,euler_acc.x_acu, rad_to_deg(euler_mag.x),euler_mag.x_acu);
+ios.printf("\n\r       y: %5.1f  %d  y: %5.1f  %d",euler_m.y,euler_acc.y_acu, rad_to_deg(euler_mag.y),euler_mag.y_acu);
+ios.printf("\n\r       z: %5.1f  %d  z: %5.1f  %d",euler_m.z,euler_acc.z_acu, rad_to_deg(euler_mag.z),euler_mag.z_acu);
+ios.printf("\n\r");
+}
+
+
+int main() {
+unsigned int count = 0;
+
+    ios.baud(115200);                                                // Baudrate XBee Funkmodul
+    um6_uart.baud(115200);                                           // Baudrate UM6-lt 
+    um6_uart.attach(&rxCallback, MODSERIAL::RxIrq);                  // Interrupt Funktion für UART
+
+    euler_m.x = 0.0;
+    euler_m.y = 0.0;
+    euler_m.z = 0.0;
+    euler_acc.x = 0.0;
+    euler_acc.y = 0.0;
+    euler_acc.z = 0.0;
+    
+    
+    while(1) {
+                if (recive) {
+                                recive = 0;
+                                count++;
+                                
+                                if ((count%2) == 1)
+                                {
+                                  position_acc();
+                                  position_mag();
+                                 
+                                 euler_m.x = euler_m.x + ((euler_acc.x - euler_m.x)/24);
+                                 euler_m.y = euler_m.y + ((euler_acc.y - euler_m.y)/24);
+                                 euler_m.z = euler_m.z + ((euler_acc.z - euler_m.z)/24);
+                                }
+                                
+                                if ((count%20) == 1)
+                                 {
+                                    //euler_m.x = euler_m.x/3;
+                                    print_data();
+                                 }                        
+                            
+                            }
+             }
+}
\ No newline at end of file