HEPTA-Sat Training 2022 / Hepta9axis

9axis-sensor library

Dependents:   HEPTA_SENSOR

Hepta9axis.cpp@12:f52c96610b21, 2022-08-19 (annotated)

Committer:
RyusukeIwata
Date:
Fri Aug 19 02:44:02 2022 +0000
Revision:
12:f52c96610b21
Parent:
10:2b227e8928ae
Child:
13:e2d6617ed02a
changed by iwata

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