forked from victzh/bradwii
-
Notifications
You must be signed in to change notification settings - Fork 6
/
rx_v202.c
363 lines (316 loc) · 13.2 KB
/
rx_v202.c
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
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
/*
Copyright 2014 Victor Joukov
Some of this code is based on Multiwii code by Alexandre Dubus (www.multiwii.com)
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "hal.h"
#include "bradwii.h"
#include "rx.h"
#include "defs.h"
#include "lib_timers.h"
#include "nrf24l01.h"
//#include "lib_digitalio.h"
//#include "lib_serial.h"
// when adding new receivers, the following functions must be included:
// initrx() // initializes the r/c receiver
// readrx() // loads global.rxvalues with r/c values as fixedpointnum's from -1 to 1 (0 is the center).
unsigned char v2x2_channelindex[] = { THROTTLEINDEX,YAWINDEX,PITCHINDEX,ROLLINDEX,AUX1INDEX,AUX2INDEX,AUX3INDEX,AUX4INDEX,8,9,10,11 };
extern globalstruct global;
extern usersettingsstruct usersettings; // user editable variables
#define BV(x) (1 << (x))
///////////////////////////////////////////////////////////////////////
// Definitions for a group of nRF24L01 based protocols
enum Proto {
PROTO_NONE = 0,
PROTO_V2X2,
PROTO_HISKY,
PROTO_SLT,
};
///////////////////////////////////////////////////////////////////////
// V2X2 protocol
#define V2X2_PAYLOAD_SIZE 16
#define V2X2_NFREQCHANNELS 16
enum {
V2X2_FLAG_CAMERA = 0x01, // also automatic Missile Launcher and Hoist in one direction
V2X2_FLAG_VIDEO = 0x02, // also Sprayer, Bubbler, Missile Launcher(1), and Hoist in the other dir.
V2X2_FLAG_FLIP = 0x04,
V2X2_FLAG_UNK9 = 0x08,
V2X2_FLAG_LED = 0x10,
V2X2_FLAG_UNK10 = 0x20,
V2X2_FLAG_BIND = 0xC0
};
// This is frequency hopping table for V202 protocol
// The table is the first 4 rows of 32 frequency hopping
// patterns, all other rows are derived from the first 4.
// For some reason the protocol avoids channels, dividing
// by 16 and replaces them by subtracting 3 from the channel
// number in this case.
// The pattern is defined by 5 least significant bits of
// sum of 3 bytes comprising TX id
static const uint8_t v2x2_freq_hopping[][V2X2_NFREQCHANNELS] = {
{ 0x27, 0x1B, 0x39, 0x28, 0x24, 0x22, 0x2E, 0x36,
0x19, 0x21, 0x29, 0x14, 0x1E, 0x12, 0x2D, 0x18 }, // 00
{ 0x2E, 0x33, 0x25, 0x38, 0x19, 0x12, 0x18, 0x16,
0x2A, 0x1C, 0x1F, 0x37, 0x2F, 0x23, 0x34, 0x10 }, // 01
{ 0x11, 0x1A, 0x35, 0x24, 0x28, 0x18, 0x25, 0x2A,
0x32, 0x2C, 0x14, 0x27, 0x36, 0x34, 0x1C, 0x17 }, // 02
{ 0x22, 0x27, 0x17, 0x39, 0x34, 0x28, 0x2B, 0x1D,
0x18, 0x2A, 0x21, 0x38, 0x10, 0x26, 0x20, 0x1F } // 03
};
static uint8_t rf_channels[MAXFHSIZE];
static uint8_t packet[V2X2_PAYLOAD_SIZE];
static uint8_t rf_ch_num;
static uint8_t nfreqchannels;
static uint8_t bind_phase;
static uint8_t boundprotocol;
static uint8_t tryprotocol;
static uint32_t packet_timer;
//static uint32_t rx_timeout;
//static uint32_t valid_packets;
//static uint32_t missed_packets;
//static uint32_t bad_packets;
#define valid_packets (global.debugvalue[0])
#define missed_packets (global.debugvalue[1])
#define bad_packets (global.debugvalue[2])
#define rx_timeout (global.debugvalue[3])
enum {
PHASE_NOT_BOUND = 0,
PHASE_JUST_BOUND,
PHASE_LOST_BINDING,
PHASE_BOUND
};
static void v2x2_set_tx_id(uint8_t *id)
{
uint8_t sum;
boundprotocol = usersettings.boundprotocol = PROTO_V2X2;
usersettings.txidsize = 3;
usersettings.txid[0] = id[0];
usersettings.txid[1] = id[1];
usersettings.txid[2] = id[2];
sum = id[0] + id[1] + id[2];
usersettings.fhsize = V2X2_NFREQCHANNELS;
// Base row is defined by lowest 2 bits
const uint8_t *fh_row = v2x2_freq_hopping[sum & 0x03];
// Higher 3 bits define increment to corresponding row
uint8_t increment = (sum & 0x1e) >> 2;
for (int i = 0; i < V2X2_NFREQCHANNELS; ++i) {
uint8_t val = fh_row[i] + increment;
// Strange avoidance of channels divisible by 16
usersettings.freqhopping[i] = (val & 0x0f) ? val : val - 3;
}
}
static void set_bound()
{
boundprotocol = usersettings.boundprotocol;
for (int i = 0; i < usersettings.fhsize; ++i) {
rf_channels[i] = usersettings.freqhopping[i];
}
nfreqchannels = usersettings.fhsize;
rx_timeout = 1000L; // find the channel as fast as possible
}
static void prepare_to_bind(void)
{
packet_timer = lib_timers_starttimer();
tryprotocol = PROTO_V2X2;
for (int i = 0; i < V2X2_NFREQCHANNELS; ++i) {
rf_channels[i] = v2x2_freq_hopping[0][i];
}
nfreqchannels = V2X2_NFREQCHANNELS;
rx_timeout = 1000L;
boundprotocol = PROTO_NONE;
}
static void switch_channel(void)
{
NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_channels[rf_ch_num]);
if (++rf_ch_num >= nfreqchannels) rf_ch_num = 0;
}
// The Beken radio chip can be improperly reset
// We try to set it into Bank 0, before that we try
// to verify that it is there at all
static void reset_beken(void)
{
NRF24L01_Activate(0x53); // magic for BK2421/BK2423 bank switch
if (NRF24L01_ReadReg(NRF24L01_07_STATUS) & 0x80) {
NRF24L01_Activate(0x53); // switch to register set 0
}
}
// Check for Beken BK2421/BK2423 chip
// It is done by using Beken specific activate code, 0x53
// and checking that status register changed appropriately
// There is no harm to run it on nRF24L01 because following
// closing activate command changes state back even if it
// does something on nRF24L01
// For detailed description of what's happening here see
// http://www.inhaos.com/uploadfile/otherpic/BK2423%20Datasheet%20v2.0.pdf
// http://www.inhaos.com/uploadfile/otherpic/AN0008-BK2423%20Communication%20In%20250Kbps%20Air%20Rate.pdf
static void initialize_beken(void)
{
NRF24L01_Activate(0x53); // magic for BK2421/BK2423 bank switch
printf("Trying to switch banks\n");
if (NRF24L01_ReadReg(NRF24L01_07_STATUS) & 0x80) {
printf("BK2421 detected\n");
// Beken registers don't have such nice names, so we just mention
// them by their numbers
// It's all magic, eavesdropped from real transfer and not even from the
// data sheet - it has slightly different values
NRF24L01_WriteRegisterMulti(0x00, (uint8_t *) "\x40\x4B\x01\xE2", 4);
NRF24L01_WriteRegisterMulti(0x01, (uint8_t *) "\xC0\x4B\x00\x00", 4);
NRF24L01_WriteRegisterMulti(0x02, (uint8_t *) "\xD0\xFC\x8C\x02", 4);
NRF24L01_WriteRegisterMulti(0x03, (uint8_t *) "\xF9\x00\x39\x21", 4); // V202
// NRF24L01_WriteRegisterMulti(0x03, (uint8_t *) "\x99\x00\x39\x41", 4); // Beken datasheet
NRF24L01_WriteRegisterMulti(0x04, (uint8_t *) "\xC1\x96\x9A\x1B", 4); // V202
NRF24L01_WriteRegisterMulti(0x05, (uint8_t *) "\x24\x06\x7F\xA6", 4); // Disable RSSI
// NRF24L01_WriteRegisterMulti(0x05, (uint8_t *) "\x3C\x02\x7F\xA6", 4); // Enable RSSI
// NRF24L01_WriteRegisterMulti(0x0C, (uint8_t *) "\x00\x12\x73\x00", 4); // PLL locking time 120us, like BK2421
NRF24L01_WriteRegisterMulti(0x0C, (uint8_t *) "\x00\x12\x73\x05", 4); // PLL locking time 130us, like nRF24L01
// NRF24L01_WriteRegisterMulti(0x0D, (uint8_t *) "\x46\xB4\x80\x00", 4);
NRF24L01_WriteRegisterMulti(0x0D, (uint8_t *) "\x36\xB4\x80\x00", 4);
NRF24L01_WriteRegisterMulti(0x0E, (uint8_t *) "\x41\x10\x04\x82\x20\x08\x08\xF2\x7D\xEF\xFF", 11);
NRF24L01_WriteRegisterMulti(0x04, (uint8_t *) "\xC7\x96\x9A\x1B", 4);
NRF24L01_WriteRegisterMulti(0x04, (uint8_t *) "\xC1\x96\x9A\x1B", 4);
} else {
printf("nRF24L01 detected\n");
}
NRF24L01_Activate(0x53); // switch bank back
}
void initrx(void)
{
NRF24L01_Initialize();
reset_beken();
// 2-bytes CRC, radio off
uint8_t config = BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO) | BV(NRF24L01_00_PRIM_RX);
NRF24L01_WriteReg(NRF24L01_00_CONFIG, config);
NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknoledgement
NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01); // Enable data pipe 0
NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address
NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0xFF); // 4ms retransmit t/o, 15 tries
// NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x08); // Channel 8 - bind
NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps
NRF24L01_SetPower(TXPOWER_100mW);
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, V2X2_PAYLOAD_SIZE); // bytes of data payload for pipe 0
NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
uint8_t rx_tx_addr[] = {0x66, 0x88, 0x68, 0x68, 0x68};
// uint8_t rx_p1_addr[] = {0x88, 0x66, 0x86, 0x86, 0x86};
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 5);
// NRF24L01_WriteRegisterMulti(NRF24L01_0B_RX_ADDR_P1, rx_p1_addr, 5);
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
initialize_beken();
lib_timers_delaymilliseconds(50);
NRF24L01_FlushTx();
NRF24L01_FlushRx();
rf_ch_num = 0;
// Turn radio power on
config |= BV(NRF24L01_00_PWR_UP);
NRF24L01_WriteReg(NRF24L01_00_CONFIG, config);
// delayMicroseconds(150);
lib_timers_delaymilliseconds(1); // 6 times more than needed
valid_packets = missed_packets = bad_packets = 0;
if (usersettings.boundprotocol == PROTO_NONE) {
bind_phase = PHASE_NOT_BOUND;
prepare_to_bind();
} else {
// Prepare to listen to bound protocol, if fails
// try to bind
bind_phase = PHASE_JUST_BOUND;
set_bound();
}
switch_channel();
}
static void decode_bind_packet(uint8_t *packet)
{
switch (tryprotocol) {
case PROTO_V2X2:
if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
// Fill out usersettings with bound protocol parameters
v2x2_set_tx_id(&packet[7]);
// Read usersettings into current values
bind_phase = PHASE_BOUND;
set_bound();
}
break;
}
}
// Returns whether the data was successfully decoded
static bool decode_packet(uint8_t *packet, uint16_t *data)
{
switch (boundprotocol) {
case PROTO_NONE:
decode_bind_packet(packet);
break;
case PROTO_V2X2:
// Decode packet
if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
return false;
}
if (packet[7] != usersettings.txid[0] ||
packet[8] != usersettings.txid[1] ||
packet[9] != usersettings.txid[2])
{
bad_packets++;
return false;
}
// Restore regular interval
rx_timeout = 10000L; // 4ms interval, duplicate packets, (8ms unique) + 25%
// TREA order in packet to MultiWii order is handled by
// correct assignment to channelindex
// Throttle 0..255 to 1000..2000
data[v2x2_channelindex[0]] = ((uint16_t)packet[0]) * 1000 / 255 + 1000;
for (int i = 1; i < 4; ++i) {
uint8_t a = packet[i];
data[v2x2_channelindex[i]] = ((uint16_t)(a < 0x80 ? 0x7f - a : a)) * 1000 / 255 + 1000;
}
uint8_t flags[] = {V2X2_FLAG_LED, V2X2_FLAG_FLIP, V2X2_FLAG_CAMERA, V2X2_FLAG_VIDEO}; // two more unknown bits
for (int i = 4; i < 8; ++i) {
data[v2x2_channelindex[i]] = 1000 + ((packet[14] & flags[i-4]) ? 1000 : 0);
}
packet_timer = lib_timers_starttimer();
if (++valid_packets > 50) bind_phase = PHASE_BOUND;
return true;
}
return false;
}
void readrx(void)
{
int chan;
uint16_t data[8];
if (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_RX_DR))) {
uint32_t t = lib_timers_gettimermicroseconds(packet_timer);
if (t > rx_timeout) {
if (boundprotocol != PROTO_NONE) {
if (++missed_packets > 500 && bind_phase == PHASE_JUST_BOUND) {
valid_packets = missed_packets = bad_packets = 0;
bind_phase = PHASE_LOST_BINDING;
prepare_to_bind();
}
} else switch_channel();
packet_timer = lib_timers_starttimer();
}
return;
}
packet_timer = lib_timers_starttimer();
NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_RX_DR));
NRF24L01_ReadPayload(packet, V2X2_PAYLOAD_SIZE);
NRF24L01_FlushRx();
switch_channel();
if (!decode_packet(packet, data))
return;
for (chan = 0; chan < 8; ++chan) {
// data = pwmRead(chan);
// if (data < 750 || data > 2250)
// data = 1500;
// convert from 1000-2000 range to -1 to 1 fixedpointnum range and low pass filter to remove glitches
lib_fp_lowpassfilter(&global.rxvalues[chan], ((fixedpointnum) data[chan] - 1500) * 131L, global.timesliver, FIXEDPOINTONEOVERONESIXTYITH, TIMESLIVEREXTRASHIFT);
}
// reset the failsafe timer
global.failsafetimer = lib_timers_starttimer();
}