[아도이노 강좌] 기울기 보정 자기장 센서 (LSM303DLH,SEN0079) 사용해보기안녕하세요! 이번에는 DFRobot의 기울기 보정 자기장 센서 LSM3030DLH센서를 사용하는 방법에대해 알아보도록 하겠습니다.
구매링크 [http://www.mechasolution.com/shop/goods/goods_view.php?goodsno=2798]
SEN0079는 자기장 센서 모듈로써 내부에는 LSM303DLH칩을 내장하고 있습니다. LMS303DLH칩은 스마트폰에서도 사용되고있는 자기장센서 로도 유명합니다. LMS303DLH칩은 기존의 다른 자기장 센서와는 달리 자기장 센서만을 사용하지않고 가속도센서(기울기측정)가 내장되어 있어 자기장센서의 측정값을 보정할 수 있습니다.
우선 다음과 같이 배선을 해주세요.
소스코드는 DFRobot에서 제공하는 소스코드를 활용합니다.
/* LSM303DLH Example Code
LSM303 Breakout ---------- Arduino Vin 5V GND GND SDA A4 SCL A5 */ #include <Wire.h> #include <math.h>
#define SCALE 2 // accel full-scale, should be 2, 4, or 8
/* LSM303 Address definitions */ #define LSM303_MAG 0x1E // assuming SA0 grounded #define LSM303_ACC 0x18 // assuming SA0 grounded
#define X 0 #define Y 1 #define Z 2
/* LSM303 Register definitions */ #define CTRL_REG1_A 0x20 #define CTRL_REG2_A 0x21 #define CTRL_REG3_A 0x22 #define CTRL_REG4_A 0x23 #define CTRL_REG5_A 0x24 #define HP_FILTER_RESET_A 0x25 #define REFERENCE_A 0x26 #define STATUS_REG_A 0x27 #define OUT_X_L_A 0x28 #define OUT_X_H_A 0x29 #define OUT_Y_L_A 0x2A #define OUT_Y_H_A 0x2B #define OUT_Z_L_A 0x2C #define OUT_Z_H_A 0x2D #define INT1_CFG_A 0x30 #define INT1_SOURCE_A 0x31 #define INT1_THS_A 0x32 #define INT1_DURATION_A 0x33 #define CRA_REG_M 0x00 #define CRB_REG_M 0x01 #define MR_REG_M 0x02 #define OUT_X_H_M 0x03 #define OUT_X_L_M 0x04 #define OUT_Y_H_M 0x05 #define OUT_Y_L_M 0x06 #define OUT_Z_H_M 0x07 #define OUT_Z_L_M 0x08 #define SR_REG_M 0x09 #define IRA_REG_M 0x0A #define IRB_REG_M 0x0B #define IRC_REG_M 0x0C
/* Global variables */ int accel[3]; // we'll store the raw acceleration values here int mag[3]; // raw magnetometer values stored here float realAccel[3]; // calculated acceleration values here
void setup() { Serial.begin(9600); // Serial is used for debugging Wire.begin(); // Start up I2C, required for LSM303 communication initLSM303(SCALE); // Initialize the LSM303, using a SCALE full-scale range //lsm303에대한 초기설정.
}
void loop() { getLSM303_accel(accel); //가속도에 대한 데이터를 accel배열에 저장 while(!(LSM303_read(SR_REG_M) & 0x01)); //자기장에 대한데이터를 얻을 수 있을때까지 대기, getLSM303_mag(mag); //자기장에 대한 데이터를 mag배열에 저장 //printValues(mag, accel); // 원시값을 출력 디버깅을 하기위함
for (int i=0; i<3; i++) realAccel[i] = accel[i] / pow(2, 15) * SCALE; //가속도값 계산
float value = getTiltHeading(mag,realAccel); if(value<45){ Serial.println("N"); } else if(value<135){ Serial.println("E"); } else if(value<225){ Serial.println("S"); } else if(value<315){ Serial.println("W"); } else{ Serial.println("N"); }
delay(100); // delay for serial readability }
void initLSM303(int fs) { LSM303_write(0x27, CTRL_REG1_A); // 0x27 = normal power mode, all accel axes on if ((fs==8)||(fs==4)) LSM303_write((0x00 | (fs-fs/2-1)<<4), CTRL_REG4_A); // set full-scale else LSM303_write(0x00, CTRL_REG4_A); LSM303_write(0x14, CRA_REG_M); // 0x14 = mag 30Hz output rate LSM303_write(0x00, MR_REG_M); // 0x00 = continouous conversion mode }
void printValues(int * magArray, int * accelArray) { /* print out mag and accel arrays all pretty-like */ Serial.print(accelArray[X], DEC); Serial.print("\t"); Serial.print(accelArray[Y], DEC); Serial.print("\t"); Serial.print(accelArray[Z], DEC); Serial.print("\t\t");
Serial.print(magArray[X], DEC); Serial.print("\t"); Serial.print(magArray[Y], DEC); Serial.print("\t"); Serial.print(magArray[Z], DEC); Serial.println(); }
float getHeading(int * magValue) { // see section 1.2 in app note AN3192 float heading = 180*atan2(magValue[Y], magValue[X])/PI; // assume pitch, roll are 0
if (heading <0) heading += 360;
return heading; }
float getTiltHeading(int * magValue, float * accelValue) { // see appendix A in app note AN3192 float pitch = asin(-accelValue[X]); float roll = asin(accelValue[Y]/cos(pitch));
float xh = magValue[X] * cos(pitch) + magValue[Z] * sin(pitch); float yh = magValue[X] * sin(roll) * sin(pitch) + magValue[Y] * cos(roll) - magValue[Z] * sin(roll) * cos(pitch); float zh = -magValue[X] * cos(roll) * sin(pitch) + magValue[Y] * sin(roll) + magValue[Z] * cos(roll) * cos(pitch);
float heading = 180 * atan2(yh, xh)/PI; if (yh >= 0) return heading; else return (360 + heading); }
void getLSM303_mag(int * rawValues) { Wire.beginTransmission(LSM303_MAG); Wire.write(OUT_X_H_M); Wire.endTransmission(); Wire.requestFrom(LSM303_MAG, 6); for (int i=0; i<3; i++) rawValues[i] = (Wire.read() << 8) | Wire.read(); }
void getLSM303_accel(int * rawValues) { rawValues[Z] = ((int)LSM303_read(OUT_X_L_A) << 8) | (LSM303_read(OUT_X_H_A)); rawValues[X] = ((int)LSM303_read(OUT_Y_L_A) << 8) | (LSM303_read(OUT_Y_H_A)); rawValues[Y] = ((int)LSM303_read(OUT_Z_L_A) << 8) | (LSM303_read(OUT_Z_H_A)); // had to swap those to right the data with the proper axis }
byte LSM303_read(byte address) { byte temp;
if (address >= 0x20) Wire.beginTransmission(LSM303_ACC); else Wire.beginTransmission(LSM303_MAG);
Wire.write(address);
if (address >= 0x20) Wire.requestFrom(LSM303_ACC, 1); else Wire.requestFrom(LSM303_MAG, 1); while(!Wire.available()) ; temp = Wire.read(); Wire.endTransmission();
return temp; }
void LSM303_write(byte data, byte address) { if (address >= 0x20) Wire.beginTransmission(LSM303_ACC); else Wire.beginTransmission(LSM303_MAG);
Wire.write(address); Wire.write(data); Wire.endTransmission(); }
해당코드에대해 간략히 설명을 드리자면 자기장 센서에서 받은 방위각에따라 방위(동서남북,EWSN)으로 나누어 출력하도록 합니다. 그리고 방위각을 얻기위해서는 getTiltHeading과 getHeading 두개의 함수를 사용합니다. getHeading함수는 자기장센서만으로 센서의 방위각에 을 반환하지만, getTiltHeading함수는 자기장 센서의 값을 가속도 센서로 보정하여서 방위각을 출력하기때문에 더욱 올바른 값을 반환하게 됩니다.
읽어주셔서 감사합니다! 
 |
최신댓글