Falcon/DEVICE_UART_RC_BRIDGE.cpp at master · yankailab/Falcon · 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
#include "DEVICE_UART_RC_BRIDGE.h"
#include "PPMOutput.h"

void DEVICE_UART_RC_BRIDGE::deviceSetup()
{
	m_bPrintIMU = true;// false;
	m_bBootSuccess = true;
	m_bHostConnected = false;
	m_counter = 0;

#ifndef ATMEGA_A328
	m_pRFSerial->begin(115200);
#endif

#ifdef USB_DEBUG
	// wait for Leonardo enumeration, others continue immediately
	while (!(*m_pUSBSerial));
#endif

	if (*m_pUSBSerial)
	{
		//To host side (Android device etc.)
		m_pUSBSerial->begin(115200);
		m_bHostConnected = true;
		m_pUSBSerial->println(F("FALCON_ON"));
	}

	//Init PPM generator
	PPM_init(22500,500);

	//Vehicle Link
	m_VLink.init();

#ifndef ATMEGA_A328
	m_VLink.m_pHostSerial = m_pUSBSerial;
	m_VLink.m_pUartSerial = m_pRFSerial;
#else
	m_VLink.m_pHostSerial = m_pUSBSerial;
#endif

}

void DEVICE_UART_RC_BRIDGE::deviceLoop()
{
	int i;
	/*
	if(g_opeMode==OPE_SERIAL_BRIDGE)
	{
	Serial_Bridge();
	return;
	}
	*/

	if (m_bHostConnected)
	{
		if (m_VLink.receiveFromHost())
		{
			for (i = 0; i < RC_CHANNEL_NUM; i++)
			{
				g_ppm[i] = constrain((uint16_t)m_VLink.m_channelValues[i], 1000, 2000);
			}
		}
	}
	else
	{
#ifndef ATMEGA_A328
		if (m_VLink.receiveFromUART())
		{
			for (i = 0; i < RC_CHANNEL_NUM; i++)
			{
				g_ppm[i] = constrain((uint16_t)m_VLink.m_channelValues[i], 1000, 2000);
			}
		}
#endif
	}


	// slow rate actions
	switch (m_counter)
	{
	case 1:
		if (m_bHostConnected)
		{
			m_VLink.sendHostHeartBeat();
		}
		else
		{
#ifndef ATMEGA_A328
			m_VLink.sendUartHeartBeat();
#endif
		}
		break;
	case 10:
		if (!m_bHostConnected)
		{
			if (Serial)
			{
				m_pUSBSerial = &Serial;
				m_bHostConnected = true;
			}
		}
		break;
	case 100:
		m_counter = 0;
		break;

	default:
		break;
	}
	m_counter++;
}