Codrone/CoDrone_request.cpp at master · truedo/Codrone · 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
133
134
135
136
137
138
139
140
#include "CoDrone.h"
#include "Arduino.h"
#include <EEPROM.h>

//-------------------------------------------------------------------------------------------------------//
//------------------------------------------- GetData ---------------------------------------------------//
//-------------------------------------------------------------------------------------------------------//
void CoDroneClass::ReceiveGetData(byte _reqType)
{
	byte *_reqCheckType;

	sendCheckFlag = 1;
	Send_Command(cType_Request, _reqType);

//---------------------------------------------------------------------------------//

	if		 (_reqType == Req_Attitude) 			_reqCheckType = &receiveAttitudeSuccess;
	else if(_reqType == Req_Battery) 				_reqCheckType = &receiveBatterySuccess;
	else if(_reqType == Req_Range) 					_reqCheckType = &receiveRangeSuccess;
	else if(_reqType == Req_State) 					_reqCheckType = &receiveStateSuccess;
	else if(_reqType == Req_ImageFlow) 			_reqCheckType = &receiveOptSuccess;
	else if(_reqType == Req_Pressure) 			_reqCheckType = &receivePressureSuccess;
	else if(_reqType == Req_TrimFlight) 		_reqCheckType = &receiveTrimSuccess;
	else if(_reqType == Req_ImuRawAndAngle) _reqCheckType = &receiveAccelSuccess;

//--------------------------------------------------------------------------------//
	*_reqCheckType = 0;

	long oldTime = millis();
	while(!*_reqCheckType)
	{
		Receive();
		if (oldTime + 500 < millis()) break; //time out check
	}
}
//--------------------------------------------------------------------------------//

int CoDroneClass::getPressure()
{
	byte _reqType = Req_Pressure;
	ReceiveGetData(_reqType);
	return pressure;
}

int CoDroneClass::getDroneTemp()
{
	byte _reqType = Req_Pressure;
	ReceiveGetData(_reqType);
	return temperature;
}

int CoDroneClass::getState()
{
	byte _reqType = Req_State;
	ReceiveGetData(_reqType);
	return droneState[2];
}

int CoDroneClass::getHeight()
{
	byte _reqType = Req_Range;
	ReceiveGetData(_reqType);
	return sensorRange[5];
}

int CoDroneClass::getBatteryPercentage()
{
	byte _reqType = Req_Battery;
	ReceiveGetData(_reqType);
	return batteryPercent;
}

int CoDroneClass::getBatteryVoltage()
{
	byte _reqType = Req_Battery;
	ReceiveGetData(_reqType);
	return batteryVoltage;
}

acceldata CoDroneClass::getAccelerometer()
{
	byte _reqType = Req_ImuRawAndAngle;
	ReceiveGetData(_reqType);

	acceldata result;
	result.x = ImuAccX;
	result.y = ImuAccY;
	result.z = ImuAccZ;
	return result;
}

trimdata CoDroneClass::getTrim()
{
	byte _reqType = Req_TrimFlight;
	ReceiveGetData(_reqType);

	trimdata result;
	result.roll = TrimAll_Roll;
	result.pitch = TrimAll_Pitch;
	result.yaw = TrimAll_Yaw;
	result.throttle = TrimAll_Throttle;

	return result;
}

optdata CoDroneClass::getOptFlowPosition()
{
	byte _reqType = Req_ImageFlow;
	ReceiveGetData(_reqType);

	optdata result;
	result.x = fVelocitySumX;
	result.y = fVelocitySumY;
	return result;
}

gyrodata CoDroneClass::getAngularSpeed()
{
	byte _reqType = Req_ImuRawAndAngle;
	ReceiveGetData(_reqType);

	gyrodata result;
	result.roll = ImuGyroRoll;
	result.pitch = ImuGyroPitch;
	result.yaw = ImuGyroYaw;
	return result;
}

gyrodata CoDroneClass::getGyroAngles()
{
	byte _reqType = Req_Attitude;
	ReceiveGetData(_reqType);

	gyrodata result;
	result.roll = attitudeRoll;
	result.pitch = attitudePitch;
	result.yaw = attitudeYaw;
	return result;
}
//-------------------------------------------------------------------------------------------------------//