-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathfrsky_tx.ino
228 lines (167 loc) · 7.03 KB
/
frsky_tx.ino
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
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
/*
skyData - markab
FrSky telemetry data module for Naza and TBS frame sensors
Created based on ideas from the following projects...
https://code.google.com/p/nazatofrsky/
https://code.google.com/p/bagaosd/
https://code.google.com/p/openxvario/
*/
// ************************************************************************************
// FRSKY PORT CONFIG
// ************************************************************************************
#define _FrSkySPort_C1 UART0_C1
#define _FrSkySPort_C3 UART0_C3
#define _FrSkySPort_S2 UART0_S2
// ************************************************************************************
// FRSKY SENSOR IDS
// ************************************************************************************
#define SENSOR_ID1 0x1B
#define SENSOR_ID2 0x0D
#define SENSOR_ID3 0x34
#define SENSOR_ID4 0x67
// ************************************************************************************
// FRSKY INTERNAL IDS
// ************************************************************************************
#define START_STOP 0x7e
#define DATA_FRAME 0x10
// ************************************************************************************
// FRSKY DATA IDS
// ************************************************************************************
#define RSSI_ID 0xf101
#define ADC1_ID 0xf102
#define ADC2_ID 0xf103
#define BATT_ID 0xf104
#define SWR_ID 0xf105
#define T1_ID 0x0400
#define T2_ID 0x0410
#define RPM_ID 0x0500
#define FUEL_ID 0x0600
#define ALT_ID 0x0100
#define VARIO_ID 0x0110
#define ACCX_ID 0x0700
#define ACCY_ID 0x0710
#define ACCZ_ID 0x0720
#define CURR_ID 0x0200
#define VFAS_ID 0x0210
#define CELLS_ID 0x0300
#define GPS_LONG_LATI_ID 0x0800
#define GPS_ALT_ID 0x0820
#define GPS_SPEED_ID 0x0830
#define GPS_COURS_ID 0x0840
#define GPS_TIME_DATE_ID 0x0850
// ************************************************************************************
// PROGRAM DEFINITIONS
// ************************************************************************************
#define MAX_ID_COUNT 9
// ************************************************************************************
// PROGRAM VARIABLES
// ************************************************************************************
short crc;
uint8_t last_rx;
uint32_t id_count = 0;
// ************************************************************************************
// INIT
// ************************************************************************************
void FrSkySerial_Init(long baud) {
FrSkySerial.begin(baud);
_FrSkySPort_C3 = 0x10; // tx invert
_FrSkySPort_C1 = 0xA0; // single wire mode
_FrSkySPort_S2 = 0x10; // rx invert
}
// ************************************************************************************
// PROCESS DATA
// ************************************************************************************
void FrSkySPort_ProcessData(void) {
if(ENABLE_FRSKY) {
uint8_t data = 0;
while (FrSkySerial.available()) {
data = FrSkySerial.read();
if (last_rx == START_STOP && ((data == SENSOR_ID1) || (data == SENSOR_ID2) || (data == SENSOR_ID3) || (data == SENSOR_ID4))) {
switch(id_count) {
case 0:
FrSkySPort_SendPackage(T1_ID, data_gps_fix_type);
break;
case 1:
FrSkySPort_SendPackage(T2_ID, data_gps_sat_count);
break;
case 2:
if(data_gps_fix_type == 3) {
FrSkySPort_SendPackage(VARIO_ID, data_gps_vario);
}
break;
case 3:
FrSkySPort_SendPackage(CURR_ID, data_curr);
break;
case 4:
FrSkySPort_SendPackage(VFAS_ID, data_vfas);
break;
case 5:
if(data_gps_fix_type == 3) {
FrSkySPort_SendPackage(GPS_LONG_LATI_ID, data_gps_long);
}
break;
case 6:
if(data_gps_fix_type == 3) {
FrSkySPort_SendPackage(GPS_LONG_LATI_ID, data_gps_lati);
}
break;
case 7:
if(data_gps_fix_type == 3) {
FrSkySPort_SendPackage(GPS_ALT_ID, data_gps_alt);
}
break;
case 8:
if(data_gps_fix_type == 3) {
FrSkySPort_SendPackage(GPS_SPEED_ID, data_gps_speed);
}
break;
case 9:
FrSkySPort_SendPackage(GPS_COURS_ID, data_gps_cours);
break;
}
id_count++;
if(id_count > MAX_ID_COUNT) id_count = 0;
}
last_rx=data;
}
}
}
// ************************************************************************************
// SEND BYTE
// ************************************************************************************
void FrSkySPort_SendByte(uint8_t byte) {
FrSkySerial.write(byte);
// crc update
crc += byte; //0-1FF
crc += crc >> 8; //0-100
crc &= 0x00ff;
crc += crc >> 8; //0-0FF
crc &= 0x00ff;
}
// ************************************************************************************
// SEND CRC
// ************************************************************************************
void FrSkySPort_SendCrc() {
FrSkySerial.write(0xFF-crc);
crc = 0; // crc reset
}
// ************************************************************************************
// SEND PACKAGE
// ************************************************************************************
void FrSkySPort_SendPackage(uint16_t id, uint32_t value) {
digitalWrite(pin_led, HIGH);
UART0_C3 |= 32; // transmit direction to s.port
FrSkySPort_SendByte(DATA_FRAME);
uint8_t *bytes = (uint8_t*)&id;
FrSkySPort_SendByte(bytes[0]);
FrSkySPort_SendByte(bytes[1]);
bytes = (uint8_t*)&value;
FrSkySPort_SendByte(bytes[0]);
FrSkySPort_SendByte(bytes[1]);
FrSkySPort_SendByte(bytes[2]);
FrSkySPort_SendByte(bytes[3]);
FrSkySPort_SendCrc();
FrSkySerial.flush();
UART0_C3 ^= 32; // transmit direction from s.port
digitalWrite(pin_led, LOW);
}