Sensorauswertung


 
#include "Wire.h"
#include "I2Cdev.h"
#include "HMC5883L.h"
#include "MPU6050.h"

#define SRF_ADRESSE_ORG 0xE0
#define ENT_BYTE 0x02
int srf_adresse=SRF_ADRESSE_ORG>>1;

#define SDA_PIN  A4
#define SCL_PIN  A5

HMC5883L mag;
MPU6050 acc;
int16_t mx, my, mz;

float heightLPF = 100;

float headingOffset = 0;

void initSensoren(){
  Wire.begin();

  //int pullups
  pinMode(SDA_PIN,INPUT);
  pinMode(SCL_PIN,INPUT);
  digitalWrite(SDA_PIN,HIGH);
  digitalWrite(SCL_PIN,HIGH);

  acc.initialize();
  acc.setI2CBypassEnabled(1);
  mag.initialize();

  // verify connection
  Serial.println(mag.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed");
}

void leseSensoren(float* drehungIst, float* heightWert){
  *drehungIst = getHeading();
  *heightWert = getHeight();
}

float getHeight(){
  digitalWrite(13, HIGH);

  Wire.beginTransmission(srf_adresse);
  //Schreibe ins Befehlsregister:
  Wire.write(0x00);
  //Ausgabe in Zentimeter:
  Wire.write(0x51);
  Wire.endTransmission();
    
  delay(70);
  //Spreche Register 2 an:
  Wire.beginTransmission(srf_adresse);
  Wire.write(ENT_BYTE);
  Wire.endTransmission();
  
  //Lese zwei Bytes aus:
  Wire.requestFrom(srf_adresse,2);
  while (Wire.available() < 2);

  byte high_byte=Wire.read();
  byte low_byte=Wire.read();

  int entfernung = (high_byte<<8) | low_byte;

  //if(entfernung==0) entfernung = 0;

  digitalWrite(13, LOW);

   heightLPF = 0.7 * heightLPF + 0.3 * ((float) entfernung);
  
  return heightLPF;//heightLPF;
}

float getHeading() {
  // read raw heading measurements from device
  mag.getHeading(&mx, &my, &mz);

  // display tab-separated gyro x/y/z values

  // To calculate heading in degrees. 0 degree indicates North
  float heading = atan2(my, mx);

  heading -= headingOffset;

  while(heading < 0)
    heading += 2*PI;

  // Check for wrap due to addition of declination.
  while(heading > 2*PI)
    heading -= 2*PI;

  return heading;
}

void setHeadingOffset(float offset){headingOffset=offset;}