Quadrifoglio / jy901

ジャイロ

Dependents:   00_yotsuba 103_JY901_practice 200_yotsuba_21 200_yotuba_21_uiChange

jy901.cpp@7:9fa716a2100a, 2021-03-04 (annotated)

Committer:
piroro4560
Date:
Thu Mar 04 11:57:19 2021 +0000
Revision:
7:9fa716a2100a
Parent:
3:1f630a1d7ed1
Child:
8:a5ce2d3c49ac
jy

Who changed what in which revision?

UserRevisionLine numberNew contents of line
number_key 0:f80d034ed481 1 #include "jy901.h"
number_key 0:f80d034ed481 2
number_key 0:f80d034ed481 3 JY901::JY901(PinName sda, PinName scl) : I2C(sda, scl)
number_key 0:f80d034ed481 4 {
number_key 0:f80d034ed481 5 }
number_key 0:f80d034ed481 6
number_key 0:f80d034ed481 7 void JY901::calibrateGyroAccel()
number_key 0:f80d034ed481 8 {
number_key 0:f80d034ed481 9 char calibrationRegistar[3]= {CALSW,0x01,0x00};
number_key 0:f80d034ed481 10 write(IICADDR,calibrationRegistar,3,false);
number_key 0:f80d034ed481 11 }
number_key 0:f80d034ed481 12
number_key 0:f80d034ed481 13 void JY901::calibrateMagnetic()
number_key 0:f80d034ed481 14 {
number_key 0:f80d034ed481 15 char calibrationRegistar[3]= {CALSW,0x02,0x00};
number_key 0:f80d034ed481 16 write(IICADDR,calibrationRegistar,3,false);
number_key 0:f80d034ed481 17 }
number_key 0:f80d034ed481 18
number_key 0:f80d034ed481 19 void JY901::calibrateHeight()
number_key 0:f80d034ed481 20 {
number_key 0:f80d034ed481 21 char calibrationRegistar[3]= {CALSW,0x03,0x00};
number_key 0:f80d034ed481 22 write(IICADDR,calibrationRegistar,3,false);
number_key 0:f80d034ed481 23 }
number_key 0:f80d034ed481 24
number_key 0:f80d034ed481 25 void JY901::endCalibrate()
number_key 0:f80d034ed481 26 {
number_key 0:f80d034ed481 27 char calibrationRegistar[3]= {CALSW,0x00,0x00};
number_key 0:f80d034ed481 28 write(IICADDR,calibrationRegistar,3,false);
number_key 0:f80d034ed481 29 }
number_key 0:f80d034ed481 30
number_key 0:f80d034ed481 31 void JY901::calibrateAll(int time)
number_key 0:f80d034ed481 32 {
number_key 0:f80d034ed481 33 calibrateGyroAccel();
piroro4560 7:9fa716a2100a 34 thread_sleep_for(time);
number_key 0:f80d034ed481 35 calibrateMagnetic();
piroro4560 7:9fa716a2100a 36 thread_sleep_for(time);
number_key 0:f80d034ed481 37 calibrateHeight();
piroro4560 7:9fa716a2100a 38 thread_sleep_for(time);
number_key 0:f80d034ed481 39 endCalibrate();
number_key 0:f80d034ed481 40 }
number_key 0:f80d034ed481 41
piroro4560 3:1f630a1d7ed1 42 void JY901::jyroReset()
piroro4560 3:1f630a1d7ed1 43 {
piroro4560 3:1f630a1d7ed1 44 char calibrationRegistar[3]= {0x01,0x04,0x00};
piroro4560 3:1f630a1d7ed1 45 write(IICADDR,calibrationRegistar,3,false);
piroro4560 3:1f630a1d7ed1 46 }
piroro4560 3:1f630a1d7ed1 47
number_key 0:f80d034ed481 48 float JY901::getXaxisAcceleration()
number_key 0:f80d034ed481 49 {
number_key 0:f80d034ed481 50 char *data = getdata(AX);
number_key 0:f80d034ed481 51 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
number_key 0:f80d034ed481 52 }
number_key 0:f80d034ed481 53
number_key 0:f80d034ed481 54 float JY901::getYaxisAcceleration()
number_key 0:f80d034ed481 55 {
number_key 0:f80d034ed481 56 char *data = getdata(AY);
number_key 0:f80d034ed481 57 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
number_key 0:f80d034ed481 58 }
number_key 0:f80d034ed481 59
number_key 0:f80d034ed481 60 float JY901::getZaxisAcceleration()
number_key 0:f80d034ed481 61 {
number_key 0:f80d034ed481 62 char *data = getdata(AZ);
number_key 0:f80d034ed481 63 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
number_key 0:f80d034ed481 64 }
number_key 0:f80d034ed481 65
number_key 0:f80d034ed481 66 float JY901::getXaxisAngularVelocity()
number_key 0:f80d034ed481 67 {
number_key 0:f80d034ed481 68 char *data = getdata(GX);
number_key 0:f80d034ed481 69 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
number_key 0:f80d034ed481 70 }
number_key 0:f80d034ed481 71
number_key 0:f80d034ed481 72 float JY901::getYaxisAngularVelocity()
number_key 0:f80d034ed481 73 {
number_key 0:f80d034ed481 74 char *data = getdata(GY);
number_key 0:f80d034ed481 75 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
number_key 0:f80d034ed481 76 }
number_key 0:f80d034ed481 77
number_key 0:f80d034ed481 78 float JY901::getZaxisAngularVelocity()
number_key 0:f80d034ed481 79 {
number_key 0:f80d034ed481 80 char *data = getdata(GZ);
number_key 0:f80d034ed481 81 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
number_key 0:f80d034ed481 82 }
number_key 0:f80d034ed481 83
number_key 0:f80d034ed481 84 float JY901::getXaxisMagnetic()
number_key 0:f80d034ed481 85 {
number_key 0:f80d034ed481 86 char *data = getdata(HX);
number_key 0:f80d034ed481 87 return (float)((*(data+1) << 8) | *data);
number_key 0:f80d034ed481 88 }
number_key 0:f80d034ed481 89
number_key 0:f80d034ed481 90 float JY901::getYaxisMagnetic()
number_key 0:f80d034ed481 91 {
number_key 0:f80d034ed481 92 char *data = getdata(HY);
number_key 0:f80d034ed481 93 return (float)((*(data+1) << 8) | *data);
number_key 0:f80d034ed481 94 }
number_key 0:f80d034ed481 95
number_key 0:f80d034ed481 96 float JY901::getZaxisMagnetic()
number_key 0:f80d034ed481 97 {
number_key 0:f80d034ed481 98 char *data = getdata(HZ);
number_key 0:f80d034ed481 99 return (float)((*(data+1) << 8) | *data);
number_key 0:f80d034ed481 100 }
number_key 0:f80d034ed481 101
number_key 0:f80d034ed481 102 float JY901::getXaxisAngle()
number_key 0:f80d034ed481 103 {
number_key 0:f80d034ed481 104 char *data = getdata(Roll);
number_key 0:f80d034ed481 105 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
number_key 0:f80d034ed481 106 }
number_key 0:f80d034ed481 107
number_key 0:f80d034ed481 108 float JY901::getYaxisAngle()
number_key 0:f80d034ed481 109 {
number_key 0:f80d034ed481 110 char *data = getdata(Pitch);
number_key 0:f80d034ed481 111 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
number_key 0:f80d034ed481 112 }
number_key 0:f80d034ed481 113
number_key 0:f80d034ed481 114 float JY901::getZaxisAngle()
number_key 0:f80d034ed481 115 {
number_key 0:f80d034ed481 116 char *data = getdata(Yaw);
number_key 0:f80d034ed481 117 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
number_key 0:f80d034ed481 118 }
number_key 0:f80d034ed481 119
number_key 0:f80d034ed481 120
number_key 0:f80d034ed481 121
number_key 0:f80d034ed481 122 char *JY901::getdata(char registar)
number_key 0:f80d034ed481 123 {
number_key 0:f80d034ed481 124 char data[2] = {};
number_key 0:f80d034ed481 125 write(IICADDR,&registar,1,true);
number_key 0:f80d034ed481 126 read(IICADDR,data,2,false);
number_key 0:f80d034ed481 127 return data;
number_key 0:f80d034ed481 128 }