this can make a serial acc data to be sent to a processing software

Dependencies:   mbed PinDetect LSM9DS1_works

main.cpp

Committer:
chebbi
Date:
2020-03-02
Revision:
3:64a8188c5a44
Parent:
0:e8167f37725c
Child:
4:7faa8147cabb

File content as of revision 3:64a8188c5a44:

#include "LSM9DS1.h"

//DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
#define PI 3,14 
#define printff_CALCULATED
#define printff_SPEED 250 
#define DECLINATION -8.58

void printfGyro();
void printfAccel();
void printfMag();
void printfAttitude(float ax, float ay, float az, float mx, float my, float mz);

LSM9DS1 lol(PC4, PC5,0xD6, 0x3C);
int main() {
    //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
    
    lol.begin();
    if (!lol.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
        while(1) ; 
    }
    lol.calibrate();
    while(1) {
        
        //lol.readTemp();
        
        if ( lol.magAvailable() )
        {
        lol.readMag();
        }
        if ( lol.accelAvailable() )
        {
        lol.readAccel();
        }
        if ( lol.gyroAvailable() )
        {
        lol.readGyro();
        }
        
        printfGyro();  // printfff "G: gx, gy, gz"
        pc.printf("\n") ;
        printfAccel(); // printfff "A: ax, ay, az"
        pc.printf("\n") ;
        printfMag();   // printfff "M: mx, my, mz"
        pc.printf("\n") ;
        // printff the heading and orientation for fun!
        // Call printfff attitude. The LSM9DS1's mag x and y
        // axes are opposite to the accelerometer, so my, mx are
        // substituted for each other.
        printfAttitude(lol.ax, lol.ay, lol.az,-lol.my, -lol.mx, lol.mz);
        pc.printf("\n") ;
        wait(1);
   } 
   }              
        
        void printfGyro()
     {
  // Now we can use the gx, gy, and gz variables as we please.
  // Either printfff them as raw ADC values, or calculated in DPS.
        pc.printf("G: ");
        #ifdef printff_CALCULATED 
  // If you want to printffff calculated values, you can use the
  // calcGyro helper function to convert a raw ADC value to
  // DPS. Give the function the value that you want to convert.
  pc.printf("%f" , lol.calcGyro(lol.gx), 2);
  pc.printf(", ");
  pc.printf("%f" , lol.calcGyro(lol.gy), 2);
  pc.printf(", ");
  pc.printf("%f" , lol.calcGyro(lol.gz), 2);
  pc.printf(" deg/s ");
  pc.printf("\n") ;
/*#elif defined printfff_RAW
  pc.printf(lol.gx);
  pc.printf(", ");
  pc.printf(lol.gy);
  pc.printf(", ");
  pc.printfln(lol.gz);*/
    #endif
}

void printfAccel()
{
  // Now we can use the ax, ay, and az variables as we please.
  // Either printfff them as raw ADC values, or calculated in g's.
  pc.printf("A: ");
#ifdef printff_CALCULATED
  // If you want to printffff calculated values, you can use the
  // calcAccel helper function to convert a raw ADC value to
  // g's. Give the function the value that you want to convert.
  pc.printf("%f" , lol.calcAccel(lol.ax), 2);
  pc.printf(", ");
  pc.printf("%f" , lol.calcAccel(lol.ay), 2);
  pc.printf(", ");
  pc.printf("%f" , lol.calcAccel(lol.az), 2);
  pc.printf(" g");
/*#elif defined printfff_RAW
  pc.printf(lol.ax);
  pc.printf(", ");
  pc.printf(lol.ay);
  pc.printf(", ");
  pc.printfln(lol.az);*/
  
#endif

}

void printfMag()
{
  // Now we can use the mx, my, and mz variables as we please.
  // Either printfff them as raw ADC values, or calculated in Gauss.
  pc.printf("M: ");
#ifdef printff_CALCULATED
  // If you want to printffff calculated values, you can use the
  // calcMag helper function to convert a raw ADC value to
  // Gauss. Give the function the value that you want to convert.
  pc.printf("%f" , lol.calcMag(lol.mx), 2);
  pc.printf(", ");
  pc.printf("%f" , lol.calcMag(lol.my), 2);
  pc.printf(", ");
  pc.printf("%f" , lol.calcMag(lol.mz), 2);
  pc.printf(" gauss");
  
/*#elif defined printfff_RAW
  pc.printf(lol.mx);
  pc.printf(", ");
  pc.printf(lol.my);
  pc.printf(", ");
  pc.printfln(lol.mz);*/
    #endif
}

void printfAttitude(float ax, float ay, float az, float mx, float my, float mz)
{
  float roll = atan2(ay, az);
  float pitch = atan2(-ax, sqrt(ay * ay + az * az));

  float heading;
  if (my == 0)
    heading = (mx < 0) ? PI : 0;
  else
    heading = atan2(mx, my);

  heading -= DECLINATION * PI / 180;

  if (heading > PI) heading -= (2 * PI);
  else if (heading < -PI) heading += (2 * PI);

  // Convert everything from radians to degrees:
  heading *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll  *= 180.0 / PI;

  pc.printf("Pitch, Roll: ");
  pc.printf("%f" , pitch, 2);
  pc.printf(", ");
  pc.printf("%f" , roll, 2);
  pc.printf("\n") ;
  pc.printf("Heading: "); pc.printf("%f" , heading, 2);
  
}
        
        //pc.printffff("%d %d %d %d %d %d %d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.ax, lol.ay, lol.az, lol.mx, lol.my, lol.mz);
        //pc.printffff("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz));
        //pc.printffff("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz);
        //pc.printffff("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az);
        //pc.printffff("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz);
        //myled = 1;
        //wait_ms(500);
        //myled = 0;
        //wait_ms(500);