INFO: Dieses Forum nutzt Cookies...
Cookies sind für den Betrieb des Forums unverzichtbar. Mit der Nutzung des Forums erklärst Du dich damit einverstanden, dass wir Cookies verwenden.

Es wird in jedem Fall ein Cookie gesetzt um diesen Hinweis nicht mehr zu erhalten. Desweiteren setzen wir Google Adsense und Google Analytics ein.

Antwort schreiben 
 
Themabewertung:
  • 0 Bewertungen - 0 im Durchschnitt
  • 1
  • 2
  • 3
  • 4
  • 5
Multikopter
20.01.2015, 22:30
Beitrag #25
RE: Multikopter
Auch der Software-Teil soll nicht zu kurz kommen. Diesmal geht's um den MPU9150.
Eine Library dazu gibt's hier: https://github.com/sparkfun/MPU-9150_Breakout. Das Ding kann auch den DMP im MPU9150 benutzen. Damit bekommt man dann eine Sensor-Fusion des Gyros und des Beschleunigungssensors hin. (Das Magnetometer wird dabei nicht beachtet.)
Ich habe eins der Beispiele etwas vereinfacht und abgewandelt:
Code:
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

// #define DEBUG
#include "MPU6050_6Axis_MotionApps20.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu(0x69);

/* =========================================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.
* ========================================================================= */

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

unsigned long last = 0;

// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    Serial.begin(115200);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();
    
    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
        // other program behavior stuff here
        // .
        // .
        // .
        // if you are really paranoid you can frequently test in between other
        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
        // while() loop to immediately process the MPU data
        // .
        // .
        // .
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

      
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * -180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[2] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            
            int16_t gyro[3];
            mpu.dmpGetGyro(gyro,fifoBuffer);
            Serial.print("\t");
            Serial.print(gyro[2]);
            Serial.print("\t");
            Serial.print(gyro[0] * -1);
            Serial.print("\t");
            Serial.print(gyro[1]);
            
            unsigned long diff = millis() - last;
            last = millis();
            Serial.print("\t");
            Serial.println(diff);
    }
}
Das gibt mir jetzt die momentane Ausrichtung (Yaw, Pitch, Roll) und die momentane Drehrate um alle drei Achsen aus:
Code:
ypr    1.04    -5.71    8.42    3    -1    2    10
ypr    1.08    -5.75    8.45    2    -1    2    11
ypr    1.11    -5.79    8.47    3    -1    2    9
ypr    1.15    -5.83    8.51    2    -1    2    10
ypr    1.19    -5.87    8.53    3    -1    2    10
ypr    1.23    -5.91    8.56    2    -1    2    11
ypr    1.26    -5.95    8.59    2    -1    2    9
ypr    1.30    -6.00    8.62    2    -1    2    10
Die ersten drei Zahlen ist die Ausrichtung in Grad, danach kommen die Drehraten in Grad/Sekunde. Das hoffe ich zumindest...
Die letzte Zahl ist die Zeit in ms zwischen zwei "Messungen". D.h. ich erhalte die Information 100 mal pro Sekunde.

Gruß,
Thorsten

Falls ich mit einer Antwort helfen konnte, wuerde ich mich freuen, ein paar Fotos oder auch ein kleines Filmchen des zugehoerigen Projekts zu sehen.
Alle Beiträge dieses Benutzers finden
Diese Nachricht in einer Antwort zitieren
21.01.2015, 23:55
Beitrag #26
RE: Multikopter
Jetzt ist das Coding etwas übersichtlicher und auch schneller.
Code:
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h

#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

// #define DEBUG
#include "MPU6050_6Axis_MotionApps20.h"

#define DEBUG   // only "local"

#ifdef DEBUG
#define DEBUG_PRINT(x) Serial.print(x)
#define DEBUG_PRINTF(x, y) Serial.print(x, y)
#define DEBUG_PRINTLN(x) Serial.println(x)
#define DEBUG_PRINTLNF(x, y) Serial.println(x, y)
#else
#define DEBUG_PRINT(x)
#define DEBUG_PRINTF(x, y)
#define DEBUG_PRINTLN(x)
#define DEBUG_PRINTLNF(x, y)
#endif


class Gyro {
public:
  Gyro();
  boolean getData(float* ypr, int16_t* rates);  // 3 components each, returns true if there is data  
private:
  static volatile bool mpuInterrupt;
  static volatile unsigned long lastTime;
  static void dmpDataReady();  
  // class default I2C address is 0x68, but we have 0x69
  MPU6050 mpu;  
  bool dmpReady;  // set true if DMP init was successful
  uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
  uint16_t fifoCount;     // count of all bytes currently in FIFO
};  

Gyro* gyro;


void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();
  // speed up I2C
  TWBR = ((16000000L / 400000L) - 16) / 2;
  
  // initialize serial communication
  Serial.begin(115200);

  gyro = new Gyro();
};



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
  
  float ypr[3];
  int16_t rates[3];
   unsigned long start = micros();
   if(!gyro->getData(ypr, rates)) return;
   unsigned long stop = micros();
    Serial.print("ypr\t");
    Serial.print(ypr[0]);
    Serial.print("\t");
    Serial.print(ypr[1]);
    Serial.print("\t");
    Serial.print(ypr[2]);

    Serial.print("\t");
    Serial.print(rates[0]);
    Serial.print("\t");
    Serial.print(rates[1]);
    Serial.print("\t");
    Serial.print(rates[2]);
    Serial.print("\t");
    Serial.println(stop - start);
}


volatile bool Gyro::mpuInterrupt = false;
volatile unsigned long Gyro::lastTime = 0;

Gyro::Gyro() :  mpu(0x69)  {
  dmpReady = false;
  // initialize device
  mpu.initialize();
  // verify connection
  if(!mpu.testConnection()) {
    DEBUG_PRINTLN(F("MPU6050 connection failed"));
    return;  //dmpReady stays false
  };
  // load and configure the DMP
  // return status after each device operation (0 = success, !0 = error)
  uint8_t devStatus = mpu.dmpInitialize();
  // make sure it worked (returns 0 if so)
  if (devStatus != 0) {
    DEBUG_PRINT(F("DMP Initialization failed (code "));
    DEBUG_PRINT(devStatus);
    DEBUG_PRINTLN(F(")"));
    return;   // dmpReady stays empty
  };  
  // turn on the DMP, now that it's ready
  mpu.setDMPEnabled(true);
  // enable Arduino interrupt detection
  attachInterrupt(0, Gyro::dmpDataReady, RISING);
  // set our DMP Ready flag so the main loop() function knows it's okay to use it
  dmpReady = true;
  // get expected DMP packet size for later comparison
  packetSize = mpu.dmpGetFIFOPacketSize();
};


void Gyro::dmpDataReady(){
    mpuInterrupt = true;
    lastTime = millis();
}


boolean Gyro::getData(float* ypr, int16_t* gyro) {
  
     uint8_t fifoBuffer[64]; // FIFO storage buffer
   uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
   // orientation/motion vars
   Quaternion q;           // [w, x, y, z]         quaternion container
   VectorFloat gravity;    // [x, y, z]            gravity vector
   float ypr_raw[3];
   int16_t gyro_raw[3];

   // if programming failed, don't try to do anything
   if (!dmpReady) return false;

   // wait for MPU interrupt or extra packet(s) available
   if(!mpuInterrupt && fifoCount < packetSize) return false;

   // reset interrupt flag and get INT_STATUS byte
   mpuInterrupt = false;
   mpuIntStatus = mpu.getIntStatus();
   // get current FIFO count
   fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    DEBUG_PRINTLN(F("FIFO overflow!"));
    return false;
    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  }else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr_raw, &q, &gravity);
    ypr[0] = ypr_raw[0] * -180/M_PI;
    ypr[1] = ypr_raw[2] * 180/M_PI;
    ypr[2] = ypr_raw[1] * 180/M_PI;
    
    mpu.dmpGetGyro(gyro_raw, fifoBuffer);
    gyro[0] = gyro_raw[2];
    gyro[1] = gyro_raw[0] * -1;
    gyro[2] = gyro_raw[1];

    return true;
  };
  return false;
};
Mit dem vorherigen Coding hat das Auslesen des MPU9150, inklusive Rausrechnen der Schwerkraft und Umrechnen auf Grad ungefähr 8ms gedauert. Jetzt dauert es nur noch 3,5ms.
Der Witz dabei ist das hier:
Code:
TWBR = ((16000000L / 400000L) - 16) / 2;

Die 16000000 sind dabei die CPU-Frequenz und die 400000 die "gewünschte" I2C-Frequenz. Standard ist 100000.
Noch schöner wäre natürlich eine non-blocking I2C-Library.

Gruß,
Thorsten

Falls ich mit einer Antwort helfen konnte, wuerde ich mich freuen, ein paar Fotos oder auch ein kleines Filmchen des zugehoerigen Projekts zu sehen.
Alle Beiträge dieses Benutzers finden
Diese Nachricht in einer Antwort zitieren
Antwort schreiben 


Gehe zu:


Benutzer, die gerade dieses Thema anschauen: 1 Gast/Gäste