File content as of revision 0:8a2c63ece2a9:

#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();
                                 }                        
                            
                            }
             }
}