MEMSCompassを用いたSTM32f3discoveryの姿勢（各軸周りの角速度）の測定
 
20140830 coskx
 T.SATOH
 

 １．　はじめに
STM32f3discoveryにはMEMSGyroscope_L3GD20が搭載されている。
L3GD20は3軸ジャイロセンサ（角速度センサ）である。

２．姿勢の表し方
一般に飛行機のような物体の姿勢は機首方向角heading，機首上げ角pitch，左右振れ角rollで表される。
方向角headingには様々な定義があるが，北を0度にして東：90度，南180度，西：270度のように時計回りに定義する。（この定義はよく用いられているが，左手系になってしまい，多くの数理系で用いる右手系とは異なっている。）
    　　
機首上げ角pitchは通常-90度から90度間で定義され，機首上げ方向が＋である。
    　　
左右振れ角rollはpitchの値にかかわらず，進行方向に対して右翼を下げる方向が＋である。
図は真後ろから飛行機を見たところである。
    　　
最初にheading角を決め，pitch分機首を上げ，最後にroll分右翼を下げる順で姿勢を決めることとする。
（この順を間違えると違った定義になってしまう。）

３．STM32f3discoveryに搭載されたL3GD20の角速度センサ軸
STM32f3discoveryに搭載されたL3GD20のセンサ軸はマニュアルDM00036465.pdfによれば，次の図のようになっている。
重力加速度・磁気センサSM303DLHCのセンサ軸とは異なるので注意。（設計が悪い）
 「STM32f3discovery」の文字が読める向きに飛行機の機首があると考える。





４．プログラム (3軸周りの角速度取り込みプログラム)
STM32F3-Discovery_FW_V1.1.0内のProject\Demonstrationにサンプルプログラムの初期化関数，読み取り関数をそのまま使って次のようなプログラムで，３軸それぞれの角速度を測定することができる。

特性を決める重要な設定　（DM00036465.pdf　および　stm32f3_discovery_l3gd20.h　より）

(1)L3GD20_InitStructure.Output_DataRate　サンプリング周波数
　L3GD20_OUTPUT_DATARATE_1　 95Hz
　L3GD20_OUTPUT_DATARATE_2　190Hz
　L3GD20_OUTPUT_DATARATE_3　380Hz
　L3GD20_OUTPUT_DATARATE_4　760Hz

(2)L3GD20_InitStructure.Band_Width　    Cut-Off[Hz]

DataRate=95 DataRate=190 DataRate=380 DataRate=760
　L3GD20_BANDWIDTH_1 12.5
 12.5
 20
 30
　L3GD20_BANDWIDTH_2 25
 25
 25
 35
　L3GD20_BANDWIDTH_3 25
 50
 50
 50
　L3GD20_BANDWIDTH_4 25
 70
 100
 100


ジャイロスコープから３軸周りの角速度の読み取りプログラム（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 "stm32f3_discovery_l3gd20.h"

void initGyroscope(void);
void readGyroscope_AngRate(float* pfData);

int main()
{
    float Buffer[3] = {0.0f};
    initGyroscope();
    while(1) {
        readGyroscope_AngRate(Buffer);
        printf("DDtheta x,y,z=%7.3f %7.3f %7.3f\n",Buffer[0],Buffer[1],Buffer[2]);
    }
}

/*ジャイロスコープインタフェイスの初期化*/
void initGyroscope(void)
{
    L3GD20_InitTypeDef L3GD20_InitStructure;
    L3GD20_FilterConfigTypeDef L3GD20_FilterStructure;

    /* Configure Mems L3GD20 */
    L3GD20_InitStructure.Power_Mode = L3GD20_MODE_ACTIVE;
    L3GD20_InitStructure.Output_DataRate = L3GD20_OUTPUT_DATARATE_1;
    L3GD20_InitStructure.Axes_Enable = L3GD20_AXES_ENABLE;
    L3GD20_InitStructure.Band_Width = L3GD20_BANDWIDTH_4;
    L3GD20_InitStructure.BlockData_Update = L3GD20_BlockDataUpdate_Continous;
    L3GD20_InitStructure.Endianness = L3GD20_BLE_LSB;
    L3GD20_InitStructure.Full_Scale = L3GD20_FULLSCALE_500;
    L3GD20_Init(&L3GD20_InitStructure);

    L3GD20_FilterStructure.HighPassFilter_Mode_Selection =L3GD20_HPM_NORMAL_MODE_RES;
    L3GD20_FilterStructure.HighPassFilter_CutOff_Frequency = L3GD20_HPFCF_0;
    L3GD20_FilterConfig(&L3GD20_FilterStructure) ;

    L3GD20_FilterCmd(L3GD20_HIGHPASSFILTER_ENABLE);
}

/*ジャイロスコープから3つの軸周りの角速度を読み取る*/
/*pfData[3]によって，x軸周り角速度，y軸周り角速度，z軸周り角速度を呼び出し側に持ち帰ってもらう*/
void readGyroscope_AngRate (float* pfData)
{
    #define L3G_Sensitivity_250dps     (float)   114.285f   /*!< gyroscope sensitivity with  250 dps full scale [LSB/dps] */
    #define L3G_Sensitivity_500dps     (float)    57.1429f  /*!< gyroscope sensitivity with  500 dps full scale [LSB/dps] */
    #define L3G_Sensitivity_2000dps    (float)    14.285f   /*!< gyroscope sensitivity with 2000 dps full scale [LSB/dps] */

    uint8_t tmpbuffer[6] ={0};
    int16_t RawData[3] = {0};
    uint8_t tmpreg = 0;
    float sensitivity = 0;
    int i =0;

    L3GD20_Read(&tmpreg,L3GD20_CTRL_REG4_ADDR,1);

    L3GD20_Read(tmpbuffer,L3GD20_OUT_X_L_ADDR,6);

    /* check in the control register 4 the data alignment (Big Endian or Little Endian)*/
    if(!(tmpreg & 0x40))
    {
        for(i=0; i<3; i++)
        {
            RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i+1] << 8) + tmpbuffer[2*i]);
        }
    }
    else
    {
        for(i=0; i<3; i++)
        {
            RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i] << 8) + tmpbuffer[2*i+1]);
        }
    }

    /* Switch the sensitivity value set in the CRTL4 */
    switch(tmpreg & 0x30)
    {
    case 0x00:
        sensitivity=L3G_Sensitivity_250dps;
        break;

    case 0x10:
        sensitivity=L3G_Sensitivity_500dps;
        break;

    case 0x20:
        sensitivity=L3G_Sensitivity_2000dps;
        break;
    }
    /* divide by sensitivity */
    for(i=0; i<3; i++)
    {
        pfData[i]=(float)RawData[i]/sensitivity;
    }
}

uint32_t L3GD20_TIMEOUT_UserCallback(void)
{
  return 0;
}



５．　各軸周りの角速度の測定例（取得の安定性と積分による角度の推定）
各軸周りの角速度を1secごとに取得し，10000秒(約2.8時間)の測定を行なう。

ジャイロスコープから３軸周りの角速度の読み取りプログラム（semihosting使用）
浮動小数点数を表示するためlib/stdio/printf.cをプロジェクトから削除してGCCのｐrintfを使う
/*
 * L3GD20 ３軸ジャイロセンサジュール

 */
/* Includes ------------------------------------------------------------------*/
#include <stm32f30x.h>
#include <stdint.h>
#include "stm32f30x_conf.h"
#include "stm32f3_discovery.h"
#include <stdio.h>

#include "stm32f3_discovery_l3gd20.h"

void initGyroscope(void);
void readGyroscope_AngRate(float* pfData);
void initIntervaltimer(int32_t frequency);

float Buffer[3] = {0.0,1.0,0.0};

volatile int request = 0; /*1:request for print 0:nothing*/

int main()
{
    initGyroscope();

    /* SysTick end of count event each 1ms (1000Hz)*/
    initIntervaltimer(1000);

    int time=0;
    printf("Gyroscope Test\r\n");
    printf("L3GD20  FULLSCALE = 500 deg/s\r\n");
    printf("time[sec],x[deg/s],y[deg/s],z[deg/s]\r\n");
    while(time<=10000) {
        if (request==1) {
            printf("%5d, %10.7f, %10.7f, %10.7f\n",time++,Buffer[0],Buffer[1],Buffer[2]);
            request = 0;
        }
    }
    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;
        readGyroscope_AngRate(Buffer);
         request = 1;
    }
}

void initGyroscope(void)
{
    L3GD20_InitTypeDef L3GD20_InitStructure;
    L3GD20_FilterConfigTypeDef L3GD20_FilterStructure;

    /* Configure Mems L3GD20 */
    L3GD20_InitStructure.Power_Mode = L3GD20_MODE_ACTIVE;
    L3GD20_InitStructure.Output_DataRate = L3GD20_OUTPUT_DATARATE_1;
    L3GD20_InitStructure.Axes_Enable = L3GD20_AXES_ENABLE;
    L3GD20_InitStructure.Band_Width = L3GD20_BANDWIDTH_4;
    L3GD20_InitStructure.BlockData_Update = L3GD20_BlockDataUpdate_Continous;
    L3GD20_InitStructure.Endianness = L3GD20_BLE_LSB;
    L3GD20_InitStructure.Full_Scale = L3GD20_FULLSCALE_500;
    L3GD20_Init(&L3GD20_InitStructure);

    L3GD20_FilterStructure.HighPassFilter_Mode_Selection =L3GD20_HPM_NORMAL_MODE_RES;
    L3GD20_FilterStructure.HighPassFilter_CutOff_Frequency = L3GD20_HPFCF_0;
    L3GD20_FilterConfig(&L3GD20_FilterStructure) ;

    L3GD20_FilterCmd(L3GD20_HIGHPASSFILTER_ENABLE);
}

void readGyroscope_AngRate(float* pfData)
{
    #define L3G_Sensitivity_250dps     (float)   114.285f         /*!< gyroscope sensitivity with 250 dps full scale [LSB/dps] */
    #define L3G_Sensitivity_500dps     (float)    57.1429f        /*!< gyroscope sensitivity with 500 dps full scale [LSB/dps] */
    #define L3G_Sensitivity_2000dps    (float)    14.285f          /*!< gyroscope sensitivity with 2000 dps full scale [LSB/dps] */

    uint8_t tmpbuffer[6] ={0};
    int16_t RawData[3] = {0};
    uint8_t tmpreg = 0;
    float sensitivity = 0;
    int i =0;

    L3GD20_Read(&tmpreg,L3GD20_CTRL_REG4_ADDR,1);

    L3GD20_Read(tmpbuffer,L3GD20_OUT_X_L_ADDR,6);

    /* check in the control register 4 the data alignment (Big Endian or Little Endian)*/
    if(!(tmpreg & 0x40))
    {
        for(i=0; i<3; i++)
        {
            RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i+1] << 8) + tmpbuffer[2*i]);
        }
    }
    else
    {
        for(i=0; i<3; i++)
        {
            RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i] << 8) + tmpbuffer[2*i+1]);
        }
    }

    /* Switch the sensitivity value set in the CRTL4 */
    switch(tmpreg & 0x30)
    {
    case 0x00:
        sensitivity=L3G_Sensitivity_250dps;
        break;

    case 0x10:
        sensitivity=L3G_Sensitivity_500dps;
        break;

    case 0x20:
        sensitivity=L3G_Sensitivity_2000dps;
        break;
    }

    /* divide by sensitivity */
    for(i=0; i<3; i++)
    {
        pfData[i]=(float)RawData[i]/sensitivity;
    }
}

uint32_t L3GD20_TIMEOUT_UserCallback(void)
{
  return 0;
}


このプログラムにより，角速度測定の安定性を２個のSTMF3DISCOVERYで調べてみた。
測定はSTMF3DISCOVERYを動かさないようにしておこなった。期待する測定値は，常時，各軸周り角速度=0，各軸周り角度=0である。

（１）　1個目のSTMF3DISCOVERY

ジャイロスコープから得られた3軸周りの角速度

上記角速度を積分（累積加算）して得た3軸周りの角度

角速度測定値の平均が，センサの持つオフセットと考えられ，積分（累積加算）はこのオフセットを除いて行なっている。
角速度測定値の標準偏差は次の通り
　　x軸周りの角速度標準偏差＝0.143
　　ｙ軸周りの角速度標準偏差＝0.112
　　ｚ軸周りの角速度標準偏差＝0.127
積分（累積加算）して得られた角度推定値は，0から離れてゆくため，このままでは角度推定値として使えない。

（2）　1個目のSTMF3DISCOVERY

ジャイロスコープから得られた3軸周りの角速度
　（8000秒付近の大きな値は予期せぬ振動をとらえたもので，影響はない）

上記角速度を積分（累積加算）して得た3軸周りの角度

角速度測定値の平均が，センサの持つオフセットと考えられ，積分（累積加算）はこのオフセットを除いて行なっている。
角速度測定値の標準偏差は次の通り
　　x軸周りの角速度標準偏差＝0.134
　　ｙ軸周りの角速度標準偏差＝0.119
　　ｚ軸周りの角速度標準偏差＝0.134
積分（累積加算）して得られた角度推定値は，0から離れてゆくため，このままでは角度推定値として使えない。

まとめ
ここで見たように，得られた軸周り角速度にはオフセットが有り，このオフセットは時間とともに変動（ドリフト）しているとかんがえられる。
軸周り角速度の誤差はオフセットを差し引いて，およそ±0.5deg/sである。
積分（累積加算）して得られた角度推定値は，0から離れてゆくため，このままでは角度推定値として使えない。