|
|
GY-511把LSM303的Vcc3.3, Gnd, SCL, SDA, INT1, INT2, DRDY引出，其中SCL, SDA利用Vin进行电压转换
我将Vcc3.3, Gnd, SCL, SDA与Arduino uno的相应端口连接， Vin连接Arduino板子上5V电源，INT1，INT2，DRDY悬空。
现在问题来了，我用wire向LSM303控制寄存器写数, 木反映， endTransimition返回2，难道还是电气连接有问题？
另外，不用示波器怎么才能知道Arduino板子TWI通信没问题。
以下代码：
————————————————————————————————————————————————————
#ifndef LSM303_h
#define LSM303_h
#include <Arduino.h> // for byte data type
// device types
#define LSM303DLH_DEVICE 0
#define LSM303DLM_DEVICE 1
#define LSM303DLHC_DEVICE 2
#define LSM303_DEVICE_AUTO 3
// SA0_A states
#define LSM303_SA0_A_LOW 0
#define LSM303_SA0_A_HIGH 1
#define LSM303_SA0_A_AUTO 2
// register addresses
#define LSM303_CTRL_REG1_A 0x20
#define LSM303_CTRL_REG2_A 0x21
#define LSM303_CTRL_REG3_A 0x22
#define LSM303_CTRL_REG4_A 0x23
#define LSM303_CTRL_REG5_A 0x24
#define LSM303_CTRL_REG6_A 0x25 // DLHC only
#define LSM303_HP_FILTER_RESET_A 0x25 // DLH, DLM only
#define LSM303_REFERENCE_A 0x26
#define LSM303_STATUS_REG_A 0x27
#define LSM303_OUT_X_L_A 0x28
#define LSM303_OUT_X_H_A 0x29
#define LSM303_OUT_Y_L_A 0x2A
#define LSM303_OUT_Y_H_A 0x2B
#define LSM303_OUT_Z_L_A 0x2C
#define LSM303_OUT_Z_H_A 0x2D
#define LSM303_FIFO_CTRL_REG_A 0x2E // DLHC only
#define LSM303_FIFO_SRC_REG_A 0x2F // DLHC only
#define LSM303_INT1_CFG_A 0x30
#define LSM303_INT1_SRC_A 0x31
#define LSM303_INT1_THS_A 0x32
#define LSM303_INT1_DURATION_A 0x33
#define LSM303_INT2_CFG_A 0x34
#define LSM303_INT2_SRC_A 0x35
#define LSM303_INT2_THS_A 0x36
#define LSM303_INT2_DURATION_A 0x37
#define LSM303_CLICK_CFG_A 0x38 // DLHC only
#define LSM303_CLICK_SRC_A 0x39 // DLHC only
#define LSM303_CLICK_THS_A 0x3A // DLHC only
#define LSM303_TIME_LIMIT_A 0x3B // DLHC only
#define LSM303_TIME_LATENCY_A 0x3C // DLHC only
#define LSM303_TIME_WINDOW_A 0x3D // DLHC only
#define LSM303_CRA_REG_M 0x00
#define LSM303_CRB_REG_M 0x01
#define LSM303_MR_REG_M 0x02
#define LSM303_OUT_X_H_M 0x03
#define LSM303_OUT_X_L_M 0x04
#define LSM303_OUT_Y_H_M -1 // The addresses of the Y and Z magnetometer output registers
#define LSM303_OUT_Y_L_M -2 // are reversed on the DLM and DLHC relative to the DLH.
#define LSM303_OUT_Z_H_M -3 // These four defines have dummy values so the library can
#define LSM303_OUT_Z_L_M -4 // determine the correct address based on the device type.
#define LSM303_SR_REG_M 0x09
#define LSM303_IRA_REG_M 0x0A
#define LSM303_IRB_REG_M 0x0B
#define LSM303_IRC_REG_M 0x0C
#define LSM303_WHO_AM_I_M 0x0F // DLM only
#define LSM303_TEMP_OUT_H_M 0x31 // DLHC only
#define LSM303_TEMP_OUT_L_M 0x32 // DLHC only
#define LSM303DLH_OUT_Y_H_M 0x05
#define LSM303DLH_OUT_Y_L_M 0x06
#define LSM303DLH_OUT_Z_H_M 0x07
#define LSM303DLH_OUT_Z_L_M 0x08
#define LSM303DLM_OUT_Z_H_M 0x05
#define LSM303DLM_OUT_Z_L_M 0x06
#define LSM303DLM_OUT_Y_H_M 0x07
#define LSM303DLM_OUT_Y_L_M 0x08
#define LSM303DLHC_OUT_Z_H_M 0x05
#define LSM303DLHC_OUT_Z_L_M 0x06
#define LSM303DLHC_OUT_Y_H_M 0x07
#define LSM303DLHC_OUT_Y_L_M 0x08
class LSM303
{
public:
typedef struct vector
{
float x, y, z;
} vector;
vector a; // accelerometer readings
vector m; // magnetometer readings
vector m_max; // maximum magnetometer values, used for calibration
vector m_min; // minimum magnetometer values, used for calibration
byte last_status; // status of last I2C transmission
// HEX = BIN RANGE GAIN X/Y/Z GAIN Z
// DLH (DLM/DLHC) DLH (DLM/DLHC)
// 0x20 = 0b00100000 ±1.3 1055 (1100) 950 (980) (default)
// 0x40 = 0b01000000 ±1.9 795 (855) 710 (760)
// 0x60 = 0b01100000 ±2.5 635 (670) 570 (600)
// 0x80 = 0b10000000 ±4.0 430 (450) 385 (400)
// 0xA0 = 0b10100000 ±4.7 375 (400) 335 (355)
// 0xC0 = 0b11000000 ±5.6 320 (330) 285 (295)
// 0xE0 = 0b11100000 ±8.1 230 (230) 205 (205)
enum magGain { magGain_13 = 0x20, magGain_19 = 0x40, magGain_25 = 0x60, magGain_40 = 0x80,
magGain_47 = 0xA0, magGain_56 = 0xC0, magGain_81 = 0xE0 };
LSM303(void);
void init(byte device = LSM303_DEVICE_AUTO, byte sa0_a = LSM303_SA0_A_AUTO);
byte getDeviceType(void) { return _device; }
void enableDefault(void);
void writeAccReg(byte reg, byte value);
byte readAccReg(byte reg);
void writeMagReg(byte reg, byte value);
byte readMagReg(int reg);
void setMagGain(magGain value);
void readAcc(void);
void readMag(void);
void read(void);
void setTimeout(unsigned int timeout);
unsigned int getTimeout(void);
bool timeoutOccurred(void);
int heading(void);
int heading(vector from);
// vector functions
static void vector_cross(const vector *a, const vector *b, vector *out);
static float vector_dot(const vector *a,const vector *b);
static void vector_normalize(vector *a);
private:
byte _device; // chip type (DLH, DLM, or DLHC)
byte acc_address;
unsigned int io_timeout;
bool did_timeout;
byte detectSA0_A(void);
};
#endif
————————————————————————————————————————————————————
#include "./LSM303.h"
#include <Wire.h>
#include <math.h>
// Defines ////////////////////////////////////////////////////////////////
// The Arduino two-wire interface uses a 7-bit number for the address,
// and sets the last bit correctly based on reads and writes
#define MAG_ADDRESS (0x3C >> 1)
#define ACC_ADDRESS_SA0_A_LOW (0x30 >> 1)
#define ACC_ADDRESS_SA0_A_HIGH (0x32 >> 1)
// Constructors ////////////////////////////////////////////////////////////////
LSM303: SM303(void)
{
// These are just some values for a particular unit; it is recommended that
// a calibration be done for your particular unit.
m_max.x = +540; m_max.y = +500; m_max.z = 180;
m_min.x = -520; m_min.y = -570; m_min.z = -770;
_device = LSM303_DEVICE_AUTO;
acc_address = ACC_ADDRESS_SA0_A_LOW;
io_timeout = 0; // 0 = no timeout
did_timeout = false;
}
// Public Methods //////////////////////////////////////////////////////////////
bool LSM303::timeoutOccurred()
{
return did_timeout;
}
void LSM303::setTimeout(unsigned int timeout)
{
io_timeout = timeout;
}
unsigned int LSM303::getTimeout()
{
return io_timeout;
}
void LSM303::init(byte device, byte sa0_a)
{
_device = device;
Serial.print("device");
Serial.print("sa0_a");
switch (_device)
{
case LSM303DLH_DEVICE:
case LSM303DLM_DEVICE:
if (sa0_a == LSM303_SA0_A_LOW)
acc_address = ACC_ADDRESS_SA0_A_LOW;
else if (sa0_a == LSM303_SA0_A_HIGH)
acc_address = ACC_ADDRESS_SA0_A_HIGH;
else
acc_address = (detectSA0_A() == LSM303_SA0_A_HIGH) ? ACC_ADDRESS_SA0_A_HIGH : ACC_ADDRESS_SA0_A_LOW;
break;
case LSM303DLHC_DEVICE:
acc_address = ACC_ADDRESS_SA0_A_HIGH;
break;
default:
Serial.print("Init here?");
// try to auto-detect device
if (detectSA0_A() == LSM303_SA0_A_HIGH)
{
// if device responds on 0011001b (SA0_A is high), assume DLHC
acc_address = ACC_ADDRESS_SA0_A_HIGH;
_device = LSM303DLHC_DEVICE;
Serial.print("Init here...1");
}
else
{
// otherwise, assume DLH or DLM (pulled low by default on Pololu boards); query magnetometer WHO_AM_I to differentiate these two
acc_address = ACC_ADDRESS_SA0_A_LOW;
_device = (readMagReg(LSM303_WHO_AM_I_M) == 0x3C) ? LSM303DLM_DEVICE : LSM303DLH_DEVICE;
Serial.print("Init here...2");
}
}
}
// Turns on the LSM303's accelerometer and magnetometers and places them in normal
// mode.
void LSM303::enableDefault(void)
{
// Enable Accelerometer
// 0x27 = 0b00100111
// Normal power mode, all axes enabled
writeAccReg(LSM303_CTRL_REG1_A, 0x27);
// Enable Magnetometer
// 0x00 = 0b00000000
// Continuous conversion mode
writeMagReg(LSM303_MR_REG_M, 0x00);
}
// Writes an accelerometer register
void LSM303::writeAccReg(byte reg, byte value)
{
Wire.beginTransmission(acc_address);
Wire.write(reg);
Wire.write(value);
last_status = Wire.endTransmission();
Serial.print("writeAdd=");
Serial.println(reg, HEX);
Serial.print("last_status = ");
Serial.println(last_status);
}
// Reads an accelerometer register
byte LSM303::readAccReg(byte reg)
{
byte value;
Wire.beginTransmission(acc_address);
Wire.write(reg);
last_status = Wire.endTransmission();
Wire.requestFrom(acc_address, (byte)1);
value = Wire.read();
Wire.endTransmission();
return value;
}
// Writes a magnetometer register
void LSM303::writeMagReg(byte reg, byte value)
{
Wire.beginTransmission(MAG_ADDRESS);
Wire.write(reg);
Wire.write(value);
last_status = Wire.endTransmission();
}
// Reads a magnetometer register
byte LSM303::readMagReg(int reg)
{
byte value;
// if dummy register address (magnetometer Y/Z), use device type to determine actual address
if (reg < 0)
{
switch (reg)
{
case LSM303_OUT_Y_H_M:
reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_H_M : LSM303DLM_OUT_Y_H_M;
break;
case LSM303_OUT_Y_L_M:
reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_L_M : LSM303DLM_OUT_Y_L_M;
break;
case LSM303_OUT_Z_H_M:
reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_H_M : LSM303DLM_OUT_Z_H_M;
break;
case LSM303_OUT_Z_L_M:
reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_L_M : LSM303DLM_OUT_Z_L_M;
break;
}
}
Wire.beginTransmission(MAG_ADDRESS);
Wire.write(reg);
last_status = Wire.endTransmission();
Wire.requestFrom(MAG_ADDRESS, 1);
value = Wire.read();
Wire.endTransmission();
return value;
}
void LSM303::setMagGain(magGain value)
{
Wire.beginTransmission(MAG_ADDRESS);
Wire.write(LSM303_CRB_REG_M);
Wire.write((byte) value);
Wire.endTransmission();
}
// Reads the 3 accelerometer channels and stores them in vector a
void LSM303::readAcc(void)
{
Wire.beginTransmission(acc_address);
// assert the MSB of the address to get the accelerometer
// to do slave-transmit subaddress updating.
Wire.write(LSM303_OUT_X_L_A | (1 << 7));
last_status = Wire.endTransmission();
Wire.requestFrom(acc_address, (byte)6);
unsigned int millis_start = millis();
did_timeout = false;
while (Wire.available() < 6) {
if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) {
did_timeout = true;
return;
}
}
byte xla = Wire.read();
byte xha = Wire.read();
byte yla = Wire.read();
byte yha = Wire.read();
byte zla = Wire.read();
byte zha = Wire.read();
// combine high and low bytes, then shift right to discard lowest 4 bits (which are meaningless)
// GCC performs an arithmetic right shift for signed negative numbers, but this code will not work
// if you port it to a compiler that does a logical right shift instead.
a.x = ((int16_t)(xha << 8 | xla)) >> 4;
a.y = ((int16_t)(yha << 8 | yla)) >> 4;
a.z = ((int16_t)(zha << 8 | zla)) >> 4;
}
// Reads the 3 magnetometer channels and stores them in vector m
void LSM303::readMag(void)
{
Wire.beginTransmission(MAG_ADDRESS);
Wire.write(LSM303_OUT_X_H_M);
last_status = Wire.endTransmission();
Wire.requestFrom(MAG_ADDRESS, 6);
unsigned int millis_start = millis();
did_timeout = false;
while (Wire.available() < 6) {
if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) {
did_timeout = true;
return;
}
}
byte xhm = Wire.read();
byte xlm = Wire.read();
byte yhm, ylm, zhm, zlm;
if (_device == LSM303DLH_DEVICE)
{
// DLH: register address for Y comes before Z
yhm = Wire.read();
ylm = Wire.read();
zhm = Wire.read();
zlm = Wire.read();
}
else
{
// DLM, DLHC: register address for Z comes before Y
zhm = Wire.read();
zlm = Wire.read();
yhm = Wire.read();
ylm = Wire.read();
}
// combine high and low bytes
m.x = (int16_t)(xhm << 8 | xlm);
m.y = (int16_t)(yhm << 8 | ylm);
m.z = (int16_t)(zhm << 8 | zlm);
}
// Reads all 6 channels of the LSM303 and stores them in the object variables
void LSM303::read(void)
{
readAcc();
readMag();
}
// Returns the number of degrees from the -Y axis that it
// is pointing.
int LSM303::heading(void)
{
return heading((vector){0,-1,0});
}
// Returns the number of degrees from the From vector projected into
// the horizontal plane is away from north.
//
// Description of heading algorithm:
// Shift and scale the magnetic reading based on calibration data to
// to find the North vector. Use the acceleration readings to
// determine the Down vector. The cross product of North and Down
// vectors is East. The vectors East and North form a basis for the
// horizontal plane. The From vector is projected into the horizontal
// plane and the angle between the projected vector and north is
// returned.
int LSM303::heading(vector from)
{
// shift and scale
m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
vector temp_a = a;
// normalize
vector_normalize(&temp_a);
//vector_normalize(&m);
// compute E and N
vector E;
vector N;
vector_cross(&m, &temp_a, &E);
vector_normalize(&E);
vector_cross(&temp_a, &E, &N);
// compute heading
int heading = round(atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI);
if (heading < 0) heading += 360;
return heading;
}
void LSM303::vector_cross(const vector *a,const vector *b, vector *out)
{
out->x = a->y*b->z - a->z*b->y;
out->y = a->z*b->x - a->x*b->z;
out->z = a->x*b->y - a->y*b->x;
}
float LSM303::vector_dot(const vector *a,const vector *b)
{
return a->x*b->x+a->y*b->y+a->z*b->z;
}
void LSM303::vector_normalize(vector *a)
{
float mag = sqrt(vector_dot(a,a));
a->x /= mag;
a->y /= mag;
a->z /= mag;
}
// Private Methods //////////////////////////////////////////////////////////////
byte LSM303::detectSA0_A(void)
{
Wire.beginTransmission(ACC_ADDRESS_SA0_A_LOW);
Wire.write(LSM303_CTRL_REG1_A);
last_status = Wire.endTransmission();
Serial.print("last_status="); ///////这里返回就是2了
//——————————————————————————
Serial.print(last_status);
Wire.requestFrom(ACC_ADDRESS_SA0_A_LOW, 1);
if (Wire.available())
{
Serial.print(" Wire.available");
Wire.read();
return LSM303_SA0_A_LOW;
}
else
{
Serial.print("Wire.unavailable");
return LSM303_SA0_A_HIGH;
}
}
————————————————————————————————————————————————————
#include <Wire.h>
#include "./LSM303.h"
LSM303 compass;
LSM303::vector running_min = {2047, 2047, 2047}, running_max = {-2048, -2048, -2048};
void setup() {
Serial.begin(9600);
Wire.begin();
compass.init();
compass.enableDefault();
Serial.print("setupover...");
}
void loop() {
Serial.print(".");
// compass.read();
// running_min.x = min(running_min.x, compass.m.x);
// running_min.y = min(running_min.y, compass.m.y);
// running_min.z = min(running_min.z, compass.m.z);
// running_max.x = max(running_max.x, compass.m.x);
// running_max.y = max(running_max.y, compass.m.y);
// running_max.z = max(running_max.z, compass.m.z);
// Serial.print("M min ");
// Serial.print("X: ");
// Serial.print((int)running_min.x);
// Serial.print(" Y: ");
// Serial.print((int)running_min.y);
// Serial.print(" Z: ");
// Serial.print((int)running_min.z);
// Serial.print(" M max ");
// Serial.print("X: ");
// Serial.print((int)running_max.x);
// Serial.print(" Y: ");
// Serial.print((int)running_max.y);
// Serial.print(" Z: ");
// Serial.println((int)running_max.z);
delay(1000);
} |
|
发表于 2015-4-12 22:19:46
楼主