hidaka sato / JY901

unfinished

Dependents:   WRS_mechanamu_test WRS2019_master mbed_2018 mbed_2019_rx3 ... more

JY901.cpp@5:a492cfb18242, 2018-11-14 (annotated)

Committer:
sgrsn
Date:
Wed Nov 14 02:14:58 2018 +0000
Revision:
5:a492cfb18242
Parent:
4:cd62e2d69f62 
Add calculateAngleOnlyGyro() and reset(). not recommended

Who changed what in which revision?

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