-
Notifications
You must be signed in to change notification settings - Fork 38
/
FaceApp.cpp
86 lines (74 loc) · 1.26 KB
/
FaceApp.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
#include "FaceApp.h"
FaceApp::FaceApp(SoftwareSerial &uart, uint32_t baud): m_puart(&uart)
{
_baud = baud;
_valid = false;
}
void FaceApp::begin(void)
{
m_puart->begin(_baud);
while (m_puart->available() > 0)
{
m_puart->read();
}
}
bool FaceApp::valid(void)
{
return _valid;
}
void FaceApp::routine(void)
{
while (m_puart->available() > 0)
{
char nullByte = char(m_puart->read());
if (nullByte == '\n')
{
comdata[data_p] = nullByte;
data_p = 0;
if ((char)comdata[0] == 'A')
{
strtok(comdata, ",");
dataRoll = atof(strtok(NULL, ","));
dataYaw = atof(strtok(NULL, ","));
dataPitch = atof(strtok(NULL, ","));
dataFace= atoi(strtok(NULL, ","));
dataX = atoi(strtok(NULL, ","));
dataY = atoi(strtok(NULL, ";"));
_valid = true;
}
else
{
_valid = false;
}
}
else
{
comdata[data_p] = nullByte ;
data_p++ ;
}
}
}
int FaceApp::getX(void)
{
return dataX;
}
int FaceApp::getY(void)
{
return dataY;
}
int FaceApp::getFace(void)
{
return dataFace;
}
float FaceApp::getRoll(void)
{
return dataRoll;
}
float FaceApp::getYaw(void)
{
return dataYaw;
}
float FaceApp::getPitch(void)
{
return dataPitch;
}