HEPTA-Sat Training 2022 / Hepta9axis

9axis-sensor library

Dependents:   HEPTA_SENSOR

Hepta9axis.cpp@13:e2d6617ed02a, 22 months ago (annotated)

Committer:
RyusukeIwata
Date:
Fri Aug 19 03:00:21 2022 +0000
Revision:
13:e2d6617ed02a
Parent:
12:f52c96610b21 
changed ver 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hepta2ume 0:5aaec0996753 1 #include"Hepta9axis.h"
hepta2ume 0:5aaec0996753 2 #include"mbed.h"
csmk18112 10:2b227e8928ae 3 #define ACC 0x19<<1 //addr_accel
csmk18112 10:2b227e8928ae 4 #define MAG 0x13<<1 //addr_compus
csmk18112 10:2b227e8928ae 5 #define GYRO 0x69<<1 //addr_gyro
hepta2ume 0:5aaec0996753 6
RyusukeIwata 12:f52c96610b21 7 Hepta9axis::Hepta9axis(PinName sda, PinName scl, int aaddr, int agaddr, int amaddr) : n_axis(sda,scl),addr_accel(aaddr),addr_gyro(agaddr),addr_compus(amaddr)
hepta2ume 0:5aaec0996753 8 {
HEPTA 3:d5eed0bb962e 9 n_axis.frequency(100000);
csmk18112 10:2b227e8928ae 10 cmd[0]=0x14; //softreset
csmk18112 10:2b227e8928ae 11 cmd[1]=0xB6; //triggers a reset
csmk18112 10:2b227e8928ae 12 n_axis.write(ACC,cmd,2);
csmk18112 10:2b227e8928ae 13 cmd[0]=0x0F; //acceleration measurement range
csmk18112 10:2b227e8928ae 14 cmd[1]=0x05; //+-4g
csmk18112 10:2b227e8928ae 15 n_axis.write(ACC,cmd,2);
csmk18112 10:2b227e8928ae 16 cmd[0]=0x11; //Selection of the main power modes and low power sleep period
csmk18112 10:2b227e8928ae 17 cmd[1]=0x00; //NORMAL mode, Sleep duration = 0.5ms
csmk18112 10:2b227e8928ae 18 n_axis.write(ACC,cmd,2);
hepta2ume 0:5aaec0996753 19 }
hepta2ume 0:5aaec0996753 20
umeume 2:306058b9d04e 21 void Hepta9axis::setup()
hepta2ume 0:5aaec0996753 22 {
HEPTA 3:d5eed0bb962e 23 n_axis.frequency(100000);
csmk18112 10:2b227e8928ae 24 cmd[0]=0x14; //softreset
csmk18112 10:2b227e8928ae 25 cmd[1]=0xB6; //triggers a reset
csmk18112 10:2b227e8928ae 26 n_axis.write(ACC,cmd,2);
csmk18112 10:2b227e8928ae 27 cmd[0]=0x0F; //acceleration measurement range
csmk18112 10:2b227e8928ae 28 cmd[1]=0x05; //+-4g
csmk18112 10:2b227e8928ae 29 n_axis.write(ACC,cmd,2);
csmk18112 10:2b227e8928ae 30 cmd[0]=0x11; //Selection of the main power modes and low power sleep period
csmk18112 10:2b227e8928ae 31 cmd[1]=0x00; //NORMAL mode, Sleep duration = 0.5ms
csmk18112 10:2b227e8928ae 32 n_axis.write(ACC,cmd,2);
hepta2ume 0:5aaec0996753 33 }
hepta2ume 0:5aaec0996753 34
umeume 2:306058b9d04e 35 void Hepta9axis::sen_acc(float *ax,float *ay,float *az)
hepta2ume 0:5aaec0996753 36 {
csmk18112 10:2b227e8928ae 37 send[0]=(char)(2);
csmk18112 10:2b227e8928ae 38 n_axis.write(ACC,send,1,true);
csmk18112 10:2b227e8928ae 39 n_axis.read(ACC,(char*)data,6);
csmk18112 10:2b227e8928ae 40 for(int i=0;i<3;i++){
csmk18112 10:2b227e8928ae 41 accel[i]=(int16_t)(((int16_t)data[i*2+1]<<8) | data[i*2]) >> 4;
csmk18112 10:2b227e8928ae 42 if(accel[i]>2047)accel[i]-=4096;
csmk18112 10:2b227e8928ae 43 accel[i]= accel[i]/512*9.8;
csmk18112 10:2b227e8928ae 44 }
csmk18112 10:2b227e8928ae 45 *ax = accel[0];
csmk18112 10:2b227e8928ae 46 *ay = accel[1];
csmk18112 10:2b227e8928ae 47 *az = accel[2];
hepta2ume 0:5aaec0996753 48 }
hepta2ume 0:5aaec0996753 49
umeume 2:306058b9d04e 50 void Hepta9axis::sen_gyro(float *gx,float *gy,float *gz)
hepta2ume 0:5aaec0996753 51 {
csmk18112 10:2b227e8928ae 52 cmd[0]=0x0F;
csmk18112 10:2b227e8928ae 53 cmd[1]=0x04;
csmk18112 10:2b227e8928ae 54 n_axis.write(GYRO,cmd,2);
csmk18112 10:2b227e8928ae 55 cmd[0]=0x10;
csmk18112 10:2b227e8928ae 56 cmd[1]=0x07;
csmk18112 10:2b227e8928ae 57 n_axis.write(GYRO,cmd,2);
csmk18112 10:2b227e8928ae 58 cmd[0]=0x11;
csmk18112 10:2b227e8928ae 59 cmd[1]=0x00;
csmk18112 10:2b227e8928ae 60 n_axis.write(GYRO,cmd,2);
csmk18112 10:2b227e8928ae 61
csmk18112 10:2b227e8928ae 62 for(int i=0;i<6;i++){
csmk18112 10:2b227e8928ae 63 send[0]=(char)(2+i);
csmk18112 10:2b227e8928ae 64 n_axis.write(GYRO,send,1);
csmk18112 10:2b227e8928ae 65 n_axis.read(GYRO,get,1);
csmk18112 10:2b227e8928ae 66 temp=get[0];
csmk18112 10:2b227e8928ae 67 data[i]=temp;
csmk18112 10:2b227e8928ae 68 }
csmk18112 10:2b227e8928ae 69 for(int i=0;i<3;i++){
csmk18112 10:2b227e8928ae 70 gyroscope[i]=(int16_t)(((int16_t)data[i*2+1]<<8) | data[i*2]) >> 4;
csmk18112 10:2b227e8928ae 71 if(gyroscope[i]>32767)gyroscope[i]-=65536;
csmk18112 10:2b227e8928ae 72 gyroscope[i]=gyroscope[i]*125/2048;
csmk18112 10:2b227e8928ae 73 }
csmk18112 10:2b227e8928ae 74 //printf("gx = %2.4f, gy = %2.4f, gz = %2.4f\r\n",gyroscope[0],gyroscope[1],gyroscope[2]);
csmk18112 10:2b227e8928ae 75 *gx = gyroscope[0];
csmk18112 10:2b227e8928ae 76 *gy = gyroscope[1];
csmk18112 10:2b227e8928ae 77 *gz = gyroscope[2];
hepta2ume 0:5aaec0996753 78 }
hepta2ume 0:5aaec0996753 79
umeume 2:306058b9d04e 80 void Hepta9axis::sen_mag(float *mx,float *my,float *mz)
hepta2ume 0:5aaec0996753 81 {
csmk18112 10:2b227e8928ae 82 cmd[0]=0x4B;
csmk18112 10:2b227e8928ae 83 cmd[1]=0x01;
csmk18112 10:2b227e8928ae 84 n_axis.write(MAG,cmd,2);
csmk18112 10:2b227e8928ae 85 cmd[0]=0x4C;
csmk18112 10:2b227e8928ae 86 cmd[1]=0x00;
csmk18112 10:2b227e8928ae 87 n_axis.write(MAG,cmd,2);
csmk18112 10:2b227e8928ae 88 cmd[0]=0x4E;
csmk18112 10:2b227e8928ae 89 cmd[1]=0x84;
csmk18112 10:2b227e8928ae 90 n_axis.write(MAG,cmd,2);
csmk18112 10:2b227e8928ae 91 cmd[0]=0x51;
csmk18112 10:2b227e8928ae 92 cmd[1]=0x04;
csmk18112 10:2b227e8928ae 93 n_axis.write(MAG,cmd,2);
csmk18112 10:2b227e8928ae 94 cmd[0]=0x52;
csmk18112 10:2b227e8928ae 95 cmd[1]=0x16;
csmk18112 10:2b227e8928ae 96 n_axis.write(MAG,cmd,2);
csmk18112 10:2b227e8928ae 97 cmd[0]=0x00;
csmk18112 10:2b227e8928ae 98 n_axis.write(MAG,cmd,1,1);
csmk18112 10:2b227e8928ae 99 n_axis.read(MAG,cmd,1);
HEPTA 4:01941772f493 100
csmk18112 10:2b227e8928ae 101 for(int i=0;i<8;i++){
csmk18112 10:2b227e8928ae 102 send[0]=(char)(0x42+i);
csmk18112 10:2b227e8928ae 103 n_axis.write(MAG,send,1);
csmk18112 10:2b227e8928ae 104 n_axis.read(MAG,get,1);
csmk18112 10:2b227e8928ae 105 temp=get[0];
csmk18112 10:2b227e8928ae 106 data[i]=temp;
csmk18112 10:2b227e8928ae 107 }
csmk18112 10:2b227e8928ae 108 for(int i=0;i<3;i++){
csmk18112 10:2b227e8928ae 109 if(i!=2)magnet[i]=(int16_t)(((int16_t)data[i*2+1]<<8) | data[i*2]) >> 3;
csmk18112 10:2b227e8928ae 110 else magnet[i]=(int16_t)(((int16_t)data[i*2+1]<<8) | data[i*2]) >> 1;
csmk18112 10:2b227e8928ae 111 if(i==2 && magnet[i]>16383)magnet[i]-=32768;
csmk18112 10:2b227e8928ae 112 else if(i!=2 && magnet[i]>4095)magnet[i]-=8092;
csmk18112 10:2b227e8928ae 113 }
csmk18112 10:2b227e8928ae 114 //printf("mx = %2.4f, my = %2.4f, mz = %2.4f\r\n\n",magnet[0],magnet[1],magnet[2]);
csmk18112 10:2b227e8928ae 115 *mx = magnet[0];
csmk18112 10:2b227e8928ae 116 *my = magnet[1];
csmk18112 10:2b227e8928ae 117 *mz = magnet[2];
umeume 2:306058b9d04e 118 }