OpenGL + adxl345 -> 藉由adxl345來轉動你的茶壺

用adxl345 sensor  可以讀出來  x, y, z  的加速度....

因為地球有重力加速度(垂直往下)....

如果你把adxl345 平放在桌上....你可以發現...x 和 y 軸的加速度為 0....然後z 軸大概為 1G 左右

由於會有些noise.....這個需要做一下校正....

但沒有校正也可以啦....

從參考網址2來看

我們可以得到   roll 和 pitch 的計算公式


    roll = (atan2(-fYg, fZg)*180.0)/M_PI;
    pitch = (atan2(fXg, sqrt(fYg*fYg + fZg*fZg))*180.0)/M_PI;

   那甚麼是roll 和 pitch 呢?

   請看reference 3 的網站...他有動畫...一看就知道.....

  原理是當你轉動你的adxl345 

  由於地球有重心引力.....sensor 的 x, y ,z 的加速度 會因為你的轉動而可得到不同的分量

  所以我們可以從adxl345所量測到的x,y,z 的加速度...反推回去 目前sensor 和地球座標的夾角...

  那有了 roll 和 pitch....也只是一堆數字.....看起來好像不夠酷....

  沒關係....我記得我有在arduino 看到一個例子....adxl345 結合  teaspot....

   那raspberry 也可以畫  teaspot壓....

  找了一下資料...

  發覺在下面的目錄有opengl 的例子....

   /opt/vc/src/hello_pi

剛好有 hello_teapot.....真是太lucky.....

打開  triangle.c 來看.... 找一下我要改的地方....

static void update_model(CUBE_STATE_T *state,double roll, double pitch)

{

   // update position

   state->rot_angle_x = inc_and_wrap_angle(state->rot_angle_x, state->rot_angle_x_inc);

   state->rot_angle_y = inc_and_wrap_angle(state->rot_angle_y, state->rot_angle_y_inc);

   state->rot_angle_z = inc_and_wrap_angle(state->rot_angle_z, state->rot_angle_z_inc);

   state->distance    = inc_and_clip_distance(state->distance, state->distance_inc);

   glLoadIdentity();

   // move camera back to see the cube

   glTranslatef(0.f, 0.f, -state->distance);

   // Rotate model to new position

   glRotatef(state->rot_angle_x, 1.f, 0.f, 0.f);

   glRotatef(state->rot_angle_y, 0.f, 1.f, 0.f);

   glRotatef(state->rot_angle_z, 0.f, 0.f, 1.f);

}

可以看出來...  他會一直去抓   x 和 y 和 z 的angle......   然後 inc 他....

 所以只要在這邊動點手腳.....把最後送到  glRotatef 的值換掉....應該可以了....

修改的code 大概如下 :

static void update_model(CUBE_STATE_T *state,double roll, double pitch)

{

   // update position

   state->rot_angle_z = inc_and_wrap_angle(state->rot_angle_z, state->rot_angle_z_inc);

   state->distance    = inc_and_clip_distance(state->distance, state->distance_inc);

   glLoadIdentity();

   // move camera back to see the cube

   glTranslatef(0.f, 0.f, -state->distance);

   // Rotate model to new position

   glRotatef(roll, 1.f, 0.f, 0.f);

   glRotatef(pitch, 0.f, 1.f, 0.f);

   glRotatef(state->rot_angle_z, 0.f, 0.f, 1.f);

}

int main(){

.......

 struct acc_dat gdata;

  double alpha = 0.5f,roll,pitch,fXg = 0.0,fYg = 0.0,fZg = 0.0;

   while (!terminate)

   {

    gdata = getAxes(fd_adxl345, 1);

    // low pass filter ,  to remove noise

    fXg = gdata.x * alpha + (fXg * (1.0f - alpha));

    fYg = gdata.y * alpha + (fYg * (1.0f - alpha));

    fZg = gdata.z * alpha + (fZg * (1.0f - alpha));

    //Roll and Pitch Equations

    roll = (atan2(-fYg, fZg)*180.0)/M_PI;

    pitch = (atan2(fXg, sqrt(fYg*fYg + fZg*fZg))*180.0)/M_PI;

      update_model(state,roll,pitch);

      redraw_scene(state);

   }

}

新增一個  c 的 file   ->  adxl345.c

#include "./adxl345.h"

void enableMeasurement(char FD_ADDR)

{

     wiringPiI2CWriteReg8(FD_ADDR,POWER_CTL,MEASURE);

}

void setBandwidthRate(char FD_ADDR, int rate_flag)

{

     wiringPiI2CWriteReg8(FD_ADDR,BW_RATE,rate_flag);

}

void setRange(char FD_ADDR, int range_flag)

{

        int value;

        value = wiringPiI2CReadReg8(FD_ADDR, DATA_FORMAT);

        value &= ~0x0F;

        value |= range_flag;

        value |= 0x08;

//        printf("set range value : %d",value);

        wiringPiI2CWriteReg8(FD_ADDR, DATA_FORMAT, value);

}

struct acc_dat getAxes(char FD_ADDR, int gforce)

{

        double dx,dy,dz;

        struct acc_dat dat;

        int x,y,z;

        char bytes[6];

        bytes[0] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA);

        bytes[1] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+1);

        bytes[2] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+2);

        bytes[3] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+3);

        bytes[4] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+4);

        bytes[5] = wiringPiI2CReadReg8(FD_ADDR, AXES_DATA+5);

        //printf("%d\n",bytes[0]);

        //printf("%d\n",bytes[1]);

        x = bytes[0] | (bytes[1] << 8);

        if(x & (1<<15))

            x = x - (1<<16);

        //printf("%d\n",x);

        y = bytes[2] | (bytes[3] << 8);

        if(y & (1<<15))

            y = y - (1<<16);

        z = bytes[4] | (bytes[5] << 8);

        if(z & (1<<15))

            z = z - (1<<16);

        dx = (double)x * SCALE_MULTIPLIER;

        dy = (double)y * SCALE_MULTIPLIER;

        dz = (double)z * SCALE_MULTIPLIER;

        if (gforce == 0)

        {

            dx = dx * EARTH_GRAVITY_MS2;

            dy = dy * EARTH_GRAVITY_MS2;

            dz = dz * EARTH_GRAVITY_MS2;

        }

        dat.x = (dx);

        dat.y = (dy);

        dat.z = (dz);

        return dat;

}

然後 adxl345.h  裡面大概就是一些定義...這裡我就不貼了....

然後修改Makefile

OBJS=triangle.o video.o models.o adxl345.o
BIN=hello_teapot.bin
LDFLAGS+=-lilclient
LDFLAGS+=-lwiringPi

include ../Makefile.include

然後就是編譯......

sudo ./make

執行 

sudo ./hello_teapot.bin

你就可以在你的螢幕看到一個  teaspot....然後轉動你的 adxl345....你就可以看到茶壺也會跟著轉


demo 的影片在下面的連結

demo video:    https://www.youtube.com/watch?v=O3V-0z6UWF0

note 1.   teaspot 所需要用的gpu-mem 比較大....請把gpu mem 設到128M....   sudo raspi-config -> Advanced option -> ....

note 2.   我有使用 wiringPi 的library....請先安裝 wiringPi  ....  http://wiringpi.com/


Reference 2 : http://blog.oscarliang.net/use-gy80-arduino-adxl345-accelerometer/

Reference 3 : http://howthingsfly.si.edu/flight-dynamics/roll-pitch-and-yaw