EEPW论坛

L3G4200D模块

引脚定义：

测试场合，是需要四根线即可：

ESP8266             L3G4200D

============================

3.3V                VCC

GND                GND

D1                  SCL

D2                  SDA

============================

依旧使用Arduino测试，代码如下：

#include "Arduino.h"
#include <Wire.h>
#include <math.h>
#define L3G4200D_ADDRESS           (0xD2 >> 1)
#define L3G4200D_REG_WHO_AM_I      (0x0F)
#define L3G4200D_REG_CTRL_REG1     (0x20)
#define L3G4200D_REG_CTRL_REG2     (0x21)
#define L3G4200D_REG_CTRL_REG3     (0x22)
#define L3G4200D_REG_CTRL_REG4     (0x23)
#define L3G4200D_REG_CTRL_REG5     (0x24)
#define L3G4200D_REG_REFERENCE     (0x25)
#define L3G4200D_REG_OUT_TEMP      (0x26)
#define L3G4200D_REG_STATUS_REG    (0x27)
#define L3G4200D_REG_OUT_X_L       (0x28)
#define L3G4200D_REG_OUT_X_H       (0x29)
#define L3G4200D_REG_OUT_Y_L       (0x2A)
#define L3G4200D_REG_OUT_Y_H       (0x2B)
#define L3G4200D_REG_OUT_Z_L       (0x2C)
#define L3G4200D_REG_OUT_Z_H       (0x2D)
#define L3G4200D_REG_FIFO_CTRL_REG (0x2E)
#define L3G4200D_REG_FIFO_SRC_REG  (0x2F)
#define L3G4200D_REG_INT1_CFG      (0x30)
#define L3G4200D_REG_INT1_SRC      (0x31)
#define L3G4200D_REG_INT1_THS_XH   (0x32)
#define L3G4200D_REG_INT1_THS_XL   (0x33)
#define L3G4200D_REG_INT1_THS_YH   (0x34)
#define L3G4200D_REG_INT1_THS_YL   (0x35)
#define L3G4200D_REG_INT1_THS_ZH   (0x36)
#define L3G4200D_REG_INT1_THS_ZL   (0x37)
#define L3G4200D_REG_INT1_DURATION (0x38)
#ifndef VECTOR_STRUCT_H
#define VECTOR_STRUCT_H
struct Vector {
    float XAxis;
    float YAxis;
    float ZAxis;
};
#endif
typedef enum {
    L3G4200D_SCALE_2000DPS = 0b10,
    L3G4200D_SCALE_500DPS  = 0b01,
    L3G4200D_SCALE_250DPS  = 0b00
} l3g4200d_dps_t;
typedef enum {
    L3G4200D_DATARATE_800HZ_110  = 0b1111,
    L3G4200D_DATARATE_800HZ_50   = 0b1110,
    L3G4200D_DATARATE_800HZ_35   = 0b1101,
    L3G4200D_DATARATE_800HZ_30   = 0b1100,
    L3G4200D_DATARATE_400HZ_110  = 0b1011,
    L3G4200D_DATARATE_400HZ_50   = 0b1010,
    L3G4200D_DATARATE_400HZ_25   = 0b1001,
    L3G4200D_DATARATE_400HZ_20   = 0b1000,
    L3G4200D_DATARATE_200HZ_70   = 0b0111,
    L3G4200D_DATARATE_200HZ_50   = 0b0110,
    L3G4200D_DATARATE_200HZ_25   = 0b0101,
    L3G4200D_DATARATE_200HZ_12_5 = 0b0100,
    L3G4200D_DATARATE_100HZ_25   = 0b0001,
    L3G4200D_DATARATE_100HZ_12_5 = 0b0000
} l3g4200d_odrbw_t;
class L3G4200D {
    public:
bool begin(l3g4200d_dps_t scale = L3G4200D_SCALE_2000DPS, l3g4200d_odrbw_t odrbw = L3G4200D_DATARATE_100HZ_12_5);
l3g4200d_dps_t getScale(void);
l3g4200d_odrbw_t getOdrBw(void);
void calibrate(uint8_t samples = 50);
void setThreshold(uint8_t multiple = 1);
uint8_t getThreshold(void);
Vector readRaw(void);
Vector readNormalize();
uint8_t readTemperature(void);
    private:
Vector r;
Vector n;
Vector d;
Vector t;
bool useCalibrate;
float actualThreshold;
float dpsPerDigit;
float thresholdX;
float thresholdY;
float thresholdZ;
void writeRegister8(uint8_t reg, uint8_t value);
uint8_t readRegister8(uint8_t reg);
uint8_t fastRegister8(uint8_t reg);
};
bool L3G4200D::begin(l3g4200d_dps_t scale, l3g4200d_odrbw_t odrbw) {
    // Reset calibrate values
    d.XAxis = 0;
    d.YAxis = 0;
    d.ZAxis = 0;
    useCalibrate = false;
    // Reset threshold values
    t.XAxis = 0;
    t.YAxis = 0;
    t.ZAxis = 0;
    actualThreshold = 0;
    Wire.begin();
    // Check L3G4200D Who Am I Register
    if (fastRegister8(L3G4200D_REG_WHO_AM_I) != 0xD3) {
    return false;
    }
    // Enable all axis and setup normal mode + Output Data Range & Bandwidth
    uint8_t reg1 = 0x00;
    reg1 |= 0x0F; // Enable all axis and setup normal mode
    reg1 |= (odrbw << 4); // Set output data rate & bandwidh
    writeRegister8(L3G4200D_REG_CTRL_REG1, reg1);
    // Disable high pass filter
    writeRegister8(L3G4200D_REG_CTRL_REG2, 0x00);
    // Generata data ready interrupt on INT2
    writeRegister8(L3G4200D_REG_CTRL_REG3, 0x08);
    // Set full scale selection in continous mode
    writeRegister8(L3G4200D_REG_CTRL_REG4, scale << 4);
    switch(scale) {
      case L3G4200D_SCALE_250DPS:
          dpsPerDigit = .00875f;
          break;
      case L3G4200D_SCALE_500DPS:
          dpsPerDigit = .0175f;
          break;
      case L3G4200D_SCALE_2000DPS:
          dpsPerDigit = .07f;
          break;
      default:
          break;
    }
    // Boot in normal mode, disable FIFO, HPF disabled
    writeRegister8(L3G4200D_REG_CTRL_REG5, 0x00);
    return true;
}
// Get current scale
l3g4200d_dps_t L3G4200D::getScale(void) {
    return (l3g4200d_dps_t)((readRegister8(L3G4200D_REG_CTRL_REG4) >> 4) & 0x03);
}
// Get current output data range and bandwidth
l3g4200d_odrbw_t L3G4200D::getOdrBw(void) {
    return (l3g4200d_odrbw_t)((readRegister8(L3G4200D_REG_CTRL_REG1) >> 4) & 0x0F);
}
// Calibrate algorithm
void L3G4200D::calibrate(uint8_t samples) {
    // Set calibrate
    useCalibrate = true;
    // Reset values
    float sumX = 0;
    float sumY = 0;
    float sumZ = 0;
    float sigmaX = 0;
    float sigmaY = 0;
    float sigmaZ = 0;
    // Read n-samples
    for (uint8_t i = 0; i < samples; ++i) {
      readRaw();
      sumX += r.XAxis;
      sumY += r.YAxis;
      sumZ += r.ZAxis;
      sigmaX += r.XAxis * r.XAxis;
      sigmaY += r.YAxis * r.YAxis;
      sigmaZ += r.ZAxis * r.ZAxis;
      
      delay(5);
    }
    // Calculate delta vectors
    d.XAxis = sumX / samples;
    d.YAxis = sumY / samples;
    d.ZAxis = sumZ / samples;
    // Calculate threshold vectors
    thresholdX = sqrt((sigmaX / samples) - (d.XAxis * d.XAxis));
    thresholdY = sqrt((sigmaY / samples) - (d.YAxis * d.YAxis));
    thresholdZ = sqrt((sigmaZ / samples) - (d.ZAxis * d.ZAxis));
    // If already set threshold, recalculate threshold vectors
    if (actualThreshold > 0) {
    setThreshold(actualThreshold);
    }
}
// Get current threshold value
uint8_t L3G4200D::getThreshold(void) {
    return actualThreshold;
}
// Set treshold value
void L3G4200D::setThreshold(uint8_t multiple)  {
    if (multiple > 0) {
      // If not calibrated, need calibrate
      if (!useCalibrate) {
          calibrate();
      }
      // Calculate threshold vectors
      t.XAxis = thresholdX * multiple;
      t.YAxis = thresholdY * multiple;
      t.ZAxis = thresholdZ * multiple;
    } else {
      // No threshold
      t.XAxis = 0;
      t.YAxis = 0;
      t.ZAxis = 0;
    }
    // Remember old threshold value
    actualThreshold = multiple;
}
// Write 8-bit to register
void L3G4200D::writeRegister8(uint8_t reg, uint8_t value) {
    Wire.beginTransmission(L3G4200D_ADDRESS);
    Wire.write(reg);
    Wire.write(value);
    Wire.endTransmission();
}
// Fast read 8-bit from register
uint8_t L3G4200D::fastRegister8(uint8_t reg) {
    uint8_t value;
    Wire.beginTransmission(L3G4200D_ADDRESS);
  Wire.write(reg);
    Wire.endTransmission();
    Wire.beginTransmission(L3G4200D_ADDRESS);
    Wire.requestFrom(L3G4200D_ADDRESS, 1);
  value = Wire.read();
    Wire.endTransmission();
    return value;
}
// Read 8-bit from register
uint8_t L3G4200D::readRegister8(uint8_t reg) {
    uint8_t value;
    Wire.beginTransmission(L3G4200D_ADDRESS);
  Wire.write(reg);
    Wire.endTransmission();
    Wire.beginTransmission(L3G4200D_ADDRESS);
    Wire.requestFrom(L3G4200D_ADDRESS, 1);
    while(!Wire.available()) {};
  value = Wire.read();
    Wire.endTransmission();
    return value;
}
// L3G4200D Temperature sensor output change vs temperature: -1digit/degrCelsius (data representation: 2's complement).
// Value represents difference respect to a reference not specified value.
// So temperature sensor can be used to measure temperature variations: temperarture sensor isn't suitable to return absolute temperatures measures.
// If you run two sequential measures and differentiate them you can get temperature variation.
// This also means that two devices in the same temp conditions can return different outputs.
// Finally, you can use this info to compensate drifts due to temperature changes.
uint8_t L3G4200D::readTemperature(void) {
    return readRegister8(L3G4200D_REG_OUT_TEMP);
}
// Read raw values
Vector L3G4200D::readRaw() {
    Wire.beginTransmission(L3G4200D_ADDRESS);
  
  Wire.write(L3G4200D_REG_OUT_X_L | (1 << 7)); 
    Wire.endTransmission();
    Wire.requestFrom(L3G4200D_ADDRESS, 6);
    while (Wire.available() < 6);
    uint8_t xla = Wire.read();
    uint8_t xha = Wire.read();
    uint8_t yla = Wire.read();
    uint8_t yha = Wire.read();
    uint8_t zla = Wire.read();
    uint8_t zha = Wire.read();
    r.XAxis = xha << 8 | xla;
    r.YAxis = yha << 8 | yla;
    r.ZAxis = zha << 8 | zla;
    return r;
}
// Read normalized values
Vector L3G4200D::readNormalize() {
    readRaw();
    if (useCalibrate) {
      n.XAxis = (r.XAxis - d.XAxis) * dpsPerDigit;
      n.YAxis = (r.YAxis - d.YAxis) * dpsPerDigit;
      n.ZAxis = (r.ZAxis - d.ZAxis) * dpsPerDigit;
    } else {
      n.XAxis = r.XAxis * dpsPerDigit;
      n.YAxis = r.YAxis * dpsPerDigit;
      n.ZAxis = r.ZAxis * dpsPerDigit;
    }
    if (actualThreshold > 0)     {
      if (abs(n.XAxis) < t.XAxis) n.XAxis = 0;
      if (abs(n.YAxis) < t.YAxis) n.YAxis = 0;
      if (abs(n.ZAxis) < t.ZAxis) n.ZAxis = 0;
    }
    return n;
}
L3G4200D gyroscope;
void setup() {
  Serial.begin(115200);
  // Initialize L3G4200D(dps, odrbw)
  // dps:
  // L3G4200D_SCALE_250DPS:   200 dps
  // L3G4200D_SCALE_500DPS:   500 dps
  // L3G4200D_SCALE_2000DPS: 2000 dps (default)
  // odrbw:
  // L3G4200D_DATARATE_800HZ_50:   Output Data Rate 800HZ, Cut-off 50
  // L3G4200D_DATARATE_800HZ_35:   Output Data Rate 800HZ, Cut-off 35
  // L3G4200D_DATARATE_800HZ_30:   Output Data Rate 800HZ, Cut-off 30
  // L3G4200D_DATARATE_400HZ_110:  Output Data Rate 400HZ, Cut-off 110
  // L3G4200D_DATARATE_400HZ_50:   Output Data Rate 400HZ, Cut-off 50
  // L3G4200D_DATARATE_400HZ_25:   Output Data Rate 400HZ, Cut-off 25
  // L3G4200D_DATARATE_400HZ_20:   Output Data Rate 400HZ, Cut-off 20
  // L3G4200D_DATARATE_200HZ_70:   Output Data Rate 200HZ, Cut-off 70
  // L3G4200D_DATARATE_200HZ_50:   Output Data Rate 200HZ, Cut-off 50
  // L3G4200D_DATARATE_200HZ_25:   Output Data Rate 200HZ, Cut-off 25
  // L3G4200D_DATARATE_200HZ_12_5: Output Data Rate 200HZ, Cut-off 12.5
  // L3G4200D_DATARATE_100HZ_25:   Output Data Rate 100HZ, Cut-off 25
  // L3G4200D_DATARATE_100HZ_12_5: Output Data Rate 100HZ, Cut-off 12.5 (default)
  Serial.println("Initialize L3G4200D");
  while(!gyroscope.begin(L3G4200D_SCALE_2000DPS, L3G4200D_DATARATE_400HZ_50)) {
    Serial.println("Could not find a valid L3G4200D sensor, check wiring!");
    delay(500);
  }
  // Check selected scale
  Serial.print("Selected scale: ");
  switch(gyroscope.getScale())   {
    case L3G4200D_SCALE_250DPS:
      Serial.println("250 dps");
      break;
    case L3G4200D_SCALE_500DPS:
      Serial.println("500 dps");
      break;
    case L3G4200D_SCALE_2000DPS:
      Serial.println("2000 dps");
      break;
    default:
      Serial.println("unknown");
      break;
  }
  // Check Output Data Rate and Bandwidth
  Serial.print("Output Data Rate: ");
  switch(gyroscope.getOdrBw())   {
    case L3G4200D_DATARATE_800HZ_110:
      Serial.println("800HZ, Cut-off 110");
      break;
    case L3G4200D_DATARATE_800HZ_50:
      Serial.println("800HZ, Cut-off 50");
      break;
    case L3G4200D_DATARATE_800HZ_35:
      Serial.println("800HZ, Cut-off 35");
      break;
    case L3G4200D_DATARATE_800HZ_30:
      Serial.println("800HZ, Cut-off 30");
      break;
    case L3G4200D_DATARATE_400HZ_110:
      Serial.println("400HZ, Cut-off 110");
      break;
    case L3G4200D_DATARATE_400HZ_50:
      Serial.println("400HZ, Cut-off 50");
      break;
    case L3G4200D_DATARATE_400HZ_25:
      Serial.println("400HZ, Cut-off 25");
      break;
    case L3G4200D_DATARATE_400HZ_20:
      Serial.println("400HZ, Cut-off 20");
      break;
    case L3G4200D_DATARATE_200HZ_70:
      Serial.println("200HZ, Cut-off 70");
      break;
    case L3G4200D_DATARATE_200HZ_50:
      Serial.println("200HZ, Cut-off 50");
      break;
    case L3G4200D_DATARATE_200HZ_25:
      Serial.println("200HZ, Cut-off 25");
      break;
    case L3G4200D_DATARATE_200HZ_12_5:
      Serial.println("200HZ, Cut-off 12.5");
      break;
    case L3G4200D_DATARATE_100HZ_25:
      Serial.println("100HZ, Cut-off 25");
      break;
    case L3G4200D_DATARATE_100HZ_12_5:
      Serial.println("100HZ, Cut-off 12.5");
      break;
    default:
      Serial.println("unknown");
      break;
  }
  // Calibrate gyroscope. The calibration must be at rest.
  // If you don't want calibrate, comment this line.
  gyroscope.calibrate();
  // Set threshold sensivty. Default 3.
  // If you don't want use threshold, comment this line or set 0.
  gyroscope.setThreshold(3);
}
void loop() {
  // Read normalized values
  Vector raw = gyroscope.readRaw();
  // Read normalized values in deg/sec
  Vector norm = gyroscope.readNormalize();
  // Output raw
  Serial.print(" Xraw = ");
  Serial.print(raw.XAxis);
  Serial.print(" Yraw = ");
  Serial.print(raw.XAxis);
  Serial.print(" Zraw = ");
  Serial.print(raw.YAxis);
  // Output normalized
  Serial.print(" Xnorm = ");
  Serial.print(norm.XAxis);
  Serial.print(" Ynorm = ");
  Serial.print(norm.YAxis);
  Serial.print(" ZNorm = ");
  Serial.print(norm.ZAxis);
  Serial.println();
   delay(1000);
}

测试效果：