WT901C/WT901C.cpp at main · Djalcoding/WT901C · GitHub

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
#include<WT901C.h>
#include<Arduino.h>
#include<HardwareSerial.h>
#include<stdio.h>
#include <cstdio>

WT901C::WT901C(byte port) {
    switch (port)
    {
    case 1: SerialWIT = &Serial1; break;

    default:
            SerialWIT = &Serial2; break;
        break;
    }
}

WT901C::WT901C(HardwareSerial* serialPort) : SerialWIT(serialPort) {}

double WT901C::AccelerationReading::getX(){return Ax;}
double WT901C::AccelerationReading::getY() { return Ay;}
double WT901C::AccelerationReading::getZ() {return Az;}

char* WT901C::AccelerationReading::format() {
    static char formatted[64];
    std::sprintf(formatted, "X : %.4f g, Y : %.4f g, Z : %.4f g", getX(), getY(), getZ());
    return formatted;
}

double WT901C::AngularVelocityReading::getX(){return Wx;}
double WT901C::AngularVelocityReading::getY() { return Wy;}
double WT901C::AngularVelocityReading::getZ() {return Wz;}

char* WT901C::AngularVelocityReading::format() {
    static char formatted[64];
    std::sprintf(formatted, "X : %.4f deg/s, Y : %.4f deg/s, Z : %.4f deg/s", getX(), getY(), getZ());
    return formatted;
}

double WT901C::RotationReading::getX(){return Rx;}
double WT901C::RotationReading::getY() { return Ry;}
double WT901C::RotationReading::getZ() {return Rz;}

char* WT901C::RotationReading::format() {
    static char formatted[64];
    std::sprintf(formatted, "X : %.4f deg, Y : %.4f deg, Z : %.4f deg", getX(), getY(), getZ());
    return formatted;
}

double WT901C::MagnetometerReading::getX(){return Mx;}
double WT901C::MagnetometerReading::getY() { return My;}
double WT901C::MagnetometerReading::getZ() {return Mz;}

char* WT901C::MagnetometerReading::format() {
    static char formatted[64];
    std::sprintf(formatted, "X : %.4f G, Y : %.4f G, Z : %.4f G", getX(), getY(), getZ());
    return formatted;
}

void WT901C::begin(int baud, int receiver, int transmitter){
    SerialWIT->begin(baud, SERIAL_8N1, receiver, transmitter);
}

void WT901C::update(){

        while(SerialWIT->available()){
            byte c = SerialWIT->read();
            if(idx ==0 && c != 0x55){
                continue;
            }

            buf[idx++] = c;

            if(idx == 11){
                byte sum = 0;
                for(int i = 0; i < 10; i++) sum += buf[i];
                if(buf[10] != sum) goto end; //Checksum

                temperature = ((short)(buf[9] << 8 | buf[8])) / 100.0f;

                switch (buf[1])
                {
                case 0x51: //Acceleration header
                    currentAcceleration.Ax = ((short)(buf[3] << 8 | buf[2]))/ 32768.0f * 16.0f;
                    currentAcceleration.Ay = ((short)(buf[5] << 8 | buf[4]))/ 32768.0f * 16.0f;
                    currentAcceleration.Az = ((short)(buf[7] << 8 | buf[6]))/ 32768.0f * 16.0f;
                    break;

                case 0x52:
                    currentAngularVelocity.Wx = ((short)(buf[3] << 8 | buf[2]))/ 32768.0f * 2000.0f;
                    currentAngularVelocity.Wy = ((short)(buf[5] << 8 | buf[4]))/ 32768.0f * 2000.0f;
                    currentAngularVelocity.Wz = ((short)(buf[7] << 8 | buf[6]))/ 32768.0f * 2000.0f;
                    break;
                case 0x53:
                    currentRotation.Rx = ((short)(buf[3] << 8 | buf[2])) / 32768.0f * 180.0f;
                    currentRotation.Ry = ((short)(buf[5] << 8 | buf[4])) / 32768.0f * 180.0f;
                    currentRotation.Rz = ((short)(buf[7] << 8 | buf[6])) / 32768.0f * 180.0f;
                    break;
                case 0x54:
                    currentMagneticReading.Mx = ((short)(buf[3] << 8 | buf[2])) / 32768.0f * 2.0f;
                    currentMagneticReading.My = ((short)(buf[5] << 8 | buf[4])) / 32768.0f * 2.0f;
                    currentMagneticReading.Mz = ((short)(buf[7] << 8 | buf[6])) / 32768.0f * 2.0f;
                    break;
                }

                end:
                idx = 0;

            }


        }
}

WT901C::AccelerationReading WT901C::getAccelerationReading(){
    return currentAcceleration;
}

WT901C::AngularVelocityReading WT901C::getAngularVelocityReading(){
    return currentAngularVelocity;
}

WT901C::RotationReading WT901C::getRotationReading(){
    return currentRotation;
}
WT901C::MagnetometerReading WT901C::getMagneticFieldReading(){
    return currentMagneticReading;
}
double WT901C::getTemperatureReading(){
    return temperature;
}