Masahiko Yamazaki / Hepta6axis

For Hepta-Sat Lite

Hepta6axis.cpp@10:e752c65c7c56, 2021-08-17 (annotated)

Committer:
heptasat2021
Date:
Tue Aug 17 10:51:08 2021 +0000
Revision:
10:e752c65c7c56
Parent:
Hepta9axis.cpp@6:fdfca3ffecd5
Child:
11:bdcabb617bfb
For Hepta-Sat Lite

Who changed what in which revision?

UserRevisionLine numberNew contents of line
heptasat2021 10:e752c65c7c56 1 #include"Hepta6axis.h"
hepta2ume 0:5aaec0996753 2 #include"mbed.h"
hepta2ume 0:5aaec0996753 3
heptasat2021 10:e752c65c7c56 4 Hepta6axis::Hepta6axis(PinName sda, PinName scl, int aaddr) : n_axis(sda,scl),addr_accel_gyro(aaddr)
heptasat2021 10:e752c65c7c56 5 {
heptasat2021 10:e752c65c7c56 6 /*n_axis.frequency(100000);
heptasat2021 10:e752c65c7c56 7 cmd[0]=0x6B;
heptasat2021 10:e752c65c7c56 8 cmd[1]=0x00;
heptasat2021 10:e752c65c7c56 9 n_axis.write(addr_accel_gyro,cmd,2);
heptasat2021 10:e752c65c7c56 10 cmd[0]=0x6C;
heptasat2021 10:e752c65c7c56 11 cmd[1]=0x00;
heptasat2021 10:e752c65c7c56 12 n_axis.write(addr_accel_gyro,cmd,2);
heptasat2021 10:e752c65c7c56 13 cmd[0] = 0x37;
heptasat2021 10:e752c65c7c56 14 cmd[1] = 0x02;
heptasat2021 10:e752c65c7c56 15 n_axis.write(addr_accel_gyro,cmd,2);
heptasat2021 10:e752c65c7c56 16 n_axis.stop();*/
heptasat2021 10:e752c65c7c56 17 }
heptasat2021 10:e752c65c7c56 18
heptasat2021 10:e752c65c7c56 19 void Hepta6axis::setup()
hepta2ume 0:5aaec0996753 20 {
HEPTA 3:d5eed0bb962e 21 n_axis.frequency(100000);
hepta2ume 0:5aaec0996753 22 cmd[0]=0x6B;
hepta2ume 0:5aaec0996753 23 cmd[1]=0x00;
HEPTA 3:d5eed0bb962e 24 n_axis.write(addr_accel_gyro,cmd,2);
hepta2ume 0:5aaec0996753 25 cmd[0] = 0x37;
hepta2ume 0:5aaec0996753 26 cmd[1] = 0x02;
HEPTA 3:d5eed0bb962e 27 n_axis.write(addr_accel_gyro,cmd,2);
HEPTA 3:d5eed0bb962e 28 n_axis.stop();
hepta2ume 0:5aaec0996753 29 }
hepta2ume 0:5aaec0996753 30
heptasat2021 10:e752c65c7c56 31 void Hepta6axis::sen_acc(float *ax,float *ay,float *az)
hepta2ume 0:5aaec0996753 32 {
heptasat2021 10:e752c65c7c56 33 setup();
heptasat2021 10:e752c65c7c56 34 setup();
HeptaSatTraining2019 6:fdfca3ffecd5 35 //x-axis accel
heptasat2021 10:e752c65c7c56 36 adata[0] = 0x3B;
heptasat2021 10:e752c65c7c56 37 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 38 n_axis.read(addr_accel_gyro|0x01,xh,1);
heptasat2021 10:e752c65c7c56 39 adata[0] = 0x3C;
heptasat2021 10:e752c65c7c56 40 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 41 n_axis.read(addr_accel_gyro|0x01,xl,1);
heptasat2021 10:e752c65c7c56 42 double acc_ax = short((xh[0]<<8) | (xl[0]));
heptasat2021 10:e752c65c7c56 43 *ax = (acc_ax)*4/32764*9.81;
hepta2ume 0:5aaec0996753 44
HeptaSatTraining2019 6:fdfca3ffecd5 45 //y-axis
heptasat2021 10:e752c65c7c56 46 adata[0] = 0x3D;
heptasat2021 10:e752c65c7c56 47 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 48 n_axis.read(addr_accel_gyro|0x01,yh,1);
heptasat2021 10:e752c65c7c56 49 adata[0] = 0x3E;
heptasat2021 10:e752c65c7c56 50 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 51 n_axis.read(addr_accel_gyro|0x01,yl,1);
heptasat2021 10:e752c65c7c56 52 double acc_ay = short((yh[0]<<8) | (yl[0]));
heptasat2021 10:e752c65c7c56 53 *ay = (acc_ay)*4/32764*9.81;
hepta2ume 0:5aaec0996753 54
HeptaSatTraining2019 6:fdfca3ffecd5 55 //z-axis
heptasat2021 10:e752c65c7c56 56 adata[0] = 0x3F;
heptasat2021 10:e752c65c7c56 57 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 58 n_axis.read(addr_accel_gyro|0x01,zh,1);
heptasat2021 10:e752c65c7c56 59 adata[0] = 0x40;
heptasat2021 10:e752c65c7c56 60 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 61 n_axis.read(addr_accel_gyro|0x01,zl,1);
heptasat2021 10:e752c65c7c56 62 double acc_az = short((zh[0]<<8) | (zl[0]));
heptasat2021 10:e752c65c7c56 63 *az = (acc_az)*4/32764*9.81;
hepta2ume 0:5aaec0996753 64 }
hepta2ume 0:5aaec0996753 65
heptasat2021 10:e752c65c7c56 66 void Hepta6axis::sen_gyro(float *gx,float *gy,float *gz)
hepta2ume 0:5aaec0996753 67 {
heptasat2021 10:e752c65c7c56 68 setup();
heptasat2021 10:e752c65c7c56 69 setup();
heptasat2021 10:e752c65c7c56 70 //x-axis gyro
heptasat2021 10:e752c65c7c56 71 adata[0] = 0x43;
heptasat2021 10:e752c65c7c56 72 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 73 n_axis.read(addr_accel_gyro|0x01,gxh,1);
heptasat2021 10:e752c65c7c56 74 adata[0] = 0x44;
heptasat2021 10:e752c65c7c56 75 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 76 n_axis.read(addr_accel_gyro|0x01,gxl,1);
heptasat2021 10:e752c65c7c56 77 double gyro_ax = short((gxh[0]<<8) | (gxl[0]));
heptasat2021 10:e752c65c7c56 78 *gx = (gyro_ax)*0.01526;
HEPTA 4:01941772f493 79
heptasat2021 10:e752c65c7c56 80 //y-axis gyro
heptasat2021 10:e752c65c7c56 81 adata[0] = 0x45;
heptasat2021 10:e752c65c7c56 82 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 83 n_axis.read(addr_accel_gyro|0x01,gyh,1);
heptasat2021 10:e752c65c7c56 84 adata[0] = 0x46;
heptasat2021 10:e752c65c7c56 85 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 86 n_axis.read(addr_accel_gyro|0x01,gyl,1);
heptasat2021 10:e752c65c7c56 87 double gyro_ay = short((gyh[0]<<8) | (gyl[0]));
heptasat2021 10:e752c65c7c56 88 *gy = (gyro_ay)*0.01526;
heptasat2021 10:e752c65c7c56 89
heptasat2021 10:e752c65c7c56 90 //z-axis gyro
heptasat2021 10:e752c65c7c56 91 adata[0] = 0x47;
heptasat2021 10:e752c65c7c56 92 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 93 n_axis.read(addr_accel_gyro|0x01,gzh,1);
heptasat2021 10:e752c65c7c56 94 adata[0] = 0x48;
heptasat2021 10:e752c65c7c56 95 n_axis.write(addr_accel_gyro,adata,1);
heptasat2021 10:e752c65c7c56 96 n_axis.read(addr_accel_gyro|0x01,gzl,1);
heptasat2021 10:e752c65c7c56 97 double gyro_az = short((gzh[0]<<8) | (gzl[0]));
heptasat2021 10:e752c65c7c56 98 *gz = (gyro_az)*0.01526;
hepta2ume 0:5aaec0996753 99 }
hepta2ume 0:5aaec0996753 100
HeptaSatTraining2019 6:fdfca3ffecd5 101 //////////////For HEX/////////////////////////////////////////
heptasat2021 10:e752c65c7c56 102 /*void Hepta6axis::sen_gyro_u16(char* gx_u16,char* gy_u16,char* gz_u16)
hepta2ume 0:5aaec0996753 103 {
HeptaSatTraining2019 6:fdfca3ffecd5 104 //x
HEPTA 3:d5eed0bb962e 105 n_axis.start();
HEPTA 3:d5eed0bb962e 106 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 107 n_axis.write(0x43);
HEPTA 3:d5eed0bb962e 108 n_axis.start();
HEPTA 3:d5eed0bb962e 109 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 110 gxh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 111 n_axis.stop();
umeume 2:306058b9d04e 112
HEPTA 3:d5eed0bb962e 113 n_axis.start();
HEPTA 3:d5eed0bb962e 114 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 115 n_axis.write(0x44);
HEPTA 3:d5eed0bb962e 116 n_axis.start();
HEPTA 3:d5eed0bb962e 117 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 118 gxl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 119 n_axis.stop();
umeume 2:306058b9d04e 120
hepta2ume 0:5aaec0996753 121 sprintf( g1, "%02X", ((gxh)) & 0xFF);
hepta2ume 0:5aaec0996753 122 sprintf( g2, "%02X", ((gxl)) & 0xFF);
hepta2ume 0:5aaec0996753 123 gx_u16[0]=g1[0];
hepta2ume 0:5aaec0996753 124 gx_u16[1]=g1[1];
hepta2ume 0:5aaec0996753 125 gx_u16[2]=g2[0];
hepta2ume 0:5aaec0996753 126 gx_u16[3]=g2[1];
hepta2ume 0:5aaec0996753 127
HeptaSatTraining2019 6:fdfca3ffecd5 128 //y
HEPTA 3:d5eed0bb962e 129 n_axis.start();
HEPTA 3:d5eed0bb962e 130 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 131 n_axis.write(0x45);
HEPTA 3:d5eed0bb962e 132 n_axis.start();
HEPTA 3:d5eed0bb962e 133 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 134 gyh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 135 n_axis.stop();
umeume 2:306058b9d04e 136
HEPTA 3:d5eed0bb962e 137 n_axis.start();
HEPTA 3:d5eed0bb962e 138 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 139 n_axis.write(0x46);
HEPTA 3:d5eed0bb962e 140 n_axis.start();
HEPTA 3:d5eed0bb962e 141 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 142 gyl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 143 n_axis.stop();
hepta2ume 1:5b35162b3b6a 144 sprintf( g1, "%02X", (gyh) & 0xFF);
hepta2ume 1:5b35162b3b6a 145 sprintf( g2, "%02X", (gyl) & 0xFF);
hepta2ume 0:5aaec0996753 146 gy_u16[0]=g1[0];
hepta2ume 0:5aaec0996753 147 gy_u16[1]=g1[1];
hepta2ume 0:5aaec0996753 148 gy_u16[2]=g2[0];
hepta2ume 0:5aaec0996753 149 gy_u16[3]=g2[1];
umeume 2:306058b9d04e 150
HeptaSatTraining2019 6:fdfca3ffecd5 151 //z
HEPTA 3:d5eed0bb962e 152 n_axis.start();
HEPTA 3:d5eed0bb962e 153 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 154 n_axis.write(0x47);
HEPTA 3:d5eed0bb962e 155 n_axis.start();
HEPTA 3:d5eed0bb962e 156 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 157 gzh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 158 n_axis.stop();
umeume 2:306058b9d04e 159
HEPTA 3:d5eed0bb962e 160 n_axis.start();
HEPTA 3:d5eed0bb962e 161 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 162 n_axis.write(0x48);
HEPTA 3:d5eed0bb962e 163 n_axis.start();
HEPTA 3:d5eed0bb962e 164 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 165 gzl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 166 n_axis.stop();
umeume 2:306058b9d04e 167
hepta2ume 0:5aaec0996753 168 sprintf( g1, "%02X", ((gzh)) & 0xFF);
hepta2ume 0:5aaec0996753 169 sprintf( g2, "%02X", ((gzl)) & 0xFF);
hepta2ume 0:5aaec0996753 170 gz_u16[0]=g1[0];
hepta2ume 0:5aaec0996753 171 gz_u16[1]=g1[1];
hepta2ume 0:5aaec0996753 172 gz_u16[2]=g2[0];
hepta2ume 0:5aaec0996753 173 gz_u16[3]=g2[1];
HEPTA 5:098423fb0371 174 //*dsize = 4;
umeume 2:306058b9d04e 175 }
umeume 2:306058b9d04e 176
heptasat2021 10:e752c65c7c56 177 void Hepta6axis::sen_acc_u16(char* ax_u16,char* ay_u16,char* az_u16)
hepta2ume 0:5aaec0996753 178 {
HeptaSatTraining2019 6:fdfca3ffecd5 179 //x
HEPTA 3:d5eed0bb962e 180 n_axis.start();
HEPTA 3:d5eed0bb962e 181 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 182 n_axis.write(0x3B);
HEPTA 3:d5eed0bb962e 183 n_axis.start();
HEPTA 3:d5eed0bb962e 184 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 185 xh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 186 n_axis.stop();
hepta2ume 0:5aaec0996753 187
HEPTA 3:d5eed0bb962e 188 n_axis.start();
HEPTA 3:d5eed0bb962e 189 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 190 n_axis.write(0x3C);
HEPTA 3:d5eed0bb962e 191 n_axis.start();
HEPTA 3:d5eed0bb962e 192 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 193 xl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 194 n_axis.stop();
umeume 2:306058b9d04e 195
hepta2ume 0:5aaec0996753 196 sprintf( a1, "%02X", ((xh)) & 0xFF);
hepta2ume 0:5aaec0996753 197 sprintf( a2, "%02X", ((xl)) & 0xFF);
hepta2ume 0:5aaec0996753 198 ax_u16[0]=a1[0];
hepta2ume 0:5aaec0996753 199 ax_u16[1]=a1[1];
hepta2ume 0:5aaec0996753 200 ax_u16[2]=a2[0];
hepta2ume 0:5aaec0996753 201 ax_u16[3]=a2[1];
hepta2ume 0:5aaec0996753 202
HeptaSatTraining2019 6:fdfca3ffecd5 203 //y
HEPTA 3:d5eed0bb962e 204 n_axis.start();
HEPTA 3:d5eed0bb962e 205 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 206 n_axis.write(0x3D);
HEPTA 3:d5eed0bb962e 207 n_axis.start();
HEPTA 3:d5eed0bb962e 208 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 209 yh = n_axis.read(0);
umeume 2:306058b9d04e 210
HEPTA 3:d5eed0bb962e 211 n_axis.stop();
HEPTA 3:d5eed0bb962e 212 n_axis.start();
HEPTA 3:d5eed0bb962e 213 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 214 n_axis.write(0x3E);
HEPTA 3:d5eed0bb962e 215 n_axis.start();
HEPTA 3:d5eed0bb962e 216 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 217 yl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 218 n_axis.stop();
umeume 2:306058b9d04e 219
hepta2ume 0:5aaec0996753 220 sprintf( a1, "%02X", ((yh)) & 0xFF);
hepta2ume 0:5aaec0996753 221 sprintf( a2, "%02X", ((yl)) & 0xFF);
hepta2ume 0:5aaec0996753 222 ay_u16[0]=a1[0];
hepta2ume 0:5aaec0996753 223 ay_u16[1]=a1[1];
hepta2ume 0:5aaec0996753 224 ay_u16[2]=a2[0];
hepta2ume 0:5aaec0996753 225 ay_u16[3]=a2[1];
HeptaSatTraining2019 6:fdfca3ffecd5 226 //z
HEPTA 3:d5eed0bb962e 227 n_axis.start();
HEPTA 3:d5eed0bb962e 228 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 229 n_axis.write(0x3F);
HEPTA 3:d5eed0bb962e 230 n_axis.start();
HEPTA 3:d5eed0bb962e 231 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 232 zh = n_axis.read(0);
HEPTA 3:d5eed0bb962e 233 n_axis.stop();
umeume 2:306058b9d04e 234
HEPTA 3:d5eed0bb962e 235 n_axis.start();
HEPTA 3:d5eed0bb962e 236 n_axis.write(addr_accel_gyro);
HEPTA 3:d5eed0bb962e 237 n_axis.write(0x40);
HEPTA 3:d5eed0bb962e 238 n_axis.start();
HEPTA 3:d5eed0bb962e 239 n_axis.write(addr_accel_gyro|0x01);
HEPTA 3:d5eed0bb962e 240 zl = n_axis.read(0);
HEPTA 3:d5eed0bb962e 241 n_axis.stop();
umeume 2:306058b9d04e 242
hepta2ume 0:5aaec0996753 243 sprintf( a1, "%02X", ((zh)) & 0xFF);
hepta2ume 0:5aaec0996753 244 sprintf( a2, "%02X", ((zl)) & 0xFF);
hepta2ume 0:5aaec0996753 245 az_u16[0]=a1[0];
hepta2ume 0:5aaec0996753 246 az_u16[1]=a1[1];
hepta2ume 0:5aaec0996753 247 az_u16[2]=a2[0];
hepta2ume 0:5aaec0996753 248 az_u16[3]=a2[1];
HEPTA 5:098423fb0371 249 //*dsize = 4;
heptasat2021 10:e752c65c7c56 250 }*/