Pārlūkot izejas kodu

Continued reordering the RX code.

Thomas Buck 8 gadus atpakaļ
vecāks
revīzija
8680d1aab7
1 mainītis faili ar 135 papildinājumiem un 225 dzēšanām
  1. 135
    225
      src/rx.c

+ 135
- 225
src/rx.c Parādīt failu

@@ -11,73 +11,57 @@
11 11
 #include "spi.h"
12 12
 #include "timer.h"
13 13
 
14
-
15 14
 // ----------------------------------------------------------------------------
16 15
 
17
-
18
-//#define DEBUG
19
-//#define DEBUG_RSSI
20
-//#define DEBUG0
21
-//#define DEBUG1
22
-//#define DEBUG2
23
-//#define DEBUG3
24
-//#define DEBUG4
25
-//#define DEBUG5
26
-//#define RSSI_OVER_PPM 7
27
-
28
-#define FAILSAFE
29
-
30
-// Used for RSSI_OVER_PPM
31
-int rssi;
32
-const int rssi_offset = 71;
33
-const int rssi_min = -103;
34
-const int rssi_max = -96;
35
-
36
-#define chanel_number 8  //set the number of chanels
37
-#define SEEK_CHANSKIP   13
38
-#define MAX_MISSING_PKT 20
39
-#define FAILSAFE_MISSING_PKT 170
40
-#define PPM_FrLen 22500
41
-#define PPM_PulseLen 300
42
-#define default_servo_value 1500
43
-#define onState 0  //set polarity of the pulses: 1 is positive, 0 is negative
44
-#define sigPin 10
45
-
46
-#define bind_pin A0     //C0 bind plug also servo8
47
-
48 16
 #define TRUE 1
49 17
 #define FALSE 0
50 18
 
51
-// Globals:
19
+#define CHANNELS 8
20
+#define PPM_MIN 1000
21
+#define PPM_MAX 2000
22
+#define RSSI_OVER_PPM 7
23
+#define HOP_DATA_LENGTH 60
24
+#define EEPROM_BASE_ADDRESS 100
25
+#define MISSING_PACKET_DELAY 9
26
+#define SEEK_CHANNEL_SKIP   13
27
+#define MAX_MISSING_PACKET 20
28
+#define FAILSAFE_MISSING_PACKET 170
29
+
52 30
 static uint8_t ccData[27];
53 31
 static uint8_t ccLen;
54 32
 static uint8_t packet = FALSE;
55 33
 static uint8_t channr = 0;
56 34
 static uint8_t missingPackets = 0;
57
-uint8_t hopData[60];
35
+uint8_t hopData[HOP_DATA_LENGTH];
58 36
 uint8_t listLength;
59 37
 uint8_t txid[2];
60 38
 static uint8_t counter = 0;
61
-volatile uint16_t Servo_data[10] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
62
-volatile int ppm[chanel_number];
63 39
 volatile uint8_t failed = FALSE;
64 40
 int count = 0;
65 41
 uint16_t c[8];
66 42
 
43
+int rssi;
44
+const int rssi_offset = 71;
45
+const int rssi_min = -103;
46
+const int rssi_max = -96;
67 47
 
68 48
 void getBind(void);
69
-void Store_bind(void);
70
-void updateRSSI(void);
71 49
 void loop(void);
72
-void nextChannel(uint8_t skip);
73 50
 void tuning(void);
74 51
 
75
-
76 52
 // ----------------------------------------------------------------------------
77 53
 
54
+static uint16_t ppmBuffer[CHANNELS];
55
+
56
+static long map(long x, long in_min, long in_max, long out_min, long out_max);
57
+static long constrain(long x, long min, long max);
78 58
 static void initialize(uint8_t bind);
79 59
 static void binding(void);
80 60
 
61
+static void nextChannel(uint8_t skip);
62
+static void readBindingData(void);
63
+static void writeBindingData(void);
64
+
81 65
 void rxInit(void) {
82 66
     initialize(1); // binding
83 67
     binding();
@@ -88,6 +72,19 @@ void rxInit(void) {
88 72
     cc2500Strobe(CC2500_SRX);
89 73
 }
90 74
 
75
+static long map(long x, long in_min, long in_max, long out_min, long out_max) {
76
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
77
+}
78
+
79
+static long constrain(long x, long min, long max) {
80
+    if (x < min) {
81
+        x = min;
82
+    }
83
+    if (x > max) {
84
+        x = max;
85
+    }
86
+    return x;
87
+}
91 88
 
92 89
 static void initialize(uint8_t bind) {
93 90
     cc2500ResetChip();
@@ -138,34 +135,24 @@ static void initialize(uint8_t bind) {
138 135
     cc2500WriteReg(CC2500_0A_CHANNR, 0x00);
139 136
 }
140 137
 
141
-
142 138
 static void binding(void) {
143 139
     uint8_t jumper2 = 0; //bind_jumper();
144 140
 
145 141
     while (1) {
146 142
         if (jumper2 == 0) {
147 143
             // bind complete or no bind
148
-            uint8_t adr = 100;
149
-            for (uint8_t i = 0; i < 2; i++) {
150
-                txid[i] = eeprom_read_byte(adr + i);
151
-            }
144
+            readBindingData();
152 145
             if ((txid[0] == 0xff) && (txid[1] == 0xff)) {
153 146
                 // No valid txid, forcing bind
154 147
                 jumper2 = 1;
155 148
                 continue;
156 149
             }
157
-            for (uint8_t i = 0; i < sizeof(hopData); i++) {
158
-                hopData[i] = eeprom_read_byte(adr + 10 + i);
159
-            }
160
-            listLength = eeprom_read_byte(adr + 100);
161
-            count = eeprom_read_byte(adr + 101);
162 150
             break;
163 151
         } else {
164 152
             tuning();
165 153
             //count=0xC8;//for test
166 154
             cc2500WriteReg(CC2500_0C_FSCTRL0, count);
167
-            int adr = 100;
168
-            eeprom_write_byte(adr + 101, count);
155
+            eeprom_write_byte(EEPROM_BASE_ADDRESS + 101, count);
169 156
             getBind();
170 157
             // TODO reset?!
171 158
             while (1) { }
@@ -173,175 +160,116 @@ static void binding(void) {
173 160
     }
174 161
 }
175 162
 
163
+static void nextChannel(uint8_t skip) {
164
+    channr += skip;
165
+    if (channr >= listLength) {
166
+        channr -= listLength;
167
+    }
176 168
 
177
-// ----------------------------------------------------------------------------
169
+    cc2500WriteReg(CC2500_0A_CHANNR, hopData[channr]);
170
+    cc2500WriteReg(CC2500_23_FSCAL3, 0x89);
171
+}
178 172
 
173
+static void readBindingData() {
174
+    for (uint8_t i = 0; i < 2; i++) {
175
+        txid[i] = eeprom_read_byte(EEPROM_BASE_ADDRESS + i);
176
+    }
177
+    for (uint8_t i = 0; i < HOP_DATA_LENGTH; i++) {
178
+        hopData[i] = eeprom_read_byte(EEPROM_BASE_ADDRESS + 10 + i);
179
+    }
180
+    listLength = eeprom_read_byte(EEPROM_BASE_ADDRESS + 100);
181
+    count = eeprom_read_byte(EEPROM_BASE_ADDRESS + 101);
182
+}
179 183
 
184
+static void writeBindingData() {
185
+    for (uint8_t i = 0; i < 2; i++) {
186
+        eeprom_write_byte(EEPROM_BASE_ADDRESS + i, txid[i]);
187
+    }
188
+    for (uint8_t i = 0; i < HOP_DATA_LENGTH; i++) {
189
+        eeprom_write_byte(EEPROM_BASE_ADDRESS + 10 + i, hopData[i]);
190
+    }
191
+    eeprom_write_byte(EEPROM_BASE_ADDRESS + 100, listLength);
192
+}
193
+
194
+// ----------------------------------------------------------------------------
180 195
 
181 196
 // Receives complete bind setup
182
-void getBind(void)
183
-{
184
-    cc2500Strobe(CC2500_SRX);//enter in rx mode
185
-    listLength = 0;
186
-    uint8_t eol = FALSE;
187
-    //           len|bind |tx id|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|01
188
-    // Start by getting bind packet 0 and the txid
189
-    //        0  1   2  txid0(3) txid1()4    5  6  7   8  9 10 11 12 13 14 15 16 17
190
-    //ccdata    //11 03 01  d7       2d          00 00 1e 3c 5b 78 00 00 00 00 00 00 01
191
-    //11 03 01  19       3e          00 02 8e 2f bb 5c 00 00 00 00 00 00 01
197
+void getBind(void) {
198
+    cc2500Strobe(CC2500_SRX); // enter in rx mode
199
+
192 200
     while (1) {
193 201
         if (GDO_1) {
194 202
             ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
195 203
             if (ccLen) {
196 204
                 cc2500ReadFifo((uint8_t *)ccData, ccLen);
197
-                if (ccData[ccLen - 1] & 0x80) {
198
-                    if (ccData[2] == 0x01) {
199
-                        if (ccData[5] == 0x00) {
200
-                            txid[0] = ccData[3];
201
-                            txid[1] = ccData[4];
202
-                            for (uint8_t n = 0; n < 5; n++) {
203
-                                hopData[ccData[5] + n] = ccData[6 + n];
204
-                            }
205
-                            break;
206
-                        }
205
+                if ((ccData[ccLen - 1] & 0x80)
206
+                        && (ccData[2] == 0x01)
207
+                        && (ccData[5] == 0x00)) {
208
+                    txid[0] = ccData[3];
209
+                    txid[1] = ccData[4];
210
+                    for (uint8_t n = 0; n < 5; n++) {
211
+                        hopData[ccData[5] + n] = ccData[6 + n];
207 212
                     }
213
+                    break;
208 214
                 }
209 215
             }
210 216
         }
211 217
     }
212 218
 
213
-    #if defined(DEBUG)
214
-        Serial.print(txid[0], HEX);
215
-        Serial.println(txid[1], HEX);
216
-    #endif
217
-
219
+    listLength = 0;
220
+    uint8_t eol = FALSE;
218 221
     for (uint8_t bindIdx = 0x05; bindIdx <= 120; bindIdx += 5) {
219 222
         while (1) {
220 223
             if (GDO_1) {
221 224
                 ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
222 225
                 if (ccLen) {
223 226
                     cc2500ReadFifo((uint8_t *)ccData, ccLen);
224
-                    if (ccData[ccLen - 1] & 0x80) {
225
-                        if (ccData[2] == 0x01) {
226
-#if DEBUG3
227
-                            Serial.print("ccLen = ");
228
-                            Serial.println(ccLen);
229
-#endif
230
-                            if ((ccData[3] == txid[0]) && (ccData[4] == txid[1])) {
231
-#if DEBUG3
232
-                                Serial.print("ccData[5] = ");
233
-                                Serial.println(ccData[5]);
234
-                                Serial.print("bindIdx = ");
235
-                                Serial.println(bindIdx);
236
-#endif
237
-                                if (ccData[5] == bindIdx) {
238
-                                    for (uint8_t n = 0; n < 5; n++) {
239
-#if DEBUG3
240
-                                        Serial.print("ccData[6 + n] = ");
241
-                                        Serial.println(ccData[6 + n]);
242
-                                        Serial.print("ccData[ccLen - 3] = ");
243
-                                        Serial.println(ccData[ccLen - 3]);
244
-#endif
245
-                                        //if (ccData[6 + n] == ccData[ccLen - 3]) {
246
-                                          if (ccData[6 + n] <= 3) {
247
-                                            eol = TRUE;
248
-                                            #if defined(DEBUG)
249
-                                                Serial.print("listLength: ");
250
-                                                Serial.println(listLength);
251
-                                            #endif
252
-                                            listLength = ccData[5] + n;
253
-                                            break;
254
-                                        }
255
-                                        hopData[ccData[5] + n] = ccData[6 + n];
256
-                                    }
257
-                                    break;
258
-                                }
227
+                    if ((ccData[ccLen - 1] & 0x80)
228
+                            && (ccData[2] == 0x01)
229
+                            && (ccData[3] == txid[0])
230
+                            && (ccData[4] == txid[1])
231
+                            && (ccData[5] == bindIdx)) {
232
+                        for (uint8_t n = 0; n < 5; n++) {
233
+                            //if (ccData[6 + n] == ccData[ccLen - 3]) {
234
+                              if (ccData[6 + n] <= 3) {
235
+                                eol = TRUE;
236
+                                listLength = ccData[5] + n;
237
+                                break;
259 238
                             }
239
+                            hopData[ccData[5] + n] = ccData[6 + n];
260 240
                         }
241
+                        break;
261 242
                     }
262 243
                 }
263 244
             }
264 245
         }
265
-        #if defined(DEBUG)
266
-                Serial.println(bindIdx / 5);
267
-        #endif
268
-        if (eol) break; // End of list found, stop!
269
-    }
270 246
 
271
-    #if defined(DEBUG)
272
-        listLength = 47;
273
-        Serial.println("jumpIdx list: ");
274
-        for (uint8_t jumpIdx = 0; jumpIdx < (listLength); jumpIdx++) {
275
-            Serial.print(" ");
276
-            Serial.print(hopData[jumpIdx], HEX);
277
-            Serial.print(" ");
247
+        if (eol) {
248
+            break; // End of list found, stop!
278 249
         }
279
-        Serial.println(" ");
280
-    #endif
281
-
282
-    Store_bind();
283
-    cc2500Strobe(CC2500_SIDLE);    // Back to idle
284
-}
285
-
286
-
287
-
288
-
289
-
290
-void Store_bind()
291
-{
292
-    uint8_t i;
293
-    int adr = 100;
294
-    for (i = 0; i < 2; i++) {
295
-        eeprom_write_byte(adr + i, txid[i]);
296
-    }
297
-    for (i = 0; i < sizeof(hopData); i++) {
298
-        eeprom_write_byte(adr + 10 + i, hopData[i]);
299
-    }
300
-    eeprom_write_byte(adr + 100, listLength);
301
-}
302
-
303
-
304
-
305
-
306
-void updateRSSI() {
307
-#if defined(RSSI_OVER_PPM)
308
-    int rssi_dec = cc2500ReadReg(CC2500_34_RSSI | CC2500_READ_BURST);
309
-    if (rssi_dec < 128) {
310
-        rssi = ((rssi_dec / 2) - rssi_offset) & 0x7f;
311
-    } else {
312
-        rssi = (((rssi_dec - 256) / 2)) - rssi_offset;
313 250
     }
314 251
 
315
-  #if defined(DEBUG_RSSI2)
316
-    Serial.print(millis());
317
-    Serial.print("\t");
318
-    Serial.println(rssi);
319
-  #endif
320
-
321
-    rssi = constrain(rssi, rssi_min, rssi_max);
322
-#endif
252
+    writeBindingData();
253
+    cc2500Strobe(CC2500_SIDLE); // Back to idle
323 254
 }
324 255
 
325
-void loop()
326
-{
256
+void loop() {
327 257
     time_t time = timerGet();
328 258
 
329
-    #if defined(FAILSAFE)
330
-        if (missingPackets > FAILSAFE_MISSING_PKT) {
331
-            failed = TRUE;
332
-            missingPackets = 0;
333
-        }
334
-    #endif
259
+    if (missingPackets > FAILSAFE_MISSING_PACKET) {
260
+        failed = TRUE;
261
+        missingPackets = 0;
262
+    }
335 263
 
336 264
     while (1) {
337
-        if ((timerGet() - time) > 9) {
265
+        if ((timerGet() - time) > MISSING_PACKET_DELAY) {
338 266
             missingPackets++;
339 267
             cc2500Strobe(CC2500_SIDLE);
340
-            if (missingPackets > MAX_MISSING_PKT) {
341
-                nextChannel(SEEK_CHANSKIP);
268
+            if (missingPackets > MAX_MISSING_PACKET) {
269
+                nextChannel(SEEK_CHANNEL_SKIP);
342 270
                 counter++;
343
-                //if (counter > (MAX_MISSING_PKT << 1)) LED_ON;
344
-                if (counter == (MAX_MISSING_PKT << 2)) counter = 0;
271
+                //if (counter > (MAX_MISSING_PACKET << 1)) LED_ON;
272
+                if (counter == (MAX_MISSING_PACKET << 2)) counter = 0;
345 273
                 break;
346 274
             } else
347 275
                 nextChannel(1);
@@ -350,7 +278,7 @@ void loop()
350 278
         if (GDO_1) {
351 279
             ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
352 280
             if (ccLen > 20)
353
-                ccLen = 20;//
281
+                ccLen = 20;
354 282
             if (ccLen) {
355 283
                 cc2500ReadFifo((uint8_t *)ccData, ccLen);
356 284
                 if (ccData[ccLen - 1] & 0x80) { // Only if correct CRC
@@ -359,7 +287,17 @@ void loop()
359 287
                         if ((ccData[1] == txid[0]) && (ccData[2] == txid[1])) { // Only if correct txid
360 288
                             packet = TRUE;
361 289
                             //sei();
362
-                            updateRSSI();
290
+
291
+#ifdef RSSI_OVER_PPM
292
+                            int rssi_dec = cc2500ReadReg(CC2500_34_RSSI | CC2500_READ_BURST);
293
+                            if (rssi_dec < 128) {
294
+                                rssi = ((rssi_dec / 2) - rssi_offset) & 0x7f;
295
+                            } else {
296
+                                rssi = (((rssi_dec - 256) / 2)) - rssi_offset;
297
+                            }
298
+                            rssi = constrain(rssi, rssi_min, rssi_max);
299
+#endif
300
+
363 301
                             cc2500Strobe(CC2500_SIDLE);
364 302
                             nextChannel(1);
365 303
                             failed = FALSE;
@@ -367,9 +305,7 @@ void loop()
367 305
                         }
368 306
                     }
369 307
                 }
370
-
371 308
             }
372
-
373 309
         }
374 310
     }
375 311
 
@@ -385,50 +321,27 @@ void loop()
385 321
         c[6] = (uint16_t)(ccData[17] & 0x0F) << 8 | ccData[14];
386 322
         c[7] = (uint16_t)(ccData[17] & 0xF0) << 4 | ccData[15];
387 323
         //sei();
388
-        for (int i = 0; i < 8; i++) {
389
-            Servo_data[i] = 0.67 * c[i];
390
-            if (Servo_data[i] < 900) { //added new
391
-                Servo_data[i] = 1500; //added new
392
-                Servo_data[2] = 1000;
324
+
325
+        for (int i = 0; i < CHANNELS; i++) {
326
+            ppmBuffer[i] = 0.67 * c[i];
327
+            /*
328
+            if (ppmBuffer[i] < 900) {
329
+                ppmBuffer[i] = 1500;
330
+                //ppmBuffer[2] = 1000;
393 331
             }
394
-            ppm[i] = Servo_data[i];
332
+            */
395 333
         }
396
-        #if defined(RSSI_OVER_PPM)
397
-          ppm[RSSI_OVER_PPM] = map(rssi, rssi_min, rssi_max, 1000, 2000);
398
-        #endif
399
-        #if defined(DEBUG5)
400
-                //Serial.println(rssi);
401
-        #endif
402
-        #if defined(DEBUG0)
403
-                for (int i = 0; i < 8; i++) {
404
-                    Serial.print(" ");
405
-                    Serial.print(Servo_data[i]);
406
-                    Serial.print(" ");
407
-                }
408
-                Serial.println(" ");
409
-        #endif
334
+
335
+#ifdef RSSI_OVER_PPM
336
+        ppmBuffer[RSSI_OVER_PPM] = map(rssi, rssi_min, rssi_max, PPM_MIN, PPM_MAX);
337
+#endif
410 338
     }
411 339
 
412 340
     cc2500Strobe(CC2500_SRX);
413 341
 }
414 342
 
415
-
416
-
417
-
418
-void nextChannel(uint8_t skip)
419
-{
420
-    channr += skip;//
421
-    if (channr >= listLength) channr -= listLength;
422
-    cc2500WriteReg(CC2500_0A_CHANNR, hopData[channr]);
423
-    cc2500WriteReg(CC2500_23_FSCAL3, 0x89);
424
-
425
-}
426
-
427
-
428
-
429
-void tuning()
430
-{
431
-    cc2500Strobe(CC2500_SRX);//enter in rx mode
343
+void tuning() {
344
+    cc2500Strobe(CC2500_SRX); // enter in rx mode
432 345
     int count1 = 0;
433 346
     while (1) {
434 347
         count1++;
@@ -439,7 +352,7 @@ void tuning()
439 352
             count1 = 0;
440 353
             cc2500WriteReg(CC2500_0C_FSCTRL0, count);  // Frequency offset hack
441 354
             count = count + 10;
442
-            //cc2500Strobe(CC2500_SRX);//enter in rx mode
355
+            //cc2500Strobe(CC2500_SRX);// enter in rx mode
443 356
         }
444 357
         if (GDO_1) {
445 358
             ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
@@ -455,8 +368,5 @@ void tuning()
455 368
             }
456 369
         }
457 370
     }
458
-    #if defined(DEBUG1)
459
-        Serial.println(count, HEX);
460
-    #endif
461 371
 }
462 372
 

Notiek ielāde…
Atcelt
Saglabāt