?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Create release.yml
[betaflight.git] / src / main / rx / cc2500_sfhss.c
blobffaee6ef76bfc11cc0ca49085625214d58dfc222
1 /*
2 * This file is part of Cleanflight and Betaflight.
3 *
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
9 *
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
17 *
18 * If not, see <http://www.gnu.org/licenses/>.
19 */
20
21 #include <stdbool.h>
22
23 #include "platform.h"
24
25 #ifdef USE_RX_SFHSS_SPI
26
27 #include "build/build_config.h"
28 #include "build/debug.h"
29
30 #include "common/maths.h"
31
32 #include "drivers/io.h"
33 #include "drivers/rx/rx_cc2500.h"
34 #include "drivers/rx/rx_spi.h"
35 #include "drivers/time.h"
36
37 #include "config/config.h"
38
39 #include "pg/rx.h"
40 #include "pg/rx_spi.h"
41 #include "pg/rx_spi_cc2500.h"
42
43 #include "rx/cc2500_common.h"
44 #include "rx/rx.h"
45 #include "rx/rx_spi.h"
46 #include "rx/rx_spi_common.h"
47
48 #include "cc2500_sfhss.h"
49
50 #define BIND_CH 15
51 #define SFHSS_PACKET_LEN 15
52 #define BIND_TUNE_STEP 4
53
54 #define SFHSSCH2CHANNR(ch) (ch * 6 + 16)
55 #define GET_CHAN(x) ((int)((x[5]>>3) & 0x1f))
56 #define GET_CODE(x) (((x[11] & 0x7)<<2 ) | ((x[12]>>6) & 0x3))
57 #define GET_COMMAND(x) (x[12] & 0xf)
58 #define GET_CH1(x) ((uint16_t)(((x[5] & 0x07)<<9 | x[6]<<1) | (x[7] & 0x80)>>7))
59 #define GET_CH2(x) (uint16_t)(((x[7] & 0x7f)<<5 | (x[8] & 0xf8)>>3))
60 #define GET_CH3(x) (uint16_t)(((x[8] & 0x07)<<9 | x[9]<<1) | (x[10] & 0x80)>>7)
61 #define GET_CH4(x) (uint16_t)(((x[10] & 0x7f)<<5 | (x[11] & 0xf8)>>3))
62 #define GET_TXID1(x) (uint8_t)(x[1])
63 #define GET_TXID2(x) (uint8_t)(x[2])
64 #define SET_STATE(x) {protocolState = x; DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_STATE, x);}
65
66 #define NEXT_CH_TIME_HUNT 500000 /* hunt */
67 #define NEXT_CH_TIME_SYNC0 6800 /* sync no recv */
68 #define NEXT_CH_TIME_SYNC1 3500 /* sync ch1-4 recv */
69 #define NEXT_CH_TIME_SYNC2 500 /* sync ch5-8 recv */
70
71 static int8_t sfhss_channel = 0;
72 static int8_t sfhss_code = 0;
73
74 static timeMs_t start_time;
75 static uint8_t protocolState;
76
77 static uint32_t missingPackets;
78
79 static uint8_t calData[32][3];
80 static timeMs_t timeTunedMs;
81 static int8_t bindOffset_max = 0;
82 static int8_t bindOffset_min = 0;
83
84 const cc2500RegisterConfigElement_t cc2500SfhssConfigPart1[] =
85 {
86 { CC2500_02_IOCFG0, 0x01 },
87 { CC2500_03_FIFOTHR, 0x07 },
88 { CC2500_04_SYNC1, 0xD3 },
89 { CC2500_05_SYNC0, 0x91 },
90 { CC2500_06_PKTLEN, 0x0D },
91 { CC2500_07_PKTCTRL1, 0x04 },
92 { CC2500_08_PKTCTRL0, 0x0C },
93 { CC2500_09_ADDR, 0x29 },
94 { CC2500_0B_FSCTRL1, 0x06 }
95 };
96
97 const cc2500RegisterConfigElement_t cc2500SfhssConfigPart2[] =
98 {
99 { CC2500_0D_FREQ2, 0x5C },
100 { CC2500_0E_FREQ1, 0x4E },
101 { CC2500_0F_FREQ0, 0xC4 },
102 { CC2500_10_MDMCFG4, 0x7C },
103 { CC2500_11_MDMCFG3, 0x43 },
104 { CC2500_12_MDMCFG2, 0x03 },
105 { CC2500_13_MDMCFG1, 0x23 },
106 { CC2500_14_MDMCFG0, 0x3B },
107 { CC2500_15_DEVIATN, 0x44 },
108 { CC2500_17_MCSM1, 0x0F },
109 { CC2500_18_MCSM0, 0x08 },
110 { CC2500_19_FOCCFG, 0x1D },
111 { CC2500_1A_BSCFG, 0x6C },
112 { CC2500_1B_AGCCTRL2, 0x03 },
113 { CC2500_1C_AGCCTRL1, 0x40 },
114 { CC2500_1D_AGCCTRL0, 0x91 },
115 { CC2500_21_FREND1, 0x56 },
116 { CC2500_22_FREND0, 0x10 },
117 { CC2500_23_FSCAL3, 0xA9 },
118 { CC2500_24_FSCAL2, 0x0A },
119 { CC2500_25_FSCAL1, 0x00 },
120 { CC2500_26_FSCAL0, 0x11 },
121 { CC2500_29_FSTEST, 0x59 },
122 { CC2500_2C_TEST2, 0x88 },
123 { CC2500_2D_TEST1, 0x31 },
124 { CC2500_2E_TEST0, 0x0B },
125 { CC2500_3E_PATABLE, 0xFF }
126 };
127
128 static void initialise()
129 {
130 cc2500Reset();
131
132 cc2500ApplyRegisterConfig(cc2500SfhssConfigPart1, sizeof(cc2500SfhssConfigPart1));
133
134 cc2500WriteReg(CC2500_0C_FSCTRL0, rxCc2500SpiConfig()->bindOffset);
135
136 cc2500ApplyRegisterConfig(cc2500SfhssConfigPart2, sizeof(cc2500SfhssConfigPart2));
137
138 for (unsigned c = 0; c < 30; c++) {
139 //calibrate all channels
140 cc2500Strobe(CC2500_SIDLE);
141 cc2500WriteReg(CC2500_0A_CHANNR, SFHSSCH2CHANNR(c));
142 cc2500Strobe(CC2500_SCAL);
143 delayMicroseconds(900);
144 calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3);
145 calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2);
146 calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1);
147 }
148 }
149
150 static bool sfhssRecv(uint8_t *packet)
151 {
152 uint8_t ccLen;
153
154 if (!(rxSpiGetExtiState())) {
155 return false;
156 }
157 ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
158 if (ccLen < SFHSS_PACKET_LEN) {
159 return false;
160 }
161
162 cc2500ReadFifo(packet, SFHSS_PACKET_LEN);
163 return true;
164 }
165
166 static bool sfhssPacketParse(uint8_t *packet, bool check_txid)
167 {
168 if (!(packet[SFHSS_PACKET_LEN - 1] & 0x80)) {
169 return false; /* crc fail */
170 }
171 if (packet[0] != 0x81) {
172 return false; /* sfhss header fail */
173 }
174 if (GET_CHAN(packet) != sfhss_channel) {
175 return false; /* channel fail */
176 }
177
178 if (check_txid) {
179 if ((rxCc2500SpiConfigMutable()->bindTxId[0] != GET_TXID1(packet)) ||
180 (rxCc2500SpiConfigMutable()->bindTxId[1] != GET_TXID2(packet))) {
181 return false; /* txid fail */
182 }
183 }
184
185 cc2500setRssiDbm(packet[SFHSS_PACKET_LEN - 2]);
186 sfhss_code = GET_CODE(packet);
187
188 return true;
189 }
190
191 void sfhssRx(void)
192 {
193 cc2500Strobe(CC2500_SIDLE);
194 cc2500WriteReg(CC2500_23_FSCAL3, calData[sfhss_channel][0]);
195 cc2500WriteReg(CC2500_24_FSCAL2, calData[sfhss_channel][1]);
196 cc2500WriteReg(CC2500_25_FSCAL1, calData[sfhss_channel][2]);
197 cc2500WriteReg(CC2500_0A_CHANNR, SFHSSCH2CHANNR(sfhss_channel));
198 cc2500Strobe(CC2500_SFRX);
199 cc2500Strobe(CC2500_SRX);
200 }
201
202 static void initTuneRx(void)
203 {
204 timeTunedMs = millis();
205 bindOffset_min = -64;
206 DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MIN, bindOffset_min);
207 cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_min);
208 sfhss_channel = BIND_CH;
209 sfhssRx();
210 }
211
212 static bool tune1Rx(uint8_t *packet)
213 {
214 if (bindOffset_min >= 126) {
215 bindOffset_min = -126;
216 DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MIN, bindOffset_min);
217 }
218 if ((millis() - timeTunedMs) > 220) { // 220ms
219 timeTunedMs = millis();
220 bindOffset_min += BIND_TUNE_STEP << 2;
221 DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MIN, bindOffset_min);
222 cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_min);
223 cc2500Strobe(CC2500_SRX);
224 }
225 if (sfhssRecv(packet)) {
226 if (sfhssPacketParse(packet, false)) {
227 if ((packet[SFHSS_PACKET_LEN - 1] & 0x7F) > 40 ) { /* lqi */
228 rxCc2500SpiConfigMutable()->bindTxId[0] = GET_TXID1(packet);
229 rxCc2500SpiConfigMutable()->bindTxId[1] = GET_TXID2(packet);
230 bindOffset_max = bindOffset_min;
231 DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MAX, bindOffset_max);
232 cc2500Strobe(CC2500_SRX);
233 timeTunedMs = millis();
234 return true;
235 }
236 }
237 cc2500Strobe(CC2500_SRX);
238 }
239 return false;
240 }
241
242 static bool tune2Rx(uint8_t *packet)
243 {
244 rxSpiLedBlink(100);
245 if (((millis() - timeTunedMs) > 880) || bindOffset_max > (126 - BIND_TUNE_STEP)) { // 220ms *4
246 timeTunedMs = millis();
247 cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_min);
248 cc2500Strobe(CC2500_SRX);
249 return true;
250 }
251 if (sfhssRecv(packet)) {
252 if (sfhssPacketParse(packet, true)) {
253 timeTunedMs = millis();
254 bindOffset_max += BIND_TUNE_STEP;
255 DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MAX, bindOffset_max);
256 cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_max);
257 }
258 cc2500Strobe(CC2500_SRX);
259 }
260 return false;
261 }
262
263 static bool tune3Rx(uint8_t *packet)
264 {
265 rxSpiLedBlink(100);
266 if (((millis() - timeTunedMs) > 880) || bindOffset_min < (-126 + BIND_TUNE_STEP)) { // 220ms *4
267 return true;
268 }
269 if (sfhssRecv(packet)) {
270 if (sfhssPacketParse(packet, true)) {
271 timeTunedMs = millis();
272 bindOffset_min -= BIND_TUNE_STEP;
273 DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MIN, bindOffset_min);
274 cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_min);
275 }
276 cc2500Strobe(CC2500_SRX);
277 }
278 return false;
279 }
280
281 void sfhssnextChannel(void)
282 {
283 do {
284 sfhss_channel += sfhss_code + 2;
285 if (sfhss_channel > 29) {
286 sfhss_channel -= 31;
287 }
288 } while ( sfhss_channel < 0);
289
290 sfhssRx();
291 }
292
293 void sfhssSpiSetRcData(uint16_t *rcData, const uint8_t *payload)
294 {
295 if ( GET_COMMAND(payload) & 0x8 ) {
296 rcData[4] = GET_CH1(payload);
297 rcData[5] = GET_CH2(payload);
298 rcData[6] = GET_CH3(payload);
299 rcData[7] = GET_CH4(payload);
300 } else {
301 rcData[0] = GET_CH1(payload);
302 rcData[1] = GET_CH2(payload);
303 rcData[2] = GET_CH3(payload);
304 rcData[3] = GET_CH4(payload);
305 }
306 }
307
308 rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet)
309 {
310 static uint16_t dataMissingFrame = 0;
311 static timeUs_t nextFrameReceiveStartTime = 0;
312 static uint8_t frame_recvd = 0;
313 timeUs_t currentPacketReceivedTime;
314 rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
315
316 currentPacketReceivedTime = micros();
317 switch (protocolState) {
318 case STATE_INIT:
319 if ((millis() - start_time) > 10) {
320 rxSpiLedOff();
321 dataMissingFrame = 0;
322 initialise();
323 SET_STATE(STATE_BIND);
324 DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_MISSING_FRAME, dataMissingFrame);
325 }
326 break;
327 case STATE_BIND:
328 if (rxSpiCheckBindRequested(true)) {
329 rxSpiLedOn();
330 initTuneRx();
331 SET_STATE(STATE_BIND_TUNING1);
332 } else {
333 SET_STATE(STATE_HUNT);
334 sfhssnextChannel();
335 setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
336 nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_HUNT;
337 }
338 break;
339 case STATE_BIND_TUNING1:
340 if (tune1Rx(packet)) {
341 SET_STATE(STATE_BIND_TUNING2);
342 }
343 break;
344 case STATE_BIND_TUNING2:
345 if (tune2Rx(packet)) {
346 SET_STATE(STATE_BIND_TUNING3);
347 }
348 break;
349 case STATE_BIND_TUNING3:
350 if (tune3Rx(packet)) {
351 if (((int16_t)bindOffset_max - (int16_t)bindOffset_min) <= 2) {
352 initTuneRx();
353 SET_STATE(STATE_BIND_TUNING1); // retry
354 } else {
355 rxCc2500SpiConfigMutable()->bindOffset = ((int16_t)bindOffset_max + (int16_t)bindOffset_min) / 2 ;
356 SET_STATE(STATE_BIND_COMPLETE);
357 }
358 }
359 break;
360 case STATE_BIND_COMPLETE:
361 writeEEPROM();
362 ret = RX_SPI_RECEIVED_BIND;
363 SET_STATE(STATE_INIT);
364 break;
365 case STATE_HUNT:
366 if (sfhssRecv(packet)) {
367 if (sfhssPacketParse(packet, true)) {
368 if (GET_COMMAND(packet) & 0x8) { /* ch=5-8 */
369 missingPackets = 0;
370 rxSpiLedOn();
371 frame_recvd = 0x3;
372 SET_STATE(STATE_SYNC);
373 nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_SYNC2;
374 return RX_SPI_RECEIVED_NONE;
375 }
376 }
377 cc2500Strobe(CC2500_SRX);
378 } else if (cmpTimeUs(currentPacketReceivedTime, nextFrameReceiveStartTime) > 0) {
379 rxSpiLedBlink(500);
380 #if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) // SE4311 chip
381 cc2500switchAntennae();
382 #endif
383 sfhssnextChannel();
384 nextFrameReceiveStartTime += NEXT_CH_TIME_HUNT;
385 } else if (rxSpiCheckBindRequested(false)) {
386 SET_STATE(STATE_INIT);
387 break;
388 }
389 break;
390 case STATE_SYNC:
391 if (sfhssRecv(packet)) {
392 if (sfhssPacketParse(packet, true)) {
393 missingPackets = 0;
394 if ( GET_COMMAND(packet) & 0x8 ) {
395 nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_SYNC2;
396 frame_recvd |= 0x2; /* ch5-8 */
397 } else {
398 nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_SYNC1;
399 cc2500Strobe(CC2500_SRX);
400 frame_recvd |= 0x1; /* ch1-4 */
401 }
402 if (GET_COMMAND(packet) & 0x4) {
403 return RX_SPI_RECEIVED_NONE; /* failsafe data */
404 }
405 return RX_SPI_RECEIVED_DATA;
406 }
407 cc2500Strobe(CC2500_SRX);
408 } else if (cmpTimeUs(currentPacketReceivedTime, nextFrameReceiveStartTime) > 0) {
409 nextFrameReceiveStartTime += NEXT_CH_TIME_SYNC0;
410 if (frame_recvd != 0x3) {
411 DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_MISSING_FRAME, ++dataMissingFrame);
412 }
413 if (frame_recvd == 0) {
414 if (++missingPackets > MAX_MISSING_PKT) {
415 SET_STATE(STATE_HUNT);
416 sfhssnextChannel();
417 setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
418 nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_HUNT;
419 break;
420 }
421 #if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) // SE4311 chip
422 if (missingPackets >= 2) {
423 cc2500switchAntennae();
424 }
425 #endif
426 }
427 frame_recvd = 0;
428 sfhssnextChannel();
429 } else if (rxSpiCheckBindRequested(false)) {
430 SET_STATE(STATE_INIT);
431 break;
432 }
433 break;
434 }
435
436 return ret;
437 }
438
439 bool sfhssSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
440 {
441 UNUSED(extiConfig);
442
443 rxSpiCommonIOInit(rxSpiConfig);
444
445 cc2500SpiInit();
446
447 rxRuntimeState->channelCount = RC_CHANNEL_COUNT_SFHSS;
448
449 start_time = millis();
450 SET_STATE(STATE_INIT);
451
452 return true;
453 }
454 #endif
455