forked from iNavFlight/inav
-
Notifications
You must be signed in to change notification settings - Fork 0
/
nrf24_v202.c
261 lines (231 loc) · 9.18 KB
/
nrf24_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
/*
* This file is part of Cleanflight.
*
* Cleanflight 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
* (at your option) any later version.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
// this file is copied with modifications from bradwii for jd385
// see https://github.com/hackocopter/bradwii-jd385
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <platform.h>
#ifdef USE_RX_V202
#include "build/build_config.h"
#include "common/utils.h"
#include "drivers/rx_nrf24l01.h"
#include "drivers/system.h"
#include "rx/rx.h"
#include "rx/rx_spi.h"
#include "rx/nrf24_v202.h"
/*
* V202 Protocol
* No auto acknowledgment
* Payload size is 16 and static
* Data rate is 1Mbps, there is a 256Kbps data rate used by the Deviation transmitter implementation
* Bind Phase
* uses address {0x66, 0x88, 0x68, 0x68, 0x68}
* uses channels from the frequency hopping table
* Data phase
* uses same address as bind phase
* hops between 16 channels that are set using the txId sent in the bind packet and the frequency hopping table
*/
#define V2X2_PAYLOAD_SIZE 16
#define V2X2_NFREQCHANNELS 16
#define TXIDSIZE 3
#define V2X2_RC_CHANNEL_COUNT 11
enum {
// packet[14] flags
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,
// packet[10] flags
V2X2_FLAG_HEADLESS = 0x02,
V2X2_FLAG_MAG_CAL_X = 0x08,
V2X2_FLAG_MAG_CAL_Y = 0x20
};
enum {
PHASE_NOT_BOUND = 0,
PHASE_BOUND
};
// 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_UNIT_TESTED uint8_t rf_channels[V2X2_NFREQCHANNELS];
STATIC_UNIT_TESTED uint8_t rf_ch_num;
STATIC_UNIT_TESTED uint8_t bind_phase;
static uint32_t packet_timer;
STATIC_UNIT_TESTED uint8_t txid[TXIDSIZE];
static uint32_t rx_timeout;
extern uint16_t rxSpiRcData[];
static const unsigned char v2x2_channelindex[] = {RC_SPI_THROTTLE,RC_SPI_YAW,RC_SPI_PITCH,RC_SPI_ROLL,
RC_SPI_AUX1,RC_SPI_AUX2,RC_SPI_AUX3,RC_SPI_AUX4,RC_SPI_AUX5,RC_SPI_AUX6,RC_SPI_AUX7};
static void prepare_to_bind(void)
{
packet_timer = micros();
for (int i = 0; i < V2X2_NFREQCHANNELS; ++i) {
rf_channels[i] = v2x2_freq_hopping[0][i];
}
rx_timeout = 1000L;
}
static void switch_channel(void)
{
NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_channels[rf_ch_num]);
if (++rf_ch_num >= V2X2_NFREQCHANNELS) rf_ch_num = 0;
}
static void v2x2_set_tx_id(uint8_t *id)
{
uint8_t sum;
txid[0] = id[0];
txid[1] = id[1];
txid[2] = id[2];
sum = id[0] + id[1] + id[2];
// 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
rf_channels[i] = (val & 0x0f) ? val : val - 3;
}
}
static void decode_bind_packet(uint8_t *packet)
{
if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
// Fill out rf_channels with bound protocol parameters
v2x2_set_tx_id(&packet[7]);
bind_phase = PHASE_BOUND;
rx_timeout = 1000L; // find the channel as fast as possible
}
}
// Returns whether the data was successfully decoded
static rx_spi_received_e decode_packet(uint8_t *packet)
{
if(bind_phase != PHASE_BOUND) {
decode_bind_packet(packet);
return RX_SPI_RECEIVED_BIND;
}
// Decode packet
if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
return RX_SPI_RECEIVED_BIND;
}
if (packet[7] != txid[0] ||
packet[8] != txid[1] ||
packet[9] != txid[2]) {
return RX_SPI_RECEIVED_NONE;
}
// 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
rxSpiRcData[v2x2_channelindex[0]] = ((uint16_t)packet[0]) * 1000 / 255 + 1000;
for (int i = 1; i < 4; ++i) {
uint8_t a = packet[i];
rxSpiRcData[v2x2_channelindex[i]] = ((uint16_t)(a < 0x80 ? 0x7f - a : a)) * 1000 / 255 + 1000;
}
const 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) {
rxSpiRcData[v2x2_channelindex[i]] = (packet[14] & flags[i-4]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
}
const uint8_t flags10[] = {V2X2_FLAG_HEADLESS, V2X2_FLAG_MAG_CAL_X, V2X2_FLAG_MAG_CAL_Y};
for (int i = 8; i < 11; ++i) {
rxSpiRcData[v2x2_channelindex[i]] = (packet[10] & flags10[i-8]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
}
packet_timer = micros();
return RX_SPI_RECEIVED_DATA;
}
void v202Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
{
UNUSED(rcData);
UNUSED(packet);
// Ideally the decoding of the packet should be moved into here, to reduce the overhead of v202DataReceived function.
}
static rx_spi_received_e readrx(uint8_t *packet)
{
if (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_STATUS_RX_DR))) {
uint32_t t = micros() - packet_timer;
if (t > rx_timeout) {
switch_channel();
packet_timer = micros();
}
return RX_SPI_RECEIVED_NONE;
}
packet_timer = micros();
NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_RX_DR)); // clear the RX_DR flag
NRF24L01_ReadPayload(packet, V2X2_PAYLOAD_SIZE);
NRF24L01_FlushRx();
switch_channel();
return decode_packet(packet);
}
/*
* This is called periodically by the scheduler.
* Returns RX_SPI_RECEIVED_DATA if a data packet was received.
*/
rx_spi_received_e v202Nrf24DataReceived(uint8_t *packet)
{
return readrx(packet);
}
static void v202Nrf24Setup(rx_spi_protocol_e protocol)
{
NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV(NRF24L01_00_CONFIG_CRCO)); // 2-bytes CRC
NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknowledgment
NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BV(NRF24L01_02_EN_RXADDR_ERX_P0)); // Enable data pipe 0
NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address
NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0xFF); // 4ms retransmit t/o, 15 tries
if (protocol == NRF24RX_V202_250K) {
NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
} else {
NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
}
NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_RX_DR) | BV(NRF24L01_07_STATUS_TX_DS) | BV(NRF24L01_07_STATUS_MAX_RT)); // 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
#define RX_TX_ADDR_LEN 5
const uint8_t rx_tx_addr[RX_TX_ADDR_LEN] = {0x66, 0x88, 0x68, 0x68, 0x68};
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, RX_TX_ADDR_LEN);
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, RX_TX_ADDR_LEN);
NRF24L01_FlushTx();
NRF24L01_FlushRx();
rf_ch_num = 0;
bind_phase = PHASE_NOT_BOUND;
prepare_to_bind();
switch_channel();
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
}
void v202Nrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
rxRuntimeConfig->channelCount = V2X2_RC_CHANNEL_COUNT;
v202Nrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol);
}
#endif