Browse Source

First prototype more or less finished.

Thomas Buck 8 years ago
parent
commit
cfd68ba5b9
5 changed files with 160 additions and 154 deletions
  1. 1
    0
      include/cppm.h
  2. 1
    0
      include/rx.h
  3. 8
    0
      src/cppm.c
  4. 5
    2
      src/main.c
  5. 145
    152
      src/rx.c

+ 1
- 0
include/cppm.h View File

14
 extern volatile uint16_t cppmData[8];
14
 extern volatile uint16_t cppmData[8];
15
 
15
 
16
 void cppmInit(void);
16
 void cppmInit(void);
17
+void cppmCopy(uint16_t *data);
17
 
18
 
18
 #endif
19
 #endif
19
 
20
 

+ 1
- 0
include/rx.h View File

6
 #define _RX_H
6
 #define _RX_H
7
 
7
 
8
 void rxInit(void);
8
 void rxInit(void);
9
+void rxReceivePacket(void);
9
 
10
 
10
 #endif
11
 #endif
11
 
12
 

+ 8
- 0
src/cppm.c View File

68
     triggerIn(PULSE_LOW);
68
     triggerIn(PULSE_LOW);
69
 }
69
 }
70
 
70
 
71
+void cppmCopy(uint16_t *data) {
72
+    cli();
73
+    for (int i = 0; i < CHANNELS; i++) {
74
+        cppmData[i] = data[i];
75
+    }
76
+    sei();
77
+}
78
+
71
 static void triggerIn(uint16_t us) {
79
 static void triggerIn(uint16_t us) {
72
     TCNT0 = 0; // Reset Timer
80
     TCNT0 = 0; // Reset Timer
73
     if (us <= (TIME_AFTER_OVERFLOW - 1)) {
81
     if (us <= (TIME_AFTER_OVERFLOW - 1)) {

+ 5
- 2
src/main.c View File

21
     timerInit();
21
     timerInit();
22
 
22
 
23
     sei(); // Enable interrupts (required for timer)
23
     sei(); // Enable interrupts (required for timer)
24
-    wdt_enable(WDTO_120MS); // Trigger Watchdog after 120ms
24
+    wdt_enable(WDTO_250MS); // Trigger Watchdog after 250ms
25
 
25
 
26
     spiInit();
26
     spiInit();
27
     rxInit();
27
     rxInit();
28
 
28
 
29
-    for(;;) { }
29
+    for(;;) {
30
+        wdt_reset();
31
+        rxReceivePacket();
32
+    }
30
 }
33
 }
31
 
34
 

+ 145
- 152
src/rx.c View File

6
 #include <avr/io.h>
6
 #include <avr/io.h>
7
 #include <avr/interrupt.h>
7
 #include <avr/interrupt.h>
8
 #include <avr/eeprom.h>
8
 #include <avr/eeprom.h>
9
+#include <avr/wdt.h>
9
 
10
 
10
 #include "cc2500.h"
11
 #include "cc2500.h"
12
+#include "cppm.h"
11
 #include "spi.h"
13
 #include "spi.h"
12
 #include "timer.h"
14
 #include "timer.h"
13
-
14
-// ----------------------------------------------------------------------------
15
-
16
-#define TRUE 1
17
-#define FALSE 0
15
+#include "rx.h"
18
 
16
 
19
 #define CHANNELS 8
17
 #define CHANNELS 8
20
 #define PPM_MIN 1000
18
 #define PPM_MIN 1000
21
 #define PPM_MAX 2000
19
 #define PPM_MAX 2000
22
-#define RSSI_OVER_PPM 7
23
 #define HOP_DATA_LENGTH 60
20
 #define HOP_DATA_LENGTH 60
24
 #define EEPROM_BASE_ADDRESS 100
21
 #define EEPROM_BASE_ADDRESS 100
25
 #define MISSING_PACKET_DELAY 9
22
 #define MISSING_PACKET_DELAY 9
26
 #define SEEK_CHANNEL_SKIP   13
23
 #define SEEK_CHANNEL_SKIP   13
27
 #define MAX_MISSING_PACKET 20
24
 #define MAX_MISSING_PACKET 20
28
 #define FAILSAFE_MISSING_PACKET 170
25
 #define FAILSAFE_MISSING_PACKET 170
29
-
30
-static uint8_t ccData[27];
31
-static uint8_t ccLen;
32
-static uint8_t packet = FALSE;
33
-static uint8_t channr = 0;
34
-static uint8_t missingPackets = 0;
26
+#define RSSI_OVER_PPM 7
27
+#define RSSI_OFFSET 71
28
+#define RSSI_MIN -103
29
+#define RSSI_MAX -96
30
+#define PPM_SCALE 0.67
31
+
32
+uint8_t ccData[27];
33
+uint8_t ccLen;
34
+uint8_t packet = 0;
35
+uint8_t channr = 0;
36
+uint8_t missingPackets = 0;
35
 uint8_t hopData[HOP_DATA_LENGTH];
37
 uint8_t hopData[HOP_DATA_LENGTH];
36
 uint8_t listLength;
38
 uint8_t listLength;
37
 uint8_t txid[2];
39
 uint8_t txid[2];
38
-static uint8_t counter = 0;
39
-volatile uint8_t failed = FALSE;
40
-int count = 0;
40
+uint8_t counter = 0;
41
+uint8_t failed = 0;
42
+uint8_t frequencyOffsetHack = 0;
41
 uint16_t c[8];
43
 uint16_t c[8];
42
-
43
 int rssi;
44
 int rssi;
44
-const int rssi_offset = 71;
45
-const int rssi_min = -103;
46
-const int rssi_max = -96;
47
-
48
-void getBind(void);
49
-void loop(void);
50
-void tuning(void);
51
-
52
-// ----------------------------------------------------------------------------
53
 
45
 
54
 static uint16_t ppmBuffer[CHANNELS];
46
 static uint16_t ppmBuffer[CHANNELS];
55
 
47
 
56
 static long map(long x, long in_min, long in_max, long out_min, long out_max);
48
 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);
49
 static long constrain(long x, long min, long max);
50
+
58
 static void initialize(uint8_t bind);
51
 static void initialize(uint8_t bind);
59
 static void binding(void);
52
 static void binding(void);
60
-
53
+static void tuning(void);
54
+static void performBind(void);
61
 static void nextChannel(uint8_t skip);
55
 static void nextChannel(uint8_t skip);
62
 static void readBindingData(void);
56
 static void readBindingData(void);
63
 static void writeBindingData(void);
57
 static void writeBindingData(void);
72
     cc2500Strobe(CC2500_SRX);
66
     cc2500Strobe(CC2500_SRX);
73
 }
67
 }
74
 
68
 
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
-}
88
-
89
 static void initialize(uint8_t bind) {
69
 static void initialize(uint8_t bind) {
90
     cc2500ResetChip();
70
     cc2500ResetChip();
91
     cc2500WriteReg(CC2500_02_IOCFG0,   0x01); // RX complete interrupt(GDO0)
71
     cc2500WriteReg(CC2500_02_IOCFG0,   0x01); // RX complete interrupt(GDO0)
130
     cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);
110
     cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);
131
 
111
 
132
     //cc2500WriteReg(CC2500_0C_FSCTRL0, 0); // Frequency offset
112
     //cc2500WriteReg(CC2500_0C_FSCTRL0, 0); // Frequency offset
133
-    cc2500WriteReg(CC2500_0C_FSCTRL0, bind ? 0x00 : count); // Frequency offset hack
113
+    cc2500WriteReg(CC2500_0C_FSCTRL0, bind ? 0x00 : frequencyOffsetHack);
134
 
114
 
135
     cc2500WriteReg(CC2500_0A_CHANNR, 0x00);
115
     cc2500WriteReg(CC2500_0A_CHANNR, 0x00);
136
 }
116
 }
137
 
117
 
138
 static void binding(void) {
118
 static void binding(void) {
139
-    uint8_t jumper2 = 0; //bind_jumper();
140
-
141
-    while (1) {
142
-        if (jumper2 == 0) {
143
-            // bind complete or no bind
144
-            readBindingData();
145
-            if ((txid[0] == 0xff) && (txid[1] == 0xff)) {
146
-                // No valid txid, forcing bind
147
-                jumper2 = 1;
148
-                continue;
149
-            }
150
-            break;
151
-        } else {
152
-            tuning();
153
-            //count=0xC8;//for test
154
-            cc2500WriteReg(CC2500_0C_FSCTRL0, count);
155
-            eeprom_write_byte(EEPROM_BASE_ADDRESS + 101, count);
156
-            getBind();
157
-            // TODO reset?!
158
-            while (1) { }
159
-        }
119
+    readBindingData();
120
+    if ((txid[0] != 0xff) || (txid[1] != 0xff)) {
121
+        // valid binding data found
122
+        return;
160
     }
123
     }
161
-}
162
 
124
 
163
-static void nextChannel(uint8_t skip) {
164
-    channr += skip;
165
-    if (channr >= listLength) {
166
-        channr -= listLength;
167
-    }
125
+    // No valid txid, forcing bind
126
+    tuning();
168
 
127
 
169
-    cc2500WriteReg(CC2500_0A_CHANNR, hopData[channr]);
170
-    cc2500WriteReg(CC2500_23_FSCAL3, 0x89);
171
-}
128
+    //frequencyOffsetHack = 0xC8; // for test
129
+    cc2500WriteReg(CC2500_0C_FSCTRL0, frequencyOffsetHack);
130
+    eeprom_write_byte(EEPROM_BASE_ADDRESS + 101, frequencyOffsetHack);
172
 
131
 
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);
132
+    performBind();
182
 }
133
 }
183
 
134
 
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]);
135
+static void tuning() {
136
+    cc2500Strobe(CC2500_SRX); // enter in rx mode
137
+
138
+    int frequencyOffsetTimer = 0;
139
+    while (1) {
140
+        frequencyOffsetTimer++;
141
+        if (frequencyOffsetTimer > 3000) {
142
+            frequencyOffsetTimer = 0;
143
+            cc2500WriteReg(CC2500_0C_FSCTRL0, frequencyOffsetHack);
144
+            frequencyOffsetHack += 10;
145
+            if (frequencyOffsetHack > 250) {
146
+                frequencyOffsetHack = 0;
147
+            }
148
+            //cc2500Strobe(CC2500_SRX); // enter in rx mode
149
+        }
150
+
151
+        if (GDO_1) {
152
+            ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
153
+            if (ccLen) {
154
+                cc2500ReadFifo(ccData, ccLen);
155
+                if ((ccData[ccLen - 1] & 0x80)
156
+                        && (ccData[2] == 0x01)
157
+                        && (ccData[5] == 0x00)) {
158
+                    break;
159
+                }
160
+            }
161
+        }
162
+
163
+        wdt_reset();
190
     }
164
     }
191
-    eeprom_write_byte(EEPROM_BASE_ADDRESS + 100, listLength);
192
 }
165
 }
193
 
166
 
194
-// ----------------------------------------------------------------------------
195
-
196
-// Receives complete bind setup
197
-void getBind(void) {
167
+static void performBind(void) {
198
     cc2500Strobe(CC2500_SRX); // enter in rx mode
168
     cc2500Strobe(CC2500_SRX); // enter in rx mode
199
 
169
 
200
     while (1) {
170
     while (1) {
201
         if (GDO_1) {
171
         if (GDO_1) {
202
             ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
172
             ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
203
             if (ccLen) {
173
             if (ccLen) {
204
-                cc2500ReadFifo((uint8_t *)ccData, ccLen);
174
+                cc2500ReadFifo(ccData, ccLen);
205
                 if ((ccData[ccLen - 1] & 0x80)
175
                 if ((ccData[ccLen - 1] & 0x80)
206
                         && (ccData[2] == 0x01)
176
                         && (ccData[2] == 0x01)
207
                         && (ccData[5] == 0x00)) {
177
                         && (ccData[5] == 0x00)) {
214
                 }
184
                 }
215
             }
185
             }
216
         }
186
         }
187
+
188
+        wdt_reset();
217
     }
189
     }
218
 
190
 
219
     listLength = 0;
191
     listLength = 0;
220
-    uint8_t eol = FALSE;
192
+    uint8_t eol = 0;
221
     for (uint8_t bindIdx = 0x05; bindIdx <= 120; bindIdx += 5) {
193
     for (uint8_t bindIdx = 0x05; bindIdx <= 120; bindIdx += 5) {
222
         while (1) {
194
         while (1) {
223
             if (GDO_1) {
195
             if (GDO_1) {
224
                 ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
196
                 ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
225
                 if (ccLen) {
197
                 if (ccLen) {
226
-                    cc2500ReadFifo((uint8_t *)ccData, ccLen);
198
+                    cc2500ReadFifo(ccData, ccLen);
227
                     if ((ccData[ccLen - 1] & 0x80)
199
                     if ((ccData[ccLen - 1] & 0x80)
228
                             && (ccData[2] == 0x01)
200
                             && (ccData[2] == 0x01)
229
                             && (ccData[3] == txid[0])
201
                             && (ccData[3] == txid[0])
232
                         for (uint8_t n = 0; n < 5; n++) {
204
                         for (uint8_t n = 0; n < 5; n++) {
233
                             //if (ccData[6 + n] == ccData[ccLen - 3]) {
205
                             //if (ccData[6 + n] == ccData[ccLen - 3]) {
234
                               if (ccData[6 + n] <= 3) {
206
                               if (ccData[6 + n] <= 3) {
235
-                                eol = TRUE;
207
+                                eol = 1;
236
                                 listLength = ccData[5] + n;
208
                                 listLength = ccData[5] + n;
237
                                 break;
209
                                 break;
238
                             }
210
                             }
242
                     }
214
                     }
243
                 }
215
                 }
244
             }
216
             }
217
+
218
+            wdt_reset();
245
         }
219
         }
246
 
220
 
247
         if (eol) {
221
         if (eol) {
253
     cc2500Strobe(CC2500_SIDLE); // Back to idle
227
     cc2500Strobe(CC2500_SIDLE); // Back to idle
254
 }
228
 }
255
 
229
 
256
-void loop() {
230
+static void nextChannel(uint8_t skip) {
231
+    channr += skip;
232
+    if (channr >= listLength) {
233
+        channr -= listLength;
234
+    }
235
+
236
+    cc2500WriteReg(CC2500_0A_CHANNR, hopData[channr]);
237
+    cc2500WriteReg(CC2500_23_FSCAL3, 0x89);
238
+}
239
+
240
+static void readBindingData() {
241
+    for (uint8_t i = 0; i < 2; i++) {
242
+        txid[i] = eeprom_read_byte(EEPROM_BASE_ADDRESS + i);
243
+    }
244
+
245
+    for (uint8_t i = 0; i < HOP_DATA_LENGTH; i++) {
246
+        hopData[i] = eeprom_read_byte(EEPROM_BASE_ADDRESS + 10 + i);
247
+    }
248
+
249
+    listLength = eeprom_read_byte(EEPROM_BASE_ADDRESS + 100);
250
+    frequencyOffsetHack = eeprom_read_byte(EEPROM_BASE_ADDRESS + 101);
251
+}
252
+
253
+static void writeBindingData() {
254
+    for (uint8_t i = 0; i < 2; i++) {
255
+        eeprom_write_byte(EEPROM_BASE_ADDRESS + i, txid[i]);
256
+    }
257
+
258
+    for (uint8_t i = 0; i < HOP_DATA_LENGTH; i++) {
259
+        eeprom_write_byte(EEPROM_BASE_ADDRESS + 10 + i, hopData[i]);
260
+    }
261
+
262
+    eeprom_write_byte(EEPROM_BASE_ADDRESS + 100, listLength);
263
+}
264
+
265
+void rxReceivePacket() {
257
     time_t time = timerGet();
266
     time_t time = timerGet();
258
 
267
 
259
     if (missingPackets > FAILSAFE_MISSING_PACKET) {
268
     if (missingPackets > FAILSAFE_MISSING_PACKET) {
260
-        failed = TRUE;
269
+        failed = 1;
261
         missingPackets = 0;
270
         missingPackets = 0;
262
     }
271
     }
263
 
272
 
275
                 nextChannel(1);
284
                 nextChannel(1);
276
             break;
285
             break;
277
         }
286
         }
287
+
278
         if (GDO_1) {
288
         if (GDO_1) {
279
             ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
289
             ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
280
-            if (ccLen > 20)
290
+            if (ccLen > 20) {
281
                 ccLen = 20;
291
                 ccLen = 20;
292
+            }
282
             if (ccLen) {
293
             if (ccLen) {
283
                 cc2500ReadFifo((uint8_t *)ccData, ccLen);
294
                 cc2500ReadFifo((uint8_t *)ccData, ccLen);
284
                 if (ccData[ccLen - 1] & 0x80) { // Only if correct CRC
295
                 if (ccData[ccLen - 1] & 0x80) { // Only if correct CRC
285
                     missingPackets = 0;
296
                     missingPackets = 0;
286
-                    if (ccData[0] == 0x11) { // Correct length
287
-                        if ((ccData[1] == txid[0]) && (ccData[2] == txid[1])) { // Only if correct txid
288
-                            packet = TRUE;
289
-                            //sei();
297
+                    if ((ccData[0] == 0x11) // Correct length
298
+                            && (ccData[1] == txid[0]) // Correct txid
299
+                            && (ccData[2] == txid[1])) {
300
+                        packet = 1;
290
 
301
 
291
 #ifdef RSSI_OVER_PPM
302
 #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);
303
+                        int rssi_dec = cc2500ReadReg(CC2500_34_RSSI | CC2500_READ_BURST);
304
+                        if (rssi_dec < 128) {
305
+                            rssi = ((rssi_dec / 2) - RSSI_OFFSET) & 0x7f;
306
+                        } else {
307
+                            rssi = (((rssi_dec - 256) / 2)) - RSSI_OFFSET;
308
+                        }
309
+                        rssi = constrain(rssi, RSSI_MIN, RSSI_MAX);
299
 #endif
310
 #endif
300
 
311
 
301
-                            cc2500Strobe(CC2500_SIDLE);
302
-                            nextChannel(1);
303
-                            failed = FALSE;
304
-                            break;
305
-                        }
312
+                        cc2500Strobe(CC2500_SIDLE);
313
+                        nextChannel(1);
314
+                        failed = 0;
315
+                        break;
306
                     }
316
                     }
307
                 }
317
                 }
308
             }
318
             }
309
         }
319
         }
320
+
321
+        wdt_reset();
310
     }
322
     }
311
 
323
 
312
-    if (packet == TRUE) {
313
-        packet = FALSE;
314
-        //cli();
324
+    if (packet != 0) {
325
+        packet = 0;
315
         c[0] = (uint16_t)(ccData[10] & 0x0F) << 8 | ccData[6];
326
         c[0] = (uint16_t)(ccData[10] & 0x0F) << 8 | ccData[6];
316
         c[1] = (uint16_t)(ccData[10] & 0xF0) << 4 | ccData[7];
327
         c[1] = (uint16_t)(ccData[10] & 0xF0) << 4 | ccData[7];
317
         c[2] = (uint16_t)(ccData[11] & 0x0F) << 8 | ccData[8];
328
         c[2] = (uint16_t)(ccData[11] & 0x0F) << 8 | ccData[8];
320
         c[5] = (uint16_t)(ccData[16] & 0xF0) << 4 | ccData[13];
331
         c[5] = (uint16_t)(ccData[16] & 0xF0) << 4 | ccData[13];
321
         c[6] = (uint16_t)(ccData[17] & 0x0F) << 8 | ccData[14];
332
         c[6] = (uint16_t)(ccData[17] & 0x0F) << 8 | ccData[14];
322
         c[7] = (uint16_t)(ccData[17] & 0xF0) << 4 | ccData[15];
333
         c[7] = (uint16_t)(ccData[17] & 0xF0) << 4 | ccData[15];
323
-        //sei();
324
 
334
 
325
         for (int i = 0; i < CHANNELS; i++) {
335
         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;
336
+            ppmBuffer[i] = PPM_SCALE * c[i];
337
+            if ((ppmBuffer[i] < 900) || (ppmBuffer[i] > 2100)) {
338
+                ppmBuffer[i] = 850;
331
             }
339
             }
332
-            */
333
         }
340
         }
334
 
341
 
335
 #ifdef RSSI_OVER_PPM
342
 #ifdef RSSI_OVER_PPM
336
-        ppmBuffer[RSSI_OVER_PPM] = map(rssi, rssi_min, rssi_max, PPM_MIN, PPM_MAX);
343
+        ppmBuffer[RSSI_OVER_PPM] = map(rssi, RSSI_MIN, RSSI_MAX, PPM_MIN, PPM_MAX);
337
 #endif
344
 #endif
345
+
346
+        cppmCopy(ppmBuffer);
338
     }
347
     }
339
 
348
 
340
     cc2500Strobe(CC2500_SRX);
349
     cc2500Strobe(CC2500_SRX);
341
 }
350
 }
342
 
351
 
343
-void tuning() {
344
-    cc2500Strobe(CC2500_SRX); // enter in rx mode
345
-    int count1 = 0;
346
-    while (1) {
347
-        count1++;
348
-        if (count >= 250) {
349
-            count = 0;
350
-        }
351
-        if (count1 > 3000) {
352
-            count1 = 0;
353
-            cc2500WriteReg(CC2500_0C_FSCTRL0, count);  // Frequency offset hack
354
-            count = count + 10;
355
-            //cc2500Strobe(CC2500_SRX);// enter in rx mode
356
-        }
357
-        if (GDO_1) {
358
-            ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
359
-            if (ccLen) {
360
-                cc2500ReadFifo((uint8_t *)ccData, ccLen);
361
-                if (ccData[ccLen - 1] & 0x80) {
362
-                    if (ccData[2] == 0x01) {
363
-                        if (ccData[5] == 0x00) {
364
-                            break;
365
-                        }
366
-                    }
367
-                }
368
-            }
369
-        }
352
+static long map(long x, long in_min, long in_max, long out_min, long out_max) {
353
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
354
+}
355
+
356
+static long constrain(long x, long min, long max) {
357
+    if (x < min) {
358
+        x = min;
370
     }
359
     }
360
+    if (x > max) {
361
+        x = max;
362
+    }
363
+    return x;
371
 }
364
 }
372
 
365
 

Loading…
Cancel
Save