hidaka sato / JY901

unfinished

Dependents:   WRS_mechanamu_test WRS2019_master mbed_2018 mbed_2019_rx3 ... more

JY901.cpp@0:6ff4cad60b67, 2016-09-18 (annotated)

Committer:
sgrsn
Date:
Sun Sep 18 05:42:49 2016 +0000
Revision:
0:6ff4cad60b67
Child:
1:6bcbd18a719a
unfinished

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