STM32f3discoveryにはMEMSCompass_LSM303DLHCが搭載されている。
LSM303DLHCは3軸重力加速度センサと3軸磁気センサである。
LSM303DLHCを利用すると，STM32f3discoveryの姿勢を測定できる。ここではこのセンサの利用について述べる。
一般に飛行機のような物体の姿勢は機首方向角heading，機首上げ角pitch，左右振れ角rollで表される。
方向角headingには様々な定義があるが，北を0度にして東：90度，南180度，西：270度のように時計回りに定義する。（この定義はよく用いられているが，左手系になってしまい，多くの数理系で用いる右手系とは異なっている。）
機首上げ角pitchは通常-90度から90度間で定義され，機首上げ方向が＋である。
左右振れ角rollはpitchの値にかかわらず，進行方向に対して右翼を下げる方向が＋である。
図は真後ろから飛行機を見たところである。
最初にheading角を決め，pitch分機首を上げ，最後にroll分右翼を下げる順で姿勢を決めることとする。
（この順を間違えると違った定義になってしまう。）
STM32f3discoveryに搭載されたSM303DLHCのセンサ軸はマニュアルDM00027543.pdfによれば，次の図のようになっている。
ジャイロセンサL3GD20のセンサ軸とは異なるので注意。（設計が悪い）
「STM32f3discovery」の文字が読める向きに飛行機の機首があると考える。
ACCセンサ（重力加速度センサ）では，3軸の出力をそれぞれ，Accx, Accy, Acczとしたとき，次のように検出される。
カードを水平にした時，
Accx = Accy = 0， Accz = G
カードを水平にして，次に機首をα上げた時（Pitch=α），
Accx = - G sinα， Accy = 0， Accz = G cosα
カードを水平にして，次に機首を90度上げた時（Pitch=90度），
Accx = - G， Accy = 0， Accz = 0
カードを水平にして，次に右翼をβ下げた時（Roll=β），
Accx = 0， Accy = - G sinβ， Accz= G cosβ
カードを水平にして，次に右翼を90度下げた時（Roll=90度），
Accx = 0， Accy = - G， Accz= 0
すなわち，軸方向と逆向きの重力加速をを受けた時＋の値として検出している。
このことより，逆にAccx, Accy, Acczより，Pitch角とRoll角を求める。
◯最初はカードを水平にしたところから考える。このｘｙｚ座標のｘｙ平面は地面と平行で固定されているとする。
ｘ軸の負の方向が機首方向となる。
◯次に機首をαだけ上げると次のようになる。X軸センサとY軸センサが値を持つ。
この後rollの姿勢変化をしてもPitch角度は変化しないため，Pitch角αは
sinα=-Accx/G
で求められる。ただしGは
G = (Accx2 + Accy2 + Accz2)0.5
で与えられる。
◯上記の状態を ｙ-z' 平面で見ると次のようになる。rollの姿勢変化はこの ｙ-z' 平面内で生ずる。
◯機体を右に傾けて，右翼をβ下げるrollの姿勢変化をすると，次のようになる。
roll角βは
tanβ = - Accy/Accz
で求められる。
３．２ 方位角の求め方
Magセンサ（磁気センサ）では，3軸の出力をそれぞれ，Magx, Magy, Magzとしたとき，次のように検出される。
（ただし，俯角=φ，すなわち地磁気の北が地面に対しφ沈んでいるとする。）
カードを水平にし，機首を北に向けた時，
Magx = - M sinβ， Magy = 0，Magz = - M cosφ
カードを水平にし，機首を北に向け，機首をφ下げた時，
Magx = - M， Magy = 0， Magz = 0
カードを水平にし，右翼を北に向けた時，
Magx = 0，Magy = M sinβ， Magz = - M cosφ
カードを水平にし，右翼を北に向け，右翼をφ下げた時，
Magx = 0， Magy = M， Magz = 0
◯最初はカードを水平にし，機首を北西に向けたところから考える。
Magy＞0，Magx＜0になっている。
磁力の方向は北方向で50度程度の俯角を持っている。（東京付近）
MagyとMagxで測定されたMyとMxの成分の大きさから北方向が推定できる。
◯次に北西を向いたまま，ｙ軸を回転中心としたpitch角を与えた場合を考えよう。磁極方向は変化していないが，x'-y-z' 座標系ではその成分が変化しているようにみえる。
◯北西を向き，pitch角を与えた状態で，さらにｘ’軸を回転中心としたroll角を与えた場合を考えよう。磁極方向は変化していないが，x'-y'-z' '座標系ではその成分が変化しているようにみえる。
すなわち，機体がpitch角とroll角を保つ場合，Magセンサ（磁気センサ）では，3軸方向の成分が図のように観測される。
機体を水平方向に戻すには，ここまでの逆順の座標変換を行えば良い。
右手系の座標系では，各軸周りの回転は回転変換マトリクス演算で表される。連続回転変換マトリクスは回転変換マトリクスの積で表される。回転の向きは軸の方向に右ねじが進む回転方向が+である。
x軸周りの回転変換マトリクス
y軸周りの回転変換マトリクス
先にｘ軸周りに回転し，次にｙ軸周りに回転した時の回転変換マトリクス
回転方向に注意して
θx = - Roll， θy = Pitch
でMagx,Magy,Magxを変換すると，Mx,My,Mzが求められる。（Mzは不要）
Mx = cosθy Magx + sinθy sinθx Magy + sinθy cosθx Magz
My = cosθx Magy - sinθx Magz
STM32F3-Discovery_FW_V1.1.0内のProject\Demonstrationにサンプルプログラムがあるが，roll角とheading角の求め方に問題があるようだ。
STM32F303VCT6とSM303DLHCの間のデータのやり取りはI2Cを利用していて，これらの関数は信頼できそうなので，そのまま使うことにする。
そのまま使うことにした関数
void initCompass(void)
{
LSM303DLHCMag_InitTypeDef LSM303DLHC_InitStructure;
LSM303DLHCAcc_InitTypeDef LSM303DLHCAcc_InitStructure;
LSM303DLHCAcc_FilterConfigTypeDef LSM303DLHCFilter_InitStructure;
/* Configure MEMS magnetometer main parameters: temp, working mode, full Scale and Data rate */
LSM303DLHC_InitStructure.Temperature_Sensor = LSM303DLHC_TEMPSENSOR_DISABLE;
LSM303DLHC_InitStructure.MagOutput_DataRate =LSM303DLHC_ODR_30_HZ ;
LSM303DLHC_InitStructure.MagFull_Scale = LSM303DLHC_FS_8_1_GA;
LSM303DLHC_InitStructure.Working_Mode = LSM303DLHC_CONTINUOS_CONVERSION;
LSM303DLHC_MagInit(&LSM303DLHC_InitStructure);
/* Fill the accelerometer structure */
LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_NORMAL_MODE;
LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_50_HZ;
LSM303DLHCAcc_InitStructure.Axes_Enable= LSM303DLHC_AXES_ENABLE;
LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB;
LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE;
/* Configure the accelerometer main parameters */
LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
/* Fill the accelerometer LPF structure */
LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection =LSM303DLHC_HPM_NORMAL_MODE;
LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency = LSM303DLHC_HPFCF_16;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1 = LSM303DLHC_HPF_AOI1_DISABLE;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2 = LSM303DLHC_HPF_AOI2_DISABLE;
/* Configure the accelerometer LPF main parameters */
LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
}
void readCompass_Acc(float* pfData)
{
#define LSM_Acc_Sensitivity_2g (float) 1.0f /*!< accelerometer sensitivity with 2 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_4g (float) 0.5f /*!< accelerometer sensitivity with 4 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_8g (float) 0.25f /*!< accelerometer sensitivity with 8 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_16g (float) 0.0834f /*!< accelerometer sensitivity with 12 g full scale [LSB/mg] */
int16_t pnRawData[3];
uint8_t ctrlx[2];
uint8_t buffer[6], cDivider;
uint8_t i = 0;
float LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
/* Read the register content */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, buffer, 6);
if(ctrlx[1]&0x40)
cDivider=64;
else
cDivider=16;
/* check in the control register4 the data alignment*/
if(!(ctrlx[0] & 0x40) || (ctrlx[1] & 0x40)) /* Little Endian Mode or FIFO mode */
{
for(i=0; i<3; i++)
{
pnRawData[i]=((int16_t)((uint16_t)buffer[2*i+1] << 8) + buffer[2*i])/cDivider;
}
}
else /* Big Endian Mode */
{
for(i=0; i<3; i++)
pnRawData[i]=((int16_t)((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])/cDivider;
}
/* Read the register content */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
if(ctrlx[1]&0x40)
{
/* FIFO mode */
LSM_Acc_Sensitivity = 0.25;
}
else
{
/* normal mode */
/* switch the sensitivity value set in the CRTL4*/
switch(ctrlx[0] & 0x30)
{
case LSM303DLHC_FULLSCALE_2G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
break;
case LSM303DLHC_FULLSCALE_4G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_4g;
break;
case LSM303DLHC_FULLSCALE_8G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_8g;
break;
case LSM303DLHC_FULLSCALE_16G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_16g;
break;
}
}
/* Obtain the mg value for the three axis */
for(i=0; i<3; i++)
{
pfData[i]=(float)pnRawData[i]/LSM_Acc_Sensitivity;
}
}
void readCompass_Mag(float* pfData)
{
static uint8_t buffer[6] = {0};
uint8_t CTRLB = 0;
uint16_t Magn_Sensitivity_XY = 0, Magn_Sensitivity_Z = 0;
uint8_t i =0;
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_CRB_REG_M, &CTRLB, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, buffer, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, buffer+1, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, buffer+2, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, buffer+3, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, buffer+4, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, buffer+5, 1);
/* Switch the sensitivity set in the CRTLB*/
switch(CTRLB & 0xE0)
{
case LSM303DLHC_FS_1_3_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_3Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_3Ga;
break;
case LSM303DLHC_FS_1_9_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_9Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_9Ga;
break;
case LSM303DLHC_FS_2_5_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_2_5Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_2_5Ga;
break;
case LSM303DLHC_FS_4_0_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4Ga;
break;
case LSM303DLHC_FS_4_7_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4_7Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4_7Ga;
break;
case LSM303DLHC_FS_5_6_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_5_6Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_5_6Ga;
break;
case LSM303DLHC_FS_8_1_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_8_1Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_8_1Ga;
break;
}
for(i=0; i<2; i++)
{
pfData[i]=(float)((int16_t)(((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])*1000)/Magn_Sensitivity_XY;
}
pfData[2]=(float)((int16_t)(((uint16_t)buffer[4] << 8) + buffer[5])*1000)/Magn_Sensitivity_Z;
}
uint32_t LSM303DLHC_TIMEOUT_UserCallback(void)
{
return 0;
}
３軸加速度センサ出力はSTM32f3discoveryの姿勢を変えることで，加速度センサの３つの出力が＋GからーGまで変化するはずで，ゲインも同 じでオフセットも無いはずである。ところが，実際に測定してみると，３つのセンサ出力値の最大値・最小値はばらついている。そこで，最大値最小値を測定 し，較正することにする。
３軸加速度センサ出力の最大値・最小値を求める（semihosting使用）
浮動小数点数を表示するためlib/stdio/printf.cをプロジェクトから削除してGCCのｐrintfを使う
#include <stm32f30x.h>
#include <stdint.h>
#include "stm32f30x_conf.h"
#include "stm32f3_discovery.h"
#include <stdio.h>
#include <math.h>
#include "stm32f3_discovery_lsm303dlhc.h"
void initCompass(void);
void readCompass_Acc(float* pfData);
void testCompass_Acc(void);
int main()
{
initCompass();
while (1) {
testCompass_Acc();
}
}
void testCompass_Acc(void)
{
float AccBuffer[3];
static float b0min=0,b0max=0,b1min=0,b1max=0,b2min=0,b2max=0;
readCompass_Acc(AccBuffer);
if(AccBuffer[0]<b0min) b0min=AccBuffer[0];
if(b0max<AccBuffer[0]) b0max=AccBuffer[0];
if(AccBuffer[1]<b1min) b1min=AccBuffer[1];
if(b1max<AccBuffer[1]) b1max=AccBuffer[1];
if(AccBuffer[2]<b2min) b2min=AccBuffer[2];
if(b2max<AccBuffer[2]) b2max=AccBuffer[2];
printf("acc (%7.2f %7.2f %7.2f) (%7.2f %7.2f %7.2f) (%7.2f %7.2f %7.2f)\n",
AccBuffer[0],b0min,b0max,AccBuffer[1],b1min,b1max,AccBuffer[2],b2min,b2max);
}
/*この後ろに，次の２つの関数を置く
void initCompass(void);
void readCompass_Acc(float* pfData);
*/
STM32f3discoveryの姿勢を変化させながら最大，最小を求めながらの実行結果
AccBuffer[0] AccBuffer[1] AccBuffer[2]
min max min max min max
途中経過省略
acc (-680.00 -1067.00 1028.00) (-516.00 -1004.00 1017.00) ( 575.00 -1073.00 1033.00)
acc (-687.00 -1067.00 1028.00) (-504.00 -1004.00 1017.00) ( 565.00 -1073.00 1033.00)
acc (-687.00 -1067.00 1028.00) (-511.00 -1004.00 1017.00) ( 576.00 -1073.00 1033.00)
acc (-689.00 -1067.00 1028.00) (-527.00 -1004.00 1017.00) ( 572.00 -1073.00 1033.00)
acc (-688.00 -1067.00 1028.00) (-510.00 -1004.00 1017.00) ( 567.00 -1073.00 1033.00)
３軸加速度センサ出力はSTM32f3discoveryの姿勢を変えることで，磁気センサの３つの出力が＋MからーMまで変化するはずで，ゲインも同 じでオフセットも無いはずである。ところが，実際に測定してみると，３つのセンサ出力値の最大値・最小値はばらついている。そこで，最大値最小値を測定 し，較正することにする。
３軸磁気センサ出力の最大値・最小値を求める（semihosting使用）
浮動小数点数を表示するためlib/stdio/printf.cをプロジェクトから削除してGCCのｐrintfを使う
#include <stm32f30x.h>
#include <stdint.h>
#include "stm32f30x_conf.h"
#include "stm32f3_discovery.h"
#include <stdio.h>
#include <math.h>
#include "stm32f3_discovery_lsm303dlhc.h"
void initCompass(void);
void readCompass_Mag(float* pfData);
void testCompass_Mag(void);
iint main()
{
initCompass();
while (1) {
testCompass_Acc();
}
}
void testCompass_Mag(void)
{
float MagBuffer[3];
static float b0min=0,b0max=0,b1min=0,b1max=0,b2min=0,b2max=0;
readCompass_Mag(MagBuffer);
if(MagBuffer[0]<b0min) b0min=MagBuffer[0];
if(b0max<MagBuffer[0]) b0max=MagBuffer[0];
if(MagBuffer[1]<b1min) b1min=MagBuffer[1];
if(b1max<MagBuffer[1]) b1max=MagBuffer[1];
if(MagBuffer[2]<b2min) b2min=MagBuffer[2];
if(b2max<MagBuffer[2]) b2max=MagBuffer[2];
printf("mag (%7.2f %7.2f %7.2f) (%7.2f %7.2f %7.2f) (%7.2f %7.2f %7.2f)\n",
MagBuffer[0],b0min,b0max,MagBuffer[1],b1min,b1max,MagBuffer[2],b2min,b2max);
}
/*この後ろに，次の２つの関数を置く
void initCompass(void);
void readCompass_Mag(float* pfData);
*/
STM32f3discoveryの姿勢を変化させながら最大，最小を求めながらの実行結果 MagBuffer[0] MagBuffer[1] MagBuffer[2]
min max min max min max
途中経過省略
mag ( 60.87 -439.13 400.00) (-230.43 -482.61 339.13) (-239.02 -404.88 351.22)
mag ( 34.78 -439.13 400.00) (-200.00 -482.61 339.13) (-253.66 -404.88 351.22)
mag ( 17.39 -439.13 400.00) (-191.30 -482.61 339.13) (-258.54 -404.88 351.22)
mag ( 26.09 -439.13 400.00) ( -26.09 -482.61 339.13) (-278.05 -404.88 351.22)
mag ( 30.43 -439.13 400.00) ( 78.26 -482.61 339.13) (-234.15 -404.88 351.22)
mag ( 47.83 -439.13 400.00) ( 26.09 -482.61 339.13) (-253.66 -404.88 351.22)
mag ( 78.26 -439.13 400.00) ( -78.26 -482.61 339.13) (-268.29 -404.88 351.22)
機首方向角（heding角，北が０度，東が９０度），機首上げ角（pitch角），横振角（roll角）を測定し，semihosting機能で表示した。
初期化関数，測定関数をそのまま他のmainで使うことができる。
STM32F3-Discovery_FW_V1.1.0内のProject\Demonstrationにサンプルプログラムの出力がおかしいので，
void getAngle(angle_t *angle);
を作った。
heading角，pitch角，roll角を求める（semihosting使用）
浮動小数点数を表示するためlib/stdio/printf.cをプロジェクトから削除してGCCのｐrintfを使う
#include <stm32f30x.h>
#include <stdint.h>
#include "stm32f30x_conf.h"
#include "stm32f3_discovery.h"
#include <stdio.h>
#include <math.h>
#include "stm32f3_discovery_lsm303dlhc.h"
typedef struct {
float roll;
float pitch;
float headingvalue;
} angle_t;
typedef struct {
float min;
float max;
float mean;
float factor;
} factor_t;
#define PI (float)3.14159265f
#define declination (float)7.0f //西へ７度，磁北がずれている
#define rad_declination (float)(declination/180.0f*PI)
void getAngle(angle_t *angle);
void initCompass(void);
void readCompass_Acc(float* pfData);
void readCompass_Mag(float* pfData);
void setfactorMag(void);
void setfactorAcc(void);
void testCompass_Mag(void);
void testCompass_Acc(void);
factor_t fctMag[3];
factor_t fctAcc[3];
int main()
{
angle_t angle;
initCompass();
setfactorMag();
setfactorAcc();
while (1) {
getAngle(&angle);
angle.roll*=180.0f/PI;
angle.pitch*=180.0f/PI;
angle.headingvalue*=180.0f/PI;
printf("roll,pitch,head=%7.2f %7.2f %7.2f\n",angle.roll,angle.pitch,angle.headingvalue);
}
}
void setfactorMag(void)
{
int i;
/*mag ( 78.26 -439.13 400.00) ( -78.26 -482.61 339.13) (-268.29 -404.88 351.22)*/
fctMag[0].min = -439.13;
fctMag[0].max = 400.00;
fctMag[1].min = -482.61;
fctMag[1].max = 339.13;
fctMag[2].min = -404.88;
fctMag[2].max = 351.22;
for(i=0; i<3; i++) {
fctMag[i].mean=(fctMag[i].min+fctMag[i].max)/2.0;
fctMag[i].factor=200./(fctMag[i].max-fctMag[i].min);
}
}
void setfactorAcc(void)
{
int i;
/*acc (-688.00 -1067.00 1028.00) (-510.00 -1004.00 1017.00) ( 567.00 -1073.00 1033.00)*/
fctAcc[0].min = -1067.00;
fctAcc[0].max = 1028.00;
fctAcc[1].min = -1004.00;
fctAcc[1].max = 1017.00;
fctAcc[2].min = -1073.00;
fctAcc[2].max = 1033.00;
for(i=0; i<3; i++) {
fctAcc[i].mean=(fctAcc[i].min+fctAcc[i].max)/2.0;
fctAcc[i].factor=200./(fctAcc[i].max-fctAcc[i].min);
}
}
/*3つの角度をrad単位で構造体angleに得る関数*/
void getAngle(angle_t *angle)
{
float AccBuffer[3];
/* Accx:AccBuffer[0], Accy:AccBuffer[1], Accz:AccBuffer[2]*/
float fNormAcc,RollAng, PitchAng;
int i;
readCompass_Acc(AccBuffer);
for (i=0; i<3; i++) {
AccBuffer[i]=(AccBuffer[i]-fctAcc[i].mean)*fctAcc[i].factor;
}
//printf("acc %7.2f %7.2f %7.2f\n",AccBuffer[0],AccBuffer[1],AccBuffer[2]);
fNormAcc = sqrt((float)(AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
PitchAng = -asin((float)(AccBuffer[0]/fNormAcc));
RollAng = -atan2((float)AccBuffer[1],(float)AccBuffer[2]); /*分子，分母の順*/
angle->pitch = PitchAng;
angle->roll = RollAng;
float MagBuffer[3] = {0.0f};
/* Magx:MagBuffer[0], Magy:MagBuffer[1], Magz:MagBuffer[2]*/
float costhy,sinthy,costhx,sinthx;
float Mx,My;
float HeadingValue = 0.0f;
readCompass_Mag(MagBuffer);
for (i=0; i<3; i++) {
MagBuffer[i]=(MagBuffer[i]-fctMag[i].mean)*fctMag[i].factor;
}
//printf("mag %7.2f %7.2f %7.2f\n",MagBuffer[0],MagBuffer[1],MagBuffer[2]);
costhy=cos((float)PitchAng);
sinthy=sin((float)PitchAng);
costhx=cos(-(float)RollAng);
sinthx=sin(-(float)RollAng);
Mx=costhy*MagBuffer[0]+sinthy*sinthx*MagBuffer[1]+sinthy*costhx*MagBuffer[2];
My=costhx*MagBuffer[1]-sinthx*MagBuffer[2];
HeadingValue = atan2(-(float)My,-(float)Mx)-rad_declination;
while (HeadingValue < 0) {
HeadingValue = HeadingValue + 2.0f*PI;
}
while (2.0f*PI <= HeadingValue) {
HeadingValue = HeadingValue - 2.0f*PI;
}
angle->headingvalue = HeadingValue;
}
void initCompass(void)
{
LSM303DLHCMag_InitTypeDef LSM303DLHC_InitStructure;
LSM303DLHCAcc_InitTypeDef LSM303DLHCAcc_InitStructure;
LSM303DLHCAcc_FilterConfigTypeDef LSM303DLHCFilter_InitStructure;
/* Configure MEMS magnetometer main parameters: temp, working mode, full Scale and Data rate */
LSM303DLHC_InitStructure.Temperature_Sensor = LSM303DLHC_TEMPSENSOR_DISABLE;
LSM303DLHC_InitStructure.MagOutput_DataRate =LSM303DLHC_ODR_30_HZ ;
LSM303DLHC_InitStructure.MagFull_Scale = LSM303DLHC_FS_8_1_GA;
LSM303DLHC_InitStructure.Working_Mode = LSM303DLHC_CONTINUOS_CONVERSION;
LSM303DLHC_MagInit(&LSM303DLHC_InitStructure);
/* Fill the accelerometer structure */
LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_NORMAL_MODE;
LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_50_HZ;
LSM303DLHCAcc_InitStructure.Axes_Enable= LSM303DLHC_AXES_ENABLE;
LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB;
LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE;
/* Configure the accelerometer main parameters */
LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
/* Fill the accelerometer LPF structure */
LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection =LSM303DLHC_HPM_NORMAL_MODE;
LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency = LSM303DLHC_HPFCF_16;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1 = LSM303DLHC_HPF_AOI1_DISABLE;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2 = LSM303DLHC_HPF_AOI2_DISABLE;
/* Configure the accelerometer LPF main parameters */
LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
}
void readCompass_Acc(float* pfData)
{
#define LSM_Acc_Sensitivity_2g (float) 1.0f /*!< accelerometer sensitivity with 2 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_4g (float) 0.5f /*!< accelerometer sensitivity with 4 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_8g (float) 0.25f /*!< accelerometer sensitivity with 8 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_16g (float) 0.0834f /*!< accelerometer sensitivity with 12 g full scale [LSB/mg] */
int16_t pnRawData[3];
uint8_t ctrlx[2];
uint8_t buffer[6], cDivider;
uint8_t i = 0;
float LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
/* Read the register content */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, buffer, 6);
if(ctrlx[1]&0x40)
cDivider=64;
else
cDivider=16;
/* check in the control register4 the data alignment*/
if(!(ctrlx[0] & 0x40) || (ctrlx[1] & 0x40)) /* Little Endian Mode or FIFO mode */
{
for(i=0; i<3; i++)
{
pnRawData[i]=((int16_t)((uint16_t)buffer[2*i+1] << 8) + buffer[2*i])/cDivider;
}
}
else /* Big Endian Mode */
{
for(i=0; i<3; i++)
pnRawData[i]=((int16_t)((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])/cDivider;
}
/* Read the register content */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
if(ctrlx[1]&0x40)
{
/* FIFO mode */
LSM_Acc_Sensitivity = 0.25;
}
else
{
/* normal mode */
/* switch the sensitivity value set in the CRTL4*/
switch(ctrlx[0] & 0x30)
{
case LSM303DLHC_FULLSCALE_2G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
break;
case LSM303DLHC_FULLSCALE_4G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_4g;
break;
case LSM303DLHC_FULLSCALE_8G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_8g;
break;
case LSM303DLHC_FULLSCALE_16G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_16g;
break;
}
}
/* Obtain the mg value for the three axis */
for(i=0; i<3; i++)
{
pfData[i]=(float)pnRawData[i]/LSM_Acc_Sensitivity;
}
}
void readCompass_Mag(float* pfData)
{
static uint8_t buffer[6] = {0};
uint8_t CTRLB = 0;
uint16_t Magn_Sensitivity_XY = 0, Magn_Sensitivity_Z = 0;
uint8_t i =0;
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_CRB_REG_M, &CTRLB, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, buffer, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, buffer+1, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, buffer+2, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, buffer+3, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, buffer+4, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, buffer+5, 1);
/* Switch the sensitivity set in the CRTLB*/
switch(CTRLB & 0xE0)
{
case LSM303DLHC_FS_1_3_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_3Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_3Ga;
break;
case LSM303DLHC_FS_1_9_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_9Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_9Ga;
break;
case LSM303DLHC_FS_2_5_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_2_5Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_2_5Ga;
break;
case LSM303DLHC_FS_4_0_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4Ga;
break;
case LSM303DLHC_FS_4_7_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4_7Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4_7Ga;
break;
case LSM303DLHC_FS_5_6_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_5_6Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_5_6Ga;
break;
case LSM303DLHC_FS_8_1_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_8_1Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_8_1Ga;
break;
}
for(i=0; i<2; i++)
{
pfData[i]=(float)((int16_t)(((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])*1000)/Magn_Sensitivity_XY;
}
pfData[2]=(float)((int16_t)(((uint16_t)buffer[4] << 8) + buffer[5])*1000)/Magn_Sensitivity_Z;
}
uint32_t LSM303DLHC_TIMEOUT_UserCallback(void)
{
return 0;
}
実行結果 roll,pitch,head= -2.33 12.61 286.94
roll,pitch,head= -2.78 12.49 285.04
roll,pitch,head= -3.06 12.45 285.29
roll,pitch,head= -3.07 12.64 285.14
roll,pitch,head= -2.48 12.34 284.89
roll,pitch,head= -2.16 12.96 286.24
roll,pitch,head= -2.90 12.42 285.99
roll,pitch,head= -2.27 12.59 286.66
roll,pitch,head= -2.88 12.72 286.52
1secごとに取得し，10000秒(約2.8時間)の測定を行なう。
heading角，pitch角，roll角を求める（semihosting使用）
浮動小数点数を表示するためlib/stdio/printf.cをプロジェクトから削除してGCCのｐrintfを使う
/*
/*
３軸ディジタルコンパス＋３軸加速度モジュール LSM303DLHC
浮動小数点数を表示するためcoocoxのlib/stdio/printf.cは削除してGCCのｐrintfを使う
◯mag出力
mcuカードをstm32f3discoveryの文字が見えるようにおいて
LSM303DLHCのx軸は下辺向き，ｙ軸は右辺向き，z軸は部品面の上向き。右手系になっている
それぞれ地球のS極側（地理的には北の地中方向）を指すと正の値が出てくる
意外と鉛直成分の絶対値が大きい
◯ACC出力
mcuカードをstm32f3discoveryの文字が見えるようにおいて
前方を上げると buf0 -
前方を下げると buf0 +
右を上げると buf1 +
右を下げると buf1 -
水平で buf2 +
すなわちx軸は下辺向き，ｙ軸は右辺向き，z軸は部品面の上向き。右手系になっているが
地球に重力で引っ張られる方向と逆方向に軸が向くと正の値を出力する
オイラー角 ピッチ 前が上がると＋ (buf0の符号反転)
オイラー角ロール 右が下がると＋ (buf1の符号反転)
*/
/* Includes ------------------------------------------------------------------*/
#include <stm32f30x.h>
#include <stdint.h>
#include "stm32f30x_conf.h"
#include "stm32f3_discovery.h"
#include <stdio.h>
#include <math.h>
#include "stm32f3_discovery_lsm303dlhc.h"
typedef struct {
float roll;
float pitch;
float headingvalue;
} angle_t;
typedef struct {
float min;
float max;
float mean;
float factor;
} factor_t;
#define PI (float)3.14159265f
#define declination (float)7.0f //西へ７度，磁北がずれている
#define rad_declination (float)(declination/180.0f*PI)
void initIntervaltimer(int32_t frequency);
void getAngle(angle_t *angle);
void initCompass(void);
void readCompass_Acc(float* pfData);
void readCompass_Mag(float* pfData);
void setfactorMag(void);
void setfactorAcc(void);
factor_t fctMag[3];
factor_t fctAcc[3];
angle_t angle={0.,0.,0.};
volatile int request=0;
int main()
{
int i=0;
initCompass();
setfactorMag();
setfactorAcc();
printf("time,roll[deg],pitch[deg],head[deg]\r\n");
/* SysTick end of count event each 1ms (1000Hz)*/
initIntervaltimer(1000);
while(i<=10000) {
while (request==0);
angle.roll*=180.0f/PI;
angle.pitch*=180.0f/PI;
angle.headingvalue*=180.0f/PI;
printf("%4d,%7.2f, %7.2f, %7.2f\n",i,angle.roll,angle.pitch,angle.headingvalue);
request=0;
i++;
}
while(1);
}
/*インターバルタイマー割り込み初期設定 割り込み周波数[Hz]を引数で渡す*/
void initIntervaltimer(int32_t frequency)
{
RCC_ClocksTypeDef RCC_Clocks;
RCC_GetClocksFreq(&RCC_Clocks);
SysTick_Config(RCC_Clocks.HCLK_Frequency / frequency);
}
void SysTick_Handler(void)
{
static int32_t count=0;
count++;
if (count==1000) {
count=0;
getAngle(&angle);
request = 1;
}
}
void setfactorMag(void)
{
int i;
/*mag ( 78.26 -439.13 400.00) ( -78.26 -482.61 339.13) (-268.29 -404.88 351.22)*/
fctMag[0].min = -439.13;
fctMag[0].max = 400.00;
fctMag[1].min = -482.61;
fctMag[1].max = 339.13;
fctMag[2].min = -404.88;
fctMag[2].max = 351.22;
for(i=0; i<3; i++) {
fctMag[i].mean=(fctMag[i].min+fctMag[i].max)/2.0;
fctMag[i].factor=200./(fctMag[i].max-fctMag[i].min);
}
}
void setfactorAcc(void)
{
int i;
/*acc (-688.00 -1067.00 1028.00) (-510.00 -1004.00 1017.00) ( 567.00 -1073.00 1033.00)*/
fctAcc[0].min = -1067.00;
fctAcc[0].max = 1028.00;
fctAcc[1].min = -1004.00;
fctAcc[1].max = 1017.00;
fctAcc[2].min = -1073.00;
fctAcc[2].max = 1033.00;
for(i=0; i<3; i++) {
fctAcc[i].mean=(fctAcc[i].min+fctAcc[i].max)/2.0;
fctAcc[i].factor=200./(fctAcc[i].max-fctAcc[i].min);
}
}
void getAngle(angle_t *angle)
{
float AccBuffer[3];
/* Accx:AccBuffer[0], Accy:AccBuffer[1], Accz:AccBuffer[2]*/
float fNormAcc,RollAng, PitchAng;
int i;
readCompass_Acc(AccBuffer);
for (i=0; i<3; i++) {
AccBuffer[i]=(AccBuffer[i]-fctAcc[i].mean)*fctAcc[i].factor;
}
//printf("acc %7.2f %7.2f %7.2f\n",AccBuffer[0],AccBuffer[1],AccBuffer[2]);
fNormAcc = sqrt((float)(AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
PitchAng = -asin((float)(AccBuffer[0]/fNormAcc));
RollAng = -atan2((float)AccBuffer[1],(float)AccBuffer[2]); /*分子，分母の順*/
angle->pitch = PitchAng;
angle->roll = RollAng;
float MagBuffer[3] = {0.0f};
/* Magx:MagBuffer[0], Magy:MagBuffer[1], Magz:MagBuffer[2]*/
float costhy,sinthy,costhx,sinthx;
float Mx,My;
float HeadingValue = 0.0f;
readCompass_Mag(MagBuffer);
for (i=0; i<3; i++) {
MagBuffer[i]=(MagBuffer[i]-fctMag[i].mean)*fctMag[i].factor;
}
//printf("mag %7.2f %7.2f %7.2f\n",MagBuffer[0],MagBuffer[1],MagBuffer[2]);
costhy=cos((float)PitchAng);
sinthy=sin((float)PitchAng);
costhx=cos(-(float)RollAng);
sinthx=sin(-(float)RollAng);
Mx=costhy*MagBuffer[0]+sinthy*sinthx*MagBuffer[1]+sinthy*costhx*MagBuffer[2];
My=costhx*MagBuffer[1]-sinthx*MagBuffer[2];
HeadingValue = atan2(-(float)My,-(float)Mx)-rad_declination;
while (HeadingValue < 0) {
HeadingValue = HeadingValue + 2.0f*PI;
}
while (2.0f*PI <= HeadingValue) {
HeadingValue = HeadingValue - 2.0f*PI;
}
angle->headingvalue = HeadingValue;
}
void initCompass(void)
{
LSM303DLHCMag_InitTypeDef LSM303DLHC_InitStructure;
LSM303DLHCAcc_InitTypeDef LSM303DLHCAcc_InitStructure;
LSM303DLHCAcc_FilterConfigTypeDef LSM303DLHCFilter_InitStructure;
/* Configure MEMS magnetometer main parameters: temp, working mode, full Scale and Data rate */
LSM303DLHC_InitStructure.Temperature_Sensor = LSM303DLHC_TEMPSENSOR_DISABLE;
LSM303DLHC_InitStructure.MagOutput_DataRate =LSM303DLHC_ODR_30_HZ ;
LSM303DLHC_InitStructure.MagFull_Scale = LSM303DLHC_FS_8_1_GA;
LSM303DLHC_InitStructure.Working_Mode = LSM303DLHC_CONTINUOS_CONVERSION;
LSM303DLHC_MagInit(&LSM303DLHC_InitStructure);
/* Fill the accelerometer structure */
LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_NORMAL_MODE;
LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_50_HZ;
LSM303DLHCAcc_InitStructure.Axes_Enable= LSM303DLHC_AXES_ENABLE;
LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB;
LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE;
/* Configure the accelerometer main parameters */
LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
/* Fill the accelerometer LPF structure */
LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection =LSM303DLHC_HPM_NORMAL_MODE;
LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency = LSM303DLHC_HPFCF_16;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1 = LSM303DLHC_HPF_AOI1_DISABLE;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2 = LSM303DLHC_HPF_AOI2_DISABLE;
/* Configure the accelerometer LPF main parameters */
LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
}
void readCompass_Acc(float* pfData)
{
#define LSM_Acc_Sensitivity_2g (float) 1.0f /*!< accelerometer sensitivity with 2 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_4g (float) 0.5f /*!< accelerometer sensitivity with 4 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_8g (float) 0.25f /*!< accelerometer sensitivity with 8 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_16g (float) 0.0834f /*!< accelerometer sensitivity with 12 g full scale [LSB/mg] */
int16_t pnRawData[3];
uint8_t ctrlx[2];
uint8_t buffer[6], cDivider;
uint8_t i = 0;
float LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
/* Read the register content */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, buffer, 6);
if(ctrlx[1]&0x40)
cDivider=64;
else
cDivider=16;
/* check in the control register4 the data alignment*/
if(!(ctrlx[0] & 0x40) || (ctrlx[1] & 0x40)) /* Little Endian Mode or FIFO mode */
{
for(i=0; i<3; i++)
{
pnRawData[i]=((int16_t)((uint16_t)buffer[2*i+1] << 8) + buffer[2*i])/cDivider;
}
}
else /* Big Endian Mode */
{
for(i=0; i<3; i++)
pnRawData[i]=((int16_t)((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])/cDivider;
}
/* Read the register content */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
if(ctrlx[1]&0x40)
{
/* FIFO mode */
LSM_Acc_Sensitivity = 0.25;
}
else
{
/* normal mode */
/* switch the sensitivity value set in the CRTL4*/
switch(ctrlx[0] & 0x30)
{
case LSM303DLHC_FULLSCALE_2G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
break;
case LSM303DLHC_FULLSCALE_4G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_4g;
break;
case LSM303DLHC_FULLSCALE_8G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_8g;
break;
case LSM303DLHC_FULLSCALE_16G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_16g;
break;
}
}
/* Obtain the mg value for the three axis */
for(i=0; i<3; i++)
{
pfData[i]=(float)pnRawData[i]/LSM_Acc_Sensitivity;
}
}
void readCompass_Mag(float* pfData)
{
static uint8_t buffer[6] = {0};
uint8_t CTRLB = 0;
uint16_t Magn_Sensitivity_XY = 0, Magn_Sensitivity_Z = 0;
uint8_t i =0;
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_CRB_REG_M, &CTRLB, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, buffer, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, buffer+1, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, buffer+2, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, buffer+3, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, buffer+4, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, buffer+5, 1);
/* Switch the sensitivity set in the CRTLB*/
switch(CTRLB & 0xE0)
{
case LSM303DLHC_FS_1_3_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_3Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_3Ga;
break;
case LSM303DLHC_FS_1_9_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_9Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_9Ga;
break;
case LSM303DLHC_FS_2_5_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_2_5Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_2_5Ga;
break;
case LSM303DLHC_FS_4_0_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4Ga;
break;
case LSM303DLHC_FS_4_7_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4_7Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4_7Ga;
break;
case LSM303DLHC_FS_5_6_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_5_6Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_5_6Ga;
break;
case LSM303DLHC_FS_8_1_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_8_1Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_8_1Ga;
break;
}
for(i=0; i<2; i++)
{
pfData[i]=(float)((int16_t)(((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])*1000)/Magn_Sensitivity_XY;
}
pfData[2]=(float)((int16_t)(((uint16_t)buffer[4] << 8) + buffer[5])*1000)/Magn_Sensitivity_Z;
}
uint32_t LSM303DLHC_TIMEOUT_UserCallback(void)
{
return 0;
}
このプログラムにより，角度測定の安定性を２個のSTMF3DISCOVERYで調べてみた。
測定はSTMF3DISCOVERYを動かさないようにしておこなった。期待する測定値は，常時，各角度=一定値である。
（１） 1個目のSTMF3DISCOVERY
得られた各角度の時間変化
角度測定値の標準偏差は次の通り
rollの角度標準偏差＝0.1359
pitchの角度標準偏差＝0.1591
headの角度標準偏差＝0.5418
（２） 2個目のSTMF3DISCOVERY
得られた各角度の時間変化
角度測定値の標準偏差は次の通り
rollの角度標準偏差＝0.1653
pitchの角度標準偏差＝0.1381
headの角度標準偏差＝0.5725
STMF3DISCOVERYの８つのLEDを使って，北を指し示したり，水平方向を示したりするデモプログラムを作る。
これは２つもモード
（１）北を指し示す
（２）水平方向を指し示す
があり，プシュSWを押すごとに切り替わるものとする。
（１）北方向を求め，STMF3DISCOVERYが斜めになっていても裏返っていても来た方向のLEDを点灯させる。分解能は22.5度であり，2つのLEDあるいは1つのLEDを点灯させることにより，北方位を示す。
（２）STMF3DISCOVERYカードを鉛直に立てると，２つのLEDが点灯し，水平方向を示すものとする。分解能は
22.5度であり，4つのLEDあるいは2つのLEDを点灯させることにより，水平方向を示す。
北を指し示すデモ（手前が北方向）
（この映像ではCPUカードはほぼ水平に保持されています）
水平方向を示すデモ
（この映像ではCPUはほぼ鉛直に立てて保持されています）
北を指し示したり，水平方向を示したりするデモプログラム
/*
３軸ディジタルコンパス＋３軸加速度モジュール LSM303DLHC
浮動小数点数を表示するためcoocoxのlib/stdio/printf.cは削除してGCCのｐrintfを使う
◯mag出力
mcuカードをstm32f3discoveryの文字が見えるようにおいて
LSM303DLHCのx軸は下辺向き，ｙ軸は右辺向き，z軸は部品面の上向き。右手系になっている
それぞれ地球のS極側（地理的には北の地中方向）を指すと正の値が出てくる
意外と鉛直成分の絶対値が大きい
◯ACC出力
mcuカードをstm32f3discoveryの文字が見えるようにおいて
前方を上げると buf0 -
前方を下げると buf0 +
右を上げると buf1 +
右を下げると buf1 -
水平で buf2 +
すなわちx軸は下辺向き，ｙ軸は右辺向き，z軸は部品面の上向き。右手系になっているが
地球に重力で引っ張られる方向と逆方向に軸が向くと正の値を出力する
オイラー角 ピッチ 前が上がると＋ (buf0の符号反転)
オイラー角ロール 右が下がると＋ (buf1の符号反転)
*/
/* Includes ------------------------------------------------------------------*/
#include <stm32f30x.h>
#include <stdint.h>
#include "stm32f30x_conf.h"
#include "stm32f3_discovery.h"
#include <stdio.h>
#include <math.h>
#include "stm32f3_discovery_lsm303dlhc.h"
typedef struct {
float roll;
float pitch;
float headingvalue;
} angle_t;
typedef struct {
float min;
float max;
float mean;
float factor;
} factor_t;
#define PI (float)3.14159265f
#define declination (float)7.0f //西へ７度，磁北がずれている
#define rad_declination (float)(declination/180.0f*PI)
void initIntervaltimer(int32_t frequency);
void getAngle(angle_t *angle);
void initCompass(void);
void readCompass_Acc(float* pfData);
void readCompass_Mag(float* pfData);
void setfactorMag(void);
void setfactorAcc(void);
void showHeading(float head, int32_t flag);
void showAngle(float rotz);
float getRotZ(void);
factor_t fctMag[3];
factor_t fctAcc[3];
angle_t angle={0.,0.,0.};
float rotz;
volatile int request=0;
int mode=0; //mode0:北方向表示 mode1:水平方向表示
int main()
{
int i=0;
int idle=0;
initCompass();
setfactorMag();
setfactorAcc();
/* Configure LED3 - LED10 on STM32F3-Discovery */
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
STM_EVAL_LEDInit(LED5);
STM_EVAL_LEDInit(LED6);
STM_EVAL_LEDInit(LED7);
STM_EVAL_LEDInit(LED8);
STM_EVAL_LEDInit(LED9);
STM_EVAL_LEDInit(LED10);
/* Initialize User_Button on STM32F3-Discovery */
STM_EVAL_PBInit(BUTTON_USER, BUTTON_MODE_GPIO);
printf("time,roll[deg],pitch[deg],head[deg],rotZ[deg]\r\n");
/* SysTick end of count event each 1ms (1000Hz)*/
initIntervaltimer(1000);
while(1) {
while (request==0);
angle.roll*=180.0f/PI;
angle.pitch*=180.0f/PI;
angle.headingvalue*=180.0f/PI;
rotz*=180.0f/PI;
printf("%7.2f, %7.2f, %7.2f, %7.2f\n",angle.roll,angle.pitch,angle.headingvalue,rotz);
request=0;
i++;
if (idle==0) {
if (STM_EVAL_PBGetState(BUTTON_USER)== SET) { //if user's button pushed)
mode=1-mode;
idle=1;
}
} else {
if (10<++idle) idle=0;
}
if (mode==0) {
int32_t flag=0;
if (angle.roll<-90.f || 90.f<angle.roll) flag=1;
showHeading(angle.headingvalue,flag);
} else {
showAngle(rotz);
}
}
while(1);
}
//head：方向角 北から東回りに増加[deg]
//flag:表面が鉛直方向上向き：0 下向き：1
void showHeading(float head, int32_t flag)
{
head+=11.25f;
if (360.f<=head) head-=360.f;
int num=head/360.f*16.f;
GPIOE ->ODR &= ~0xff00; //turn all LED off
if (flag==1) {
num=(16-num)&15;
}
switch (num) {
case 0:
STM_EVAL_LEDOn(LED3); //Turn LED3 On
break;
case 1:
STM_EVAL_LEDOn(LED3); //Turn LED3 On
STM_EVAL_LEDOn(LED4); //Turn LED4 On
break;
case 2:
STM_EVAL_LEDOn(LED4); //Turn LED4 On
break;
case 3:
STM_EVAL_LEDOn(LED4); //Turn LED4 On
STM_EVAL_LEDOn(LED6); //Turn LED6 On
break;
case 4:
STM_EVAL_LEDOn(LED6); //Turn LED6 On
break;
case 5:
STM_EVAL_LEDOn(LED6); //Turn LED6 On
STM_EVAL_LEDOn(LED8); //Turn LED8 On
break;
case 6:
STM_EVAL_LEDOn(LED8); //Turn LED8 On
break;
case 7:
STM_EVAL_LEDOn(LED8); //Turn LED8 On
STM_EVAL_LEDOn(LED10); //Turn LED10 On
break;
case 8:
STM_EVAL_LEDOn(LED10); //Turn LED10 On
break;
case 9:
STM_EVAL_LEDOn(LED10); //Turn LED10 On
STM_EVAL_LEDOn(LED9); //Turn LED9 On
break;
case 10:
STM_EVAL_LEDOn(LED9); //Turn LED9 On
break;
case 11:
STM_EVAL_LEDOn(LED9); //Turn LED9 On
STM_EVAL_LEDOn(LED7); //Turn LED7 On
break;
case 12:
STM_EVAL_LEDOn(LED7); //Turn LED7 On
break;
case 13:
STM_EVAL_LEDOn(LED7); //Turn LED7 On
STM_EVAL_LEDOn(LED5); //Turn LED5 On
break;
case 14:
STM_EVAL_LEDOn(LED5); //Turn LED5 On
break;
case 15:
STM_EVAL_LEDOn(LED5); //Turn LED5 On
STM_EVAL_LEDOn(LED3); //Turn LED3 On
break;
default:
break;
}
}
void showAngle(float rotz)
{
GPIOE ->ODR &= ~0xff00; //turn all LED off
//if (90.f<rotz) rotz=180.f-rotz;
//if (rotz<-90.f) rotz=-180.f+rotz;
if (rotz<0.f) rotz=180.f+rotz;
rotz+=11.25f;
int num=((int)(rotz/90.f*3.999f))&7;
//printf("%7.2f %5d\n",rotz,num);
switch (num) {
case 7:
STM_EVAL_LEDOn(LED3); //Turn LED3 On
STM_EVAL_LEDOn(LED10); //Turn LED10 On
STM_EVAL_LEDOn(LED4); //Turn LED4 On
STM_EVAL_LEDOn(LED9); //Turn LED9 On
break;
case 6:
STM_EVAL_LEDOn(LED4); //Turn LED4 On
STM_EVAL_LEDOn(LED9); //Turn LED9 On
break;
case 5:
STM_EVAL_LEDOn(LED4); //Turn LED4 On
STM_EVAL_LEDOn(LED9); //Turn LED9 On
STM_EVAL_LEDOn(LED6); //Turn LED6 On
STM_EVAL_LEDOn(LED7); //Turn LED7 On
break;
case 4:
STM_EVAL_LEDOn(LED6); //Turn LED6 On
STM_EVAL_LEDOn(LED7); //Turn LED7 On
break;
case 3:
STM_EVAL_LEDOn(LED6); //Turn LED6 On
STM_EVAL_LEDOn(LED7); //Turn LED7 On
STM_EVAL_LEDOn(LED5); //Turn LED5 On
STM_EVAL_LEDOn(LED8); //Turn LED8 On
break;
case 2:
STM_EVAL_LEDOn(LED5); //Turn LED5 On
STM_EVAL_LEDOn(LED8); //Turn LED8 On
break;
case 1:
STM_EVAL_LEDOn(LED5); //Turn LED5 On
STM_EVAL_LEDOn(LED8); //Turn LED8 On
STM_EVAL_LEDOn(LED3); //Turn LED3 On
STM_EVAL_LEDOn(LED10); //Turn LED10 On
break;
case 0:
STM_EVAL_LEDOn(LED3); //Turn LED3 On
STM_EVAL_LEDOn(LED10); //Turn LED10 On
break;
default:
break;
}
}
/*インターバルタイマー割り込み初期設定 割り込み周波数[Hz]を引数で渡す*/
void initIntervaltimer(int32_t frequency)
{
RCC_ClocksTypeDef RCC_Clocks;
RCC_GetClocksFreq(&RCC_Clocks);
SysTick_Config(RCC_Clocks.HCLK_Frequency / frequency);
}
void SysTick_Handler(void)
{
static int32_t count=0;
count++;
if (count==100) {
count=0;
getAngle(&angle);
rotz=getRotZ();
request = 1;
}
}
void setfactorMag(void)
{
int i;
/*mag ( 78.26 -439.13 400.00) ( -78.26 -482.61 339.13) (-268.29 -404.88 351.22)*/
fctMag[0].min = -439.13;
fctMag[0].max = 400.00;
fctMag[1].min = -482.61;
fctMag[1].max = 339.13;
fctMag[2].min = -404.88;
fctMag[2].max = 351.22;
for(i=0; i<3; i++) {
fctMag[i].mean=(fctMag[i].min+fctMag[i].max)/2.0;
fctMag[i].factor=200./(fctMag[i].max-fctMag[i].min);
}
}
void setfactorAcc(void)
{
int i;
/*acc (-688.00 -1067.00 1028.00) (-510.00 -1004.00 1017.00) ( 567.00 -1073.00 1033.00)*/
fctAcc[0].min = -1067.00;
fctAcc[0].max = 1028.00;
fctAcc[1].min = -1004.00;
fctAcc[1].max = 1017.00;
fctAcc[2].min = -1073.00;
fctAcc[2].max = 1033.00;
for(i=0; i<3; i++) {
fctAcc[i].mean=(fctAcc[i].min+fctAcc[i].max)/2.0;
fctAcc[i].factor=200./(fctAcc[i].max-fctAcc[i].min);
}
}
float getRotZ(void)
{
float AccBuffer[3];
/* Accx:AccBuffer[0], Accy:AccBuffer[1], Accz:AccBuffer[2]*/
//float fNormAcc,RollAng, PitchAng;
int i;
readCompass_Acc(AccBuffer);
for (i=0; i<3; i++) {
AccBuffer[i]=(AccBuffer[i]-fctAcc[i].mean)*fctAcc[i].factor;
}
//printf("acc %7.2f %7.2f %7.2f\n",AccBuffer[0],AccBuffer[1],AccBuffer[2]);
//fNormAcc = sqrt((float)(AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
//PitchAng = -asin((float)(AccBuffer[0]/fNormAcc));
//RollAng = -atan2((float)AccBuffer[1],(float)AccBuffer[2]); /*分子，分母の順*/
rotz = atan2(-(float)AccBuffer[0],-(float)AccBuffer[1]); /*分子，分母の順*/
return rotz;
}
void getAngle(angle_t *angle)
{
float AccBuffer[3];
/* Accx:AccBuffer[0], Accy:AccBuffer[1], Accz:AccBuffer[2]*/
float fNormAcc,RollAng, PitchAng;
int i;
readCompass_Acc(AccBuffer);
for (i=0; i<3; i++) {
AccBuffer[i]=(AccBuffer[i]-fctAcc[i].mean)*fctAcc[i].factor;
}
//printf("acc %7.2f %7.2f %7.2f\n",AccBuffer[0],AccBuffer[1],AccBuffer[2]);
fNormAcc = sqrt((float)(AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
PitchAng = -asin((float)(AccBuffer[0]/fNormAcc));
RollAng = -atan2((float)AccBuffer[1],(float)AccBuffer[2]); /*分子，分母の順*/
angle->pitch = PitchAng;
angle->roll = RollAng;
float MagBuffer[3] = {0.0f};
/* Magx:MagBuffer[0], Magy:MagBuffer[1], Magz:MagBuffer[2]*/
float costhy,sinthy,costhx,sinthx;
float Mx,My;
float HeadingValue = 0.0f;
readCompass_Mag(MagBuffer);
for (i=0; i<3; i++) {
MagBuffer[i]=(MagBuffer[i]-fctMag[i].mean)*fctMag[i].factor;
}
//printf("mag %7.2f %7.2f %7.2f\n",MagBuffer[0],MagBuffer[1],MagBuffer[2]);
costhy=cos((float)PitchAng);
sinthy=sin((float)PitchAng);
costhx=cos(-(float)RollAng);
sinthx=sin(-(float)RollAng);
Mx=costhy*MagBuffer[0]+sinthy*sinthx*MagBuffer[1]+sinthy*costhx*MagBuffer[2];
My=costhx*MagBuffer[1]-sinthx*MagBuffer[2];
HeadingValue = atan2(-(float)My,-(float)Mx)-rad_declination;
while (HeadingValue < 0) {
HeadingValue = HeadingValue + 2.0f*PI;
}
while (2.0f*PI <= HeadingValue) {
HeadingValue = HeadingValue - 2.0f*PI;
}
angle->headingvalue = HeadingValue;
}
void initCompass(void)
{
LSM303DLHCMag_InitTypeDef LSM303DLHC_InitStructure;
LSM303DLHCAcc_InitTypeDef LSM303DLHCAcc_InitStructure;
LSM303DLHCAcc_FilterConfigTypeDef LSM303DLHCFilter_InitStructure;
/* Configure MEMS magnetometer main parameters: temp, working mode, full Scale and Data rate */
LSM303DLHC_InitStructure.Temperature_Sensor = LSM303DLHC_TEMPSENSOR_DISABLE;
LSM303DLHC_InitStructure.MagOutput_DataRate =LSM303DLHC_ODR_30_HZ ;
LSM303DLHC_InitStructure.MagFull_Scale = LSM303DLHC_FS_8_1_GA;
LSM303DLHC_InitStructure.Working_Mode = LSM303DLHC_CONTINUOS_CONVERSION;
LSM303DLHC_MagInit(&LSM303DLHC_InitStructure);
/* Fill the accelerometer structure */
LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_NORMAL_MODE;
LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_50_HZ;
LSM303DLHCAcc_InitStructure.Axes_Enable= LSM303DLHC_AXES_ENABLE;
LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB;
LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE;
/* Configure the accelerometer main parameters */
LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
/* Fill the accelerometer LPF structure */
LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection =LSM303DLHC_HPM_NORMAL_MODE;
LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency = LSM303DLHC_HPFCF_16;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1 = LSM303DLHC_HPF_AOI1_DISABLE;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2 = LSM303DLHC_HPF_AOI2_DISABLE;
/* Configure the accelerometer LPF main parameters */
LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
}
/**
* @brief Read LSM303DLHC output register, and calculate the acceleration ACC=(1/SENSITIVITY)* (out_h*256+out_l)/16 (12 bit rappresentation)
* @param pnData: pointer to float buffer where to store data
* @retval None
*/
void readCompass_Acc(float* pfData)
{
#define LSM_Acc_Sensitivity_2g (float) 1.0f /*!< accelerometer sensitivity with 2 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_4g (float) 0.5f /*!< accelerometer sensitivity with 4 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_8g (float) 0.25f /*!< accelerometer sensitivity with 8 g full scale [LSB/mg] */
#define LSM_Acc_Sensitivity_16g (float) 0.0834f /*!< accelerometer sensitivity with 12 g full scale [LSB/mg] */
int16_t pnRawData[3];
uint8_t ctrlx[2];
uint8_t buffer[6], cDivider;
uint8_t i = 0;
float LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
/* Read the register content */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, buffer, 6);
if(ctrlx[1]&0x40)
cDivider=64;
else
cDivider=16;
/* check in the control register4 the data alignment*/
if(!(ctrlx[0] & 0x40) || (ctrlx[1] & 0x40)) /* Little Endian Mode or FIFO mode */
{
for(i=0; i<3; i++)
{
pnRawData[i]=((int16_t)((uint16_t)buffer[2*i+1] << 8) + buffer[2*i])/cDivider;
}
}
else /* Big Endian Mode */
{
for(i=0; i<3; i++)
pnRawData[i]=((int16_t)((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])/cDivider;
}
/* Read the register content */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, ctrlx,2);
if(ctrlx[1]&0x40)
{
/* FIFO mode */
LSM_Acc_Sensitivity = 0.25;
}
else
{
/* normal mode */
/* switch the sensitivity value set in the CRTL4*/
switch(ctrlx[0] & 0x30)
{
case LSM303DLHC_FULLSCALE_2G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g;
break;
case LSM303DLHC_FULLSCALE_4G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_4g;
break;
case LSM303DLHC_FULLSCALE_8G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_8g;
break;
case LSM303DLHC_FULLSCALE_16G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_16g;
break;
}
}
/* Obtain the mg value for the three axis */
for(i=0; i<3; i++)
{
pfData[i]=(float)pnRawData[i]/LSM_Acc_Sensitivity;
}
}
/**
* @brief calculate the magnetic field Magn.
* @param pfData: pointer to the data out
* @retval None
*/
void readCompass_Mag(float* pfData)
{
static uint8_t buffer[6] = {0};
uint8_t CTRLB = 0;
uint16_t Magn_Sensitivity_XY = 0, Magn_Sensitivity_Z = 0;
uint8_t i =0;
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_CRB_REG_M, &CTRLB, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, buffer, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, buffer+1, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, buffer+2, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, buffer+3, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, buffer+4, 1);
LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, buffer+5, 1);
/* Switch the sensitivity set in the CRTLB*/
switch(CTRLB & 0xE0)
{
case LSM303DLHC_FS_1_3_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_3Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_3Ga;
break;
case LSM303DLHC_FS_1_9_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_1_9Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_9Ga;
break;
case LSM303DLHC_FS_2_5_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_2_5Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_2_5Ga;
break;
case LSM303DLHC_FS_4_0_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4Ga;
break;
case LSM303DLHC_FS_4_7_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4_7Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4_7Ga;
break;
case LSM303DLHC_FS_5_6_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_5_6Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_5_6Ga;
break;
case LSM303DLHC_FS_8_1_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_8_1Ga;
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_8_1Ga;
break;
}
for(i=0; i<2; i++)
{
pfData[i]=(float)((int16_t)(((uint16_t)buffer[2*i] << 8) + buffer[2*i+1])*1000)/Magn_Sensitivity_XY;
}
pfData[2]=(float)((int16_t)(((uint16_t)buffer[4] << 8) + buffer[5])*1000)/Magn_Sensitivity_Z;
}
uint32_t LSM303DLHC_TIMEOUT_UserCallback(void)
{
return 0;
}
