-
Notifications
You must be signed in to change notification settings - Fork 0
/
FXAS21002C_Basic.cpp
133 lines (110 loc) · 2.86 KB
/
FXAS21002C_Basic.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
#include "FXAS21002C_Basic.h"
#include "FXAS21002C_Registers.h"
//Constructor
FXAS21002CBasic::FXAS21002CBasic(byte address, TwoWire* preferred_wire)
{
while (!((address >= 0x20) && (address <= 0x21)))
{
Serial.println("Invalid address for FXAS21002C");
delay(1000);
}
setupDevice(address,preferred_wire);
checkConnection();
changeRange(DEFAULT_FSR);
changeODR(DEFAULT_ODR);
}
//Changes device between active and Standby Mode
void FXAS21002CBasic::changePowerMode(byte mode)
{
writeBitsToReg(CTRL_REG1,POWER_MODE_BITS,mode);
}
//Wait till power mode transits
void FXAS21002CBasic::waitTill(byte mode)
{
while (readPowerMode() != mode)
{
}
}
//Returns the current system mode
byte FXAS21002CBasic::readPowerMode()
{
return readBitsFromReg(CTRL_REG1,POWER_MODE_BITS);
}
//Changes the device output data rate
void FXAS21002CBasic::changeODR(int odr)
{
if (readPowerMode() != STANDBY)
{
changePowerMode(STANDBY);
waitTill(STANDBY);
}
switch (odr)
{
case 800:
writeBitsToReg(CTRL_REG1,ODR_BITS,0b00000000);
break;
case 400:
writeBitsToReg(CTRL_REG1,ODR_BITS,0b00000100);
break;
case 200:
writeBitsToReg(CTRL_REG1,ODR_BITS,0b00001000);
break;
case 100:
writeBitsToReg(CTRL_REG1,ODR_BITS,0b00001100);
break;
case 50:
writeBitsToReg(CTRL_REG1,ODR_BITS,0b00010000);
break;
case 25:
writeBitsToReg(CTRL_REG1,ODR_BITS,0b00010100);
break;
case 12:
writeBitsToReg(CTRL_REG1,ODR_BITS,0b00011000);
break;
default:
break;
}
changePowerMode(ACTIVE);
waitTill(ACTIVE);
}
//change range of gyro, doesn't offer fsr boost!
void FXAS21002CBasic::changeRange(int fsr)
{
if (readPowerMode() != STANDBY)
{
changePowerMode(STANDBY);
waitTill(STANDBY);
}
switch (fsr)
{
case 2000:
writeBitsToReg(CTRL_REG0,FSR_BITS,0b00000000);
sensitivity_ = 7.8125*8*0.001;
break;
case 1000:
writeBitsToReg(CTRL_REG0,FSR_BITS,0b00000001);
sensitivity_ = 7.8125*4*0.001;
break;
case 500:
writeBitsToReg(CTRL_REG0,FSR_BITS,0b00000010);
sensitivity_ = 7.8125*2*0.001;
break;
case 250:
writeBitsToReg(CTRL_REG0,FSR_BITS,0b00000011);
sensitivity_ = 7.8125*1*0.001;
break;
default:
break;
}
//Entering Active Mode
changePowerMode(ACTIVE);
waitTill(ACTIVE);
}
//update angular acceleration
void FXAS21002CBasic::updateGyroData(float* gyro_data)
{
gyro_data[0] = (readShortIntFromReg(OUT_X_MSB_REG)*sensitivity);
gyro_data[1] = (readShortIntFromReg(OUT_Y_MSB_REG)*sensitivity);
gyro_data[2] = (readShortIntFromReg(OUT_Z_MSB_REG)*sensitivity);
return ;
}