hidaka sato / JY901

unfinished

Dependents:   WRS_mechanamu_test WRS2019_master mbed_2018 mbed_2019_rx3 ... more

JY901.cpp@1:6bcbd18a719a, 2018-02-04 (annotated)

Committer:
sgrsn
Date:
Sun Feb 04 13:41:04 2018 +0000
Revision:
1:6bcbd18a719a
Parent:
0:6ff4cad60b67
Child:
2:5af50b300137
little little change

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:6ff4cad60b67 1 #include "JY901.h"
sgrsn 0:6ff4cad60b67 2
sgrsn 0:6ff4cad60b67 3 JY901::JY901(PinName sda, PinName scl) : I2C(sda, scl)
sgrsn 0:6ff4cad60b67 4 {
sgrsn 0:6ff4cad60b67 5 }
sgrsn 0:6ff4cad60b67 6
sgrsn 1:6bcbd18a719a 7 void JY901::calibrateGyroAccel()
sgrsn 0:6ff4cad60b67 8 {
sgrsn 0:6ff4cad60b67 9 start();
sgrsn 0:6ff4cad60b67 10 write(IICADDR);
sgrsn 0:6ff4cad60b67 11 write(CALSW);
sgrsn 0:6ff4cad60b67 12 write(0x01);
sgrsn 0:6ff4cad60b67 13 write(0x00);
sgrsn 0:6ff4cad60b67 14 stop();
sgrsn 0:6ff4cad60b67 15 }
sgrsn 0:6ff4cad60b67 16
sgrsn 1:6bcbd18a719a 17 void JY901::calibrateMagnetic()
sgrsn 0:6ff4cad60b67 18 {
sgrsn 0:6ff4cad60b67 19 start();
sgrsn 0:6ff4cad60b67 20 write(IICADDR);
sgrsn 0:6ff4cad60b67 21 write(CALSW);
sgrsn 0:6ff4cad60b67 22 write(0x02);
sgrsn 0:6ff4cad60b67 23 write(0x00);
sgrsn 0:6ff4cad60b67 24 stop();
sgrsn 0:6ff4cad60b67 25 }
sgrsn 0:6ff4cad60b67 26
sgrsn 1:6bcbd18a719a 27 void JY901::calibrateHeight()
sgrsn 0:6ff4cad60b67 28 {
sgrsn 0:6ff4cad60b67 29 start();
sgrsn 0:6ff4cad60b67 30 write(IICADDR);
sgrsn 0:6ff4cad60b67 31 write(CALSW);
sgrsn 0:6ff4cad60b67 32 write(0x03);
sgrsn 0:6ff4cad60b67 33 write(0x00);
sgrsn 0:6ff4cad60b67 34 stop();
sgrsn 0:6ff4cad60b67 35 }
sgrsn 0:6ff4cad60b67 36
sgrsn 1:6bcbd18a719a 37 void JY901::endCalibrate()
sgrsn 0:6ff4cad60b67 38 {
sgrsn 0:6ff4cad60b67 39 start();
sgrsn 0:6ff4cad60b67 40 write(IICADDR);
sgrsn 0:6ff4cad60b67 41 write(CALSW);
sgrsn 0:6ff4cad60b67 42 write(0x00);
sgrsn 0:6ff4cad60b67 43 write(0x00);
sgrsn 0:6ff4cad60b67 44 stop();
sgrsn 0:6ff4cad60b67 45 }
sgrsn 0:6ff4cad60b67 46
sgrsn 1:6bcbd18a719a 47 void JY901::calibrateAll(int time_ms)
sgrsn 0:6ff4cad60b67 48 {
sgrsn 1:6bcbd18a719a 49 calibrateGyroAccel();
sgrsn 1:6bcbd18a719a 50 calibrateMagnetic();
sgrsn 1:6bcbd18a719a 51 calibrateHeight();
sgrsn 1:6bcbd18a719a 52 wait_ms(time_ms);
sgrsn 1:6bcbd18a719a 53 endCalibrate();
sgrsn 0:6ff4cad60b67 54 }
sgrsn 0:6ff4cad60b67 55
sgrsn 0:6ff4cad60b67 56 int JY901::getYear()
sgrsn 0:6ff4cad60b67 57 {
sgrsn 0:6ff4cad60b67 58 char *data = getdata(YYMM);
sgrsn 0:6ff4cad60b67 59 return (int)(*data);
sgrsn 0:6ff4cad60b67 60 }
sgrsn 0:6ff4cad60b67 61
sgrsn 0:6ff4cad60b67 62 int JY901::getMonth()
sgrsn 0:6ff4cad60b67 63 {
sgrsn 0:6ff4cad60b67 64 char *data = getdata(YYMM);
sgrsn 0:6ff4cad60b67 65 return (int)(*(data+1));
sgrsn 0:6ff4cad60b67 66 }
sgrsn 0:6ff4cad60b67 67
sgrsn 0:6ff4cad60b67 68 int JY901::getDay()
sgrsn 0:6ff4cad60b67 69 {
sgrsn 0:6ff4cad60b67 70 char *data = getdata(DDHH);
sgrsn 0:6ff4cad60b67 71 return (int)(*data);
sgrsn 0:6ff4cad60b67 72 }
sgrsn 0:6ff4cad60b67 73
sgrsn 0:6ff4cad60b67 74 int JY901::getHour()
sgrsn 0:6ff4cad60b67 75 {
sgrsn 0:6ff4cad60b67 76 char *data = getdata(DDHH);
sgrsn 0:6ff4cad60b67 77 return (int)(*(data+1));
sgrsn 0:6ff4cad60b67 78 }
sgrsn 0:6ff4cad60b67 79
sgrsn 0:6ff4cad60b67 80 int JY901::getMinute()
sgrsn 0:6ff4cad60b67 81 {
sgrsn 0:6ff4cad60b67 82 char *data = getdata(MMSS);
sgrsn 0:6ff4cad60b67 83 return (int)(*data);
sgrsn 0:6ff4cad60b67 84 }
sgrsn 0:6ff4cad60b67 85
sgrsn 0:6ff4cad60b67 86 int JY901::getSecond()
sgrsn 0:6ff4cad60b67 87 {
sgrsn 0:6ff4cad60b67 88 char *data = getdata(MMSS);
sgrsn 0:6ff4cad60b67 89 return (int)(*(data+1));
sgrsn 0:6ff4cad60b67 90 }
sgrsn 0:6ff4cad60b67 91
sgrsn 0:6ff4cad60b67 92 int JY901::getMillisecond()
sgrsn 0:6ff4cad60b67 93 {
sgrsn 0:6ff4cad60b67 94 char *data = getdata(MS);
sgrsn 0:6ff4cad60b67 95 return (int)((*(data+1) << 8) | *data);
sgrsn 0:6ff4cad60b67 96 }
sgrsn 0:6ff4cad60b67 97
sgrsn 0:6ff4cad60b67 98 float JY901::getXaxisAcceleration()
sgrsn 0:6ff4cad60b67 99 {
sgrsn 0:6ff4cad60b67 100 char *data = getdata(AX);
sgrsn 0:6ff4cad60b67 101 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
sgrsn 0:6ff4cad60b67 102 }
sgrsn 0:6ff4cad60b67 103
sgrsn 0:6ff4cad60b67 104 float JY901::getYaxisAcceleration()
sgrsn 0:6ff4cad60b67 105 {
sgrsn 0:6ff4cad60b67 106 char *data = getdata(AY);
sgrsn 0:6ff4cad60b67 107 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
sgrsn 0:6ff4cad60b67 108 }
sgrsn 0:6ff4cad60b67 109
sgrsn 0:6ff4cad60b67 110 float JY901::getZaxisAcceleration()
sgrsn 0:6ff4cad60b67 111 {
sgrsn 0:6ff4cad60b67 112 char *data = getdata(AZ);
sgrsn 0:6ff4cad60b67 113 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
sgrsn 0:6ff4cad60b67 114 }
sgrsn 0:6ff4cad60b67 115
sgrsn 0:6ff4cad60b67 116 float JY901::getXaxisAngularVelocity()
sgrsn 0:6ff4cad60b67 117 {
sgrsn 0:6ff4cad60b67 118 char *data = getdata(GX);
sgrsn 0:6ff4cad60b67 119 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
sgrsn 0:6ff4cad60b67 120 }
sgrsn 0:6ff4cad60b67 121
sgrsn 0:6ff4cad60b67 122 float JY901::getYaxisAngularVelocity()
sgrsn 0:6ff4cad60b67 123 {
sgrsn 0:6ff4cad60b67 124 char *data = getdata(GY);
sgrsn 0:6ff4cad60b67 125 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
sgrsn 0:6ff4cad60b67 126 }
sgrsn 0:6ff4cad60b67 127
sgrsn 0:6ff4cad60b67 128 float JY901::getZaxisAngularVelocity()
sgrsn 0:6ff4cad60b67 129 {
sgrsn 0:6ff4cad60b67 130 char *data = getdata(GZ);
sgrsn 0:6ff4cad60b67 131 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
sgrsn 0:6ff4cad60b67 132 }
sgrsn 0:6ff4cad60b67 133
sgrsn 0:6ff4cad60b67 134 float JY901::getXaxisMagnetic()
sgrsn 0:6ff4cad60b67 135 {
sgrsn 0:6ff4cad60b67 136 char *data = getdata(HX);
sgrsn 0:6ff4cad60b67 137 return (float)((*(data+1) << 8) | *data);
sgrsn 0:6ff4cad60b67 138 }
sgrsn 0:6ff4cad60b67 139
sgrsn 0:6ff4cad60b67 140 float JY901::getYaxisMagnetic()
sgrsn 0:6ff4cad60b67 141 {
sgrsn 0:6ff4cad60b67 142 char *data = getdata(HY);
sgrsn 0:6ff4cad60b67 143 return (float)((*(data+1) << 8) | *data);
sgrsn 0:6ff4cad60b67 144 }
sgrsn 0:6ff4cad60b67 145
sgrsn 0:6ff4cad60b67 146 float JY901::getZaxisMagnetic()
sgrsn 0:6ff4cad60b67 147 {
sgrsn 0:6ff4cad60b67 148 char *data = getdata(HZ);
sgrsn 0:6ff4cad60b67 149 return (float)((*(data+1) << 8) | *data);
sgrsn 0:6ff4cad60b67 150 }
sgrsn 0:6ff4cad60b67 151
sgrsn 0:6ff4cad60b67 152 float JY901::getXaxisAngle()
sgrsn 0:6ff4cad60b67 153 {
sgrsn 0:6ff4cad60b67 154 char *data = getdata(Roll);
sgrsn 0:6ff4cad60b67 155 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
sgrsn 0:6ff4cad60b67 156 }
sgrsn 0:6ff4cad60b67 157
sgrsn 0:6ff4cad60b67 158 float JY901::getYaxisAngle()
sgrsn 0:6ff4cad60b67 159 {
sgrsn 0:6ff4cad60b67 160 char *data = getdata(Pitch);
sgrsn 0:6ff4cad60b67 161 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
sgrsn 0:6ff4cad60b67 162 }
sgrsn 0:6ff4cad60b67 163
sgrsn 0:6ff4cad60b67 164 float JY901::getZaxisAngle()
sgrsn 0:6ff4cad60b67 165 {
sgrsn 0:6ff4cad60b67 166 char *data = getdata(Yaw);
sgrsn 0:6ff4cad60b67 167 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
sgrsn 0:6ff4cad60b67 168 }
sgrsn 0:6ff4cad60b67 169
sgrsn 0:6ff4cad60b67 170 float JY901::getTemperature()
sgrsn 0:6ff4cad60b67 171 {
sgrsn 0:6ff4cad60b67 172 char *data = getdata(TEMP);
sgrsn 0:6ff4cad60b67 173 return (float)((*(data+1) << 8) | *data) / 100;
sgrsn 0:6ff4cad60b67 174 }
sgrsn 0:6ff4cad60b67 175
sgrsn 0:6ff4cad60b67 176 float JY901::getD0Status()
sgrsn 0:6ff4cad60b67 177 {
sgrsn 0:6ff4cad60b67 178 char *data = getdata(D0Status);
sgrsn 0:6ff4cad60b67 179 return (float)((*(data+1) << 8) | *data) / 4095;
sgrsn 0:6ff4cad60b67 180 }
sgrsn 0:6ff4cad60b67 181
sgrsn 0:6ff4cad60b67 182 float JY901::getD1Status()
sgrsn 0:6ff4cad60b67 183 {
sgrsn 0:6ff4cad60b67 184 char *data = getdata(D1Status);
sgrsn 0:6ff4cad60b67 185 return (float)((*(data+1) << 8) | *data) / 4095;
sgrsn 0:6ff4cad60b67 186 }
sgrsn 0:6ff4cad60b67 187
sgrsn 0:6ff4cad60b67 188 float JY901::getD2Status()
sgrsn 0:6ff4cad60b67 189 {
sgrsn 0:6ff4cad60b67 190 char *data = getdata(D2Status);
sgrsn 0:6ff4cad60b67 191 return (float)((*(data+1) << 8) | *data) / 4095;
sgrsn 0:6ff4cad60b67 192 }
sgrsn 0:6ff4cad60b67 193
sgrsn 0:6ff4cad60b67 194 float JY901::getD3Status()
sgrsn 0:6ff4cad60b67 195 {
sgrsn 0:6ff4cad60b67 196 char *data = getdata(D3Status);
sgrsn 0:6ff4cad60b67 197 return (float)((*(data+1) << 8) | *data) / 4095;
sgrsn 0:6ff4cad60b67 198 }
sgrsn 0:6ff4cad60b67 199
sgrsn 0:6ff4cad60b67 200 int JY901::getmode(float (JY901::*getFunc)(), const int kaisu)
sgrsn 0:6ff4cad60b67 201 {
sgrsn 0:6ff4cad60b67 202 /*exampl
sgrsn 0:6ff4cad60b67 203 printf("%d\r\n", obj.getmode(&JY901::getYawAngle, 10));
sgrsn 0:6ff4cad60b67 204 */
sgrsn 0:6ff4cad60b67 205 int yawangle[128] = {};
sgrsn 0:6ff4cad60b67 206 int samecount[128] = {};
sgrsn 0:6ff4cad60b67 207 int tmpcount[128] = {};
sgrsn 0:6ff4cad60b67 208 int num = 0;
sgrsn 0:6ff4cad60b67 209 for(int i = 0; i < kaisu; i++)
sgrsn 0:6ff4cad60b67 210 {
sgrsn 0:6ff4cad60b67 211 yawangle[i] = (int)((this->*getFunc)() + 0.5);
sgrsn 0:6ff4cad60b67 212 }
sgrsn 0:6ff4cad60b67 213 for(int i = 0; i < kaisu; i++)
sgrsn 0:6ff4cad60b67 214 {
sgrsn 0:6ff4cad60b67 215 for(int j = 0; j < kaisu; j++)
sgrsn 0:6ff4cad60b67 216 {
sgrsn 0:6ff4cad60b67 217 if(yawangle[i] == yawangle[j])
sgrsn 0:6ff4cad60b67 218 {
sgrsn 0:6ff4cad60b67 219 samecount[i]++;
sgrsn 0:6ff4cad60b67 220 }
sgrsn 0:6ff4cad60b67 221 }
sgrsn 0:6ff4cad60b67 222 tmpcount[i] = samecount[i];
sgrsn 0:6ff4cad60b67 223 }
sgrsn 0:6ff4cad60b67 224 for(int i = 0; i < kaisu; i++)
sgrsn 0:6ff4cad60b67 225 {
sgrsn 0:6ff4cad60b67 226 for(int j = 0; j < i; j++)
sgrsn 0:6ff4cad60b67 227 {
sgrsn 0:6ff4cad60b67 228 if(tmpcount[i] > tmpcount[j])
sgrsn 0:6ff4cad60b67 229 {
sgrsn 0:6ff4cad60b67 230 int tmp = tmpcount[j];
sgrsn 0:6ff4cad60b67 231 tmpcount[j] = tmpcount[i];
sgrsn 0:6ff4cad60b67 232 tmpcount[i] = tmp;
sgrsn 0:6ff4cad60b67 233 }
sgrsn 0:6ff4cad60b67 234 }
sgrsn 0:6ff4cad60b67 235 }
sgrsn 0:6ff4cad60b67 236 int mostcount = tmpcount[0];
sgrsn 0:6ff4cad60b67 237 for(int i = 0; i < kaisu; i++)
sgrsn 0:6ff4cad60b67 238 {
sgrsn 0:6ff4cad60b67 239 if(samecount[i] == mostcount)
sgrsn 0:6ff4cad60b67 240 {
sgrsn 0:6ff4cad60b67 241 num = i;
sgrsn 0:6ff4cad60b67 242 }
sgrsn 0:6ff4cad60b67 243 }
sgrsn 0:6ff4cad60b67 244 return yawangle[num];
sgrsn 0:6ff4cad60b67 245 }
sgrsn 0:6ff4cad60b67 246
sgrsn 0:6ff4cad60b67 247 float JY901::getPressure() //not use
sgrsn 0:6ff4cad60b67 248 {
sgrsn 0:6ff4cad60b67 249 char *dataL = getdata(PressureL);
sgrsn 0:6ff4cad60b67 250 char *dataH = getdata(PressureH);
sgrsn 0:6ff4cad60b67 251 return (float)((*(dataH+1) << 24) | (*dataH << 16) | (*(dataL+1) << 8) | *dataL);
sgrsn 0:6ff4cad60b67 252 }
sgrsn 0:6ff4cad60b67 253
sgrsn 0:6ff4cad60b67 254 float JY901::getHeight() //not use
sgrsn 0:6ff4cad60b67 255 {
sgrsn 0:6ff4cad60b67 256 char *dataL = getdata(HeightL);
sgrsn 0:6ff4cad60b67 257 char *dataH = getdata(HeightH);
sgrsn 0:6ff4cad60b67 258 return (float)( ( *(dataH+1) << 24) | (*dataH << 16) | (*(dataL+1) << 8) | *dataL );
sgrsn 0:6ff4cad60b67 259 }
sgrsn 0:6ff4cad60b67 260
sgrsn 0:6ff4cad60b67 261 float JY901::getLongitude() //not use
sgrsn 0:6ff4cad60b67 262 {
sgrsn 0:6ff4cad60b67 263 char *dataL = getdata(LonL);
sgrsn 0:6ff4cad60b67 264 char *dataH = getdata(LonH);
sgrsn 0:6ff4cad60b67 265 return (float)( ( *(dataH+1) << 24) | (*dataH << 16) | (*(dataL+1) << 8) | *dataL );
sgrsn 0:6ff4cad60b67 266 }
sgrsn 0:6ff4cad60b67 267
sgrsn 0:6ff4cad60b67 268 float JY901::getLatitude() //not use
sgrsn 0:6ff4cad60b67 269 {
sgrsn 0:6ff4cad60b67 270 char *dataL = getdata(LatL);
sgrsn 0:6ff4cad60b67 271 char *dataH = getdata(LatH);
sgrsn 0:6ff4cad60b67 272 return (float)( ( *(dataH+1) << 24) | (*dataH << 16) | (*(dataL+1) << 8) | *dataL );
sgrsn 0:6ff4cad60b67 273 }
sgrsn 0:6ff4cad60b67 274
sgrsn 0:6ff4cad60b67 275 float JY901::getGPSHeight()
sgrsn 0:6ff4cad60b67 276 {
sgrsn 0:6ff4cad60b67 277 char *data = getdata(GPSHeight);
sgrsn 0:6ff4cad60b67 278 return (float)((*(data+1) << 8) | *data);
sgrsn 0:6ff4cad60b67 279 }
sgrsn 0:6ff4cad60b67 280 float JY901::getGPSYaw()
sgrsn 0:6ff4cad60b67 281 {
sgrsn 0:6ff4cad60b67 282 char *data = getdata(GPSYaw);
sgrsn 0:6ff4cad60b67 283 return (float)((*(data+1) << 8) | *data);
sgrsn 0:6ff4cad60b67 284 }
sgrsn 0:6ff4cad60b67 285 float JY901::getGPSspeed()
sgrsn 0:6ff4cad60b67 286 {
sgrsn 0:6ff4cad60b67 287 char *dataL = getdata(GPSVL);
sgrsn 0:6ff4cad60b67 288 char *dataH = getdata(GPSVH);
sgrsn 0:6ff4cad60b67 289 return (float)( ( *(dataH+1) << 24) | (*dataH << 16) | (*(dataL+1) << 8) | *dataL );
sgrsn 0:6ff4cad60b67 290 }
sgrsn 0:6ff4cad60b67 291
sgrsn 0:6ff4cad60b67 292 char *JY901::getdata(char registar)
sgrsn 0:6ff4cad60b67 293 {
sgrsn 0:6ff4cad60b67 294 char data[2] = {};
sgrsn 0:6ff4cad60b67 295 start();
sgrsn 0:6ff4cad60b67 296 write(IICADDR);
sgrsn 0:6ff4cad60b67 297 write(registar);
sgrsn 0:6ff4cad60b67 298 read(IICADDR, data, 2);
sgrsn 0:6ff4cad60b67 299 stop();
sgrsn 0:6ff4cad60b67 300 return data;
sgrsn 0:6ff4cad60b67 301 }