haruki hasegawa / jy901

change code by hasegawa

Dependents:   haseomni_main

jy901.cpp@3:ba9da7dbbd9a, 2020-09-29 (annotated)

Committer:
LVRhase01
Date:
Tue Sep 29 13:25:52 2020 +0000
Revision:
3:ba9da7dbbd9a
Parent:
0:f80d034ed481 
Add 0-point setting for Yaw axis and 6-axis mode code

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 }
LVRhase01 3:ba9da7dbbd9a 30 //ここより下は長谷川が追加しました。不必要な場合は教えてください。2020/09/29
LVRhase01 3:ba9da7dbbd9a 31 void JY901::yawcalibrate()
LVRhase01 3:ba9da7dbbd9a 32 {
LVRhase01 3:ba9da7dbbd9a 33 char calibrationRegistar[3]= {0x01,0x04,0x00};
LVRhase01 3:ba9da7dbbd9a 34 write(IICADDR,calibrationRegistar,3,false);
LVRhase01 3:ba9da7dbbd9a 35 }
LVRhase01 3:ba9da7dbbd9a 36
LVRhase01 3:ba9da7dbbd9a 37 void JY901::algorithmtrasition()
LVRhase01 3:ba9da7dbbd9a 38 {
LVRhase01 3:ba9da7dbbd9a 39 char calibrationRegistar[3]= {0x24,0x01,0x00};
LVRhase01 3:ba9da7dbbd9a 40 write(IICADDR,calibrationRegistar,3,false);
LVRhase01 3:ba9da7dbbd9a 41 }
number_key 0:f80d034ed481 42
number_key 0:f80d034ed481 43 void JY901::calibrateAll(int time)
number_key 0:f80d034ed481 44 {
number_key 0:f80d034ed481 45 calibrateGyroAccel();
number_key 0:f80d034ed481 46 wait_ms(time);
number_key 0:f80d034ed481 47 calibrateMagnetic();
number_key 0:f80d034ed481 48 wait_ms(time);
number_key 0:f80d034ed481 49 calibrateHeight();
number_key 0:f80d034ed481 50 wait_ms(time);
number_key 0:f80d034ed481 51 endCalibrate();
LVRhase01 3:ba9da7dbbd9a 52 wait_ms(time);
LVRhase01 3:ba9da7dbbd9a 53 algorithmtrasition();
LVRhase01 3:ba9da7dbbd9a 54 wait_ms(time);
LVRhase01 3:ba9da7dbbd9a 55 yawcalibrate();
LVRhase01 3:ba9da7dbbd9a 56
number_key 0:f80d034ed481 57 }
number_key 0:f80d034ed481 58
number_key 0:f80d034ed481 59 float JY901::getXaxisAcceleration()
number_key 0:f80d034ed481 60 {
number_key 0:f80d034ed481 61 char *data = getdata(AX);
number_key 0:f80d034ed481 62 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
number_key 0:f80d034ed481 63 }
number_key 0:f80d034ed481 64
number_key 0:f80d034ed481 65 float JY901::getYaxisAcceleration()
number_key 0:f80d034ed481 66 {
number_key 0:f80d034ed481 67 char *data = getdata(AY);
number_key 0:f80d034ed481 68 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
number_key 0:f80d034ed481 69 }
number_key 0:f80d034ed481 70
number_key 0:f80d034ed481 71 float JY901::getZaxisAcceleration()
number_key 0:f80d034ed481 72 {
number_key 0:f80d034ed481 73 char *data = getdata(AZ);
number_key 0:f80d034ed481 74 return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
number_key 0:f80d034ed481 75 }
number_key 0:f80d034ed481 76
number_key 0:f80d034ed481 77 float JY901::getXaxisAngularVelocity()
number_key 0:f80d034ed481 78 {
number_key 0:f80d034ed481 79 char *data = getdata(GX);
number_key 0:f80d034ed481 80 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
number_key 0:f80d034ed481 81 }
number_key 0:f80d034ed481 82
number_key 0:f80d034ed481 83 float JY901::getYaxisAngularVelocity()
number_key 0:f80d034ed481 84 {
number_key 0:f80d034ed481 85 char *data = getdata(GY);
number_key 0:f80d034ed481 86 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
number_key 0:f80d034ed481 87 }
number_key 0:f80d034ed481 88
number_key 0:f80d034ed481 89 float JY901::getZaxisAngularVelocity()
number_key 0:f80d034ed481 90 {
number_key 0:f80d034ed481 91 char *data = getdata(GZ);
number_key 0:f80d034ed481 92 return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
number_key 0:f80d034ed481 93 }
number_key 0:f80d034ed481 94
number_key 0:f80d034ed481 95 float JY901::getXaxisMagnetic()
number_key 0:f80d034ed481 96 {
number_key 0:f80d034ed481 97 char *data = getdata(HX);
number_key 0:f80d034ed481 98 return (float)((*(data+1) << 8) | *data);
number_key 0:f80d034ed481 99 }
number_key 0:f80d034ed481 100
number_key 0:f80d034ed481 101 float JY901::getYaxisMagnetic()
number_key 0:f80d034ed481 102 {
number_key 0:f80d034ed481 103 char *data = getdata(HY);
number_key 0:f80d034ed481 104 return (float)((*(data+1) << 8) | *data);
number_key 0:f80d034ed481 105 }
number_key 0:f80d034ed481 106
number_key 0:f80d034ed481 107 float JY901::getZaxisMagnetic()
number_key 0:f80d034ed481 108 {
number_key 0:f80d034ed481 109 char *data = getdata(HZ);
number_key 0:f80d034ed481 110 return (float)((*(data+1) << 8) | *data);
number_key 0:f80d034ed481 111 }
number_key 0:f80d034ed481 112
number_key 0:f80d034ed481 113 float JY901::getXaxisAngle()
number_key 0:f80d034ed481 114 {
number_key 0:f80d034ed481 115 char *data = getdata(Roll);
number_key 0:f80d034ed481 116 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
number_key 0:f80d034ed481 117 }
number_key 0:f80d034ed481 118
number_key 0:f80d034ed481 119 float JY901::getYaxisAngle()
number_key 0:f80d034ed481 120 {
number_key 0:f80d034ed481 121 char *data = getdata(Pitch);
number_key 0:f80d034ed481 122 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
number_key 0:f80d034ed481 123 }
number_key 0:f80d034ed481 124
number_key 0:f80d034ed481 125 float JY901::getZaxisAngle()
number_key 0:f80d034ed481 126 {
number_key 0:f80d034ed481 127 char *data = getdata(Yaw);
number_key 0:f80d034ed481 128 return (float)((*(data+1) << 8) | *data) / 32768 * 180;
number_key 0:f80d034ed481 129 }
number_key 0:f80d034ed481 130
number_key 0:f80d034ed481 131
number_key 0:f80d034ed481 132
number_key 0:f80d034ed481 133 char *JY901::getdata(char registar)
number_key 0:f80d034ed481 134 {
number_key 0:f80d034ed481 135 char data[2] = {};
number_key 0:f80d034ed481 136 write(IICADDR,&registar,1,true);
number_key 0:f80d034ed481 137 read(IICADDR,data,2,false);
number_key 0:f80d034ed481 138 return data;
number_key 0:f80d034ed481 139 }
LVRhase01 3:ba9da7dbbd9a 140