Masahiko Yamazaki / Hepta6axis

For Hepta-Sat Lite

Hepta6axis.cpp@12:811d24f1d74e, 2021-10-19 (annotated)

Committer:
heptasat2021
Date:
Tue Oct 19 06:17:26 2021 +0000
Revision:
12:811d24f1d74e
Parent:
11:bdcabb617bfb 
hepta-sat lite fix(20211019)

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