| blob | a34485917039e1d464cb4ce0c9d7d954ab35bf9e |
1 /*
2 This project is free software: you can redistribute it and/or modify
3 it under the terms of the GNU General Public License as published by
4 the Free Software Foundation, either version 3 of the License, or
5 (at your option) any later version.
7 Multiprotocol is distributed in the hope that it will be useful,
8 but WITHOUT ANY WARRANTY; without even the implied warranty of
9 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 GNU General Public License for more details.
12 You should have received a copy of the GNU General Public License
13 along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
14 */
15 //**************************
16 // Telemetry serial code *
17 //**************************
18 #if defined TELEMETRY
20 uint8_t RetrySequence ;
22 #ifdef MULTI_TELEMETRY
23 uint32_t lastMulti = 0;
24 #define MULTI_TIME 500 //in ms
25 #ifdef MULTI_SYNC
26 #define INPUT_SYNC_TIME 100 //in ms
27 #define INPUT_ADDITIONAL_DELAY 100 // in 10µs, 100 => 1000 µs
28 uint32_t lastInputSync = 0;
29 uint16_t inputDelay = 0;
30 #endif // MULTI_SYNC
31 #endif // MULTI_TELEMETRY
33 #if defined SPORT_TELEMETRY
34 #define FRSKY_SPORT_PACKET_SIZE 8
35 #define FX_BUFFERS 4
36 uint8_t Sport_Data = 0;
37 uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS];
39 // Store for out of sequence packet
40 uint8_t FrSkyX_RX_ValidSeq ;
41 struct t_FrSkyX_RX_Frame
42 {
43 boolean valid;
44 uint8_t count;
45 uint8_t payload[6];
46 } ;
48 // Store for FrskyX telemetry
49 struct t_FrSkyX_RX_Frame FrSkyX_RX_Frames[4] ;
50 uint8_t FrSkyX_RX_NextFrame=0;
51 #endif // SPORT_TELEMETRY
53 #if defined HUB_TELEMETRY
54 #define USER_MAX_BYTES 6
55 uint8_t prev_index;
57 struct t_FrSkyD_User_Frame
58 {
59 uint8_t ID;
60 uint8_t low;
61 uint8_t high;
62 } FrSkyD_User_Frame[8];
63 uint8_t FrSkyD_User_Frame_Start=0, FrSkyD_User_Frame_End=0;
64 #endif // HUB_TELEMETRY
66 #define START_STOP 0x7e
67 #define BYTESTUFF 0x7d
68 #define STUFF_MASK 0x20
69 #define MAX_PKTX 10
70 uint8_t pktx[MAX_PKTX];
71 uint8_t frame[18];
73 #ifdef MULTI_TELEMETRY
74 static void multi_send_header(uint8_t type, uint8_t len)
75 {
76 Serial_write('M');
77 Serial_write('P');
78 Serial_write(type);
79 Serial_write(len);
80 }
82 #ifdef MULTI_SYNC
83 static void telemetry_set_input_sync(uint16_t refreshRate)
84 {
85 #if defined(STM32_BOARD) && defined(DEBUG_PIN)
86 static uint8_t c=0;
87 if (c++%2==0)
88 { DEBUG_PIN_on; }
89 else
90 { DEBUG_PIN_off; }
91 #endif
92 // Only record input Delay after a frame has really been received
93 // Otherwise protocols with faster refresh rates then the TX sends (e.g. 3ms vs 6ms) will screw up the calcualtion
94 inputRefreshRate = refreshRate;
95 if (last_serial_input != 0)
96 {
97 cli(); // Disable global int due to RW of 16 bits registers
98 inputDelay = TCNT1;
99 sei(); // Enable global int
100 //inputDelay = (inputDelay - last_serial_input)>>1;
101 inputDelay -= last_serial_input;
102 //if(inputDelay & 0x8000)
103 // inputDelay = inputDelay - 0x8000;
104 last_serial_input=0;
105 }
106 }
107 #endif
109 #ifdef MULTI_SYNC
110 static void mult_send_inputsync()
111 {
112 multi_send_header(MULTI_TELEMETRY_SYNC, 6);
113 Serial_write(inputRefreshRate >> 8);
114 Serial_write(inputRefreshRate & 0xff);
115 // Serial_write(inputDelay >> 8);
116 // Serial_write(inputDelay & 0xff);
117 Serial_write(inputDelay >> 9);
118 Serial_write(inputDelay >> 1);
119 Serial_write(INPUT_SYNC_TIME);
120 Serial_write(INPUT_ADDITIONAL_DELAY);
121 }
122 #endif //MULTI_SYNC
124 static void multi_send_status()
125 {
126 if(protocol == 0) return;
128 multi_send_header(MULTI_TELEMETRY_STATUS, 24);
130 // Build flags
131 uint8_t flags=0;
132 if(IS_INPUT_SIGNAL_on)
133 flags |= 0x01;
134 if(mode_select==MODE_SERIAL)
135 flags |= 0x02;
136 if(remote_callback != 0)
137 {
138 if(multi_protocols_index != 0xFF && IS_SUB_PROTO_VALID)
139 flags |= 0x04; //Invalid protocol / sub protocol, can't make the distinction since there is no more flags...
140 if(IS_WAIT_BIND_on)
141 flags |= 0x10;
142 else
143 if(IS_BIND_IN_PROGRESS)
144 flags |= 0x08;
145 if(multi_protocols_index != 0xFF)
146 {
147 if(multi_protocols[multi_protocols_index].chMap)
148 flags |= 0x40; //Disable_ch_mapping supported
149 #ifdef FAILSAFE_ENABLE
150 if(multi_protocols[multi_protocols_index].failSafe)
151 flags |= 0x20; //Failsafe supported
152 #endif
153 }
154 if(IS_DATA_BUFFER_LOW_on)
155 flags |= 0x80;
156 }
157 Serial_write(flags);
159 // Version number example: 1.1.6.1
160 Serial_write(VERSION_MAJOR);
161 Serial_write(VERSION_MINOR);
162 Serial_write(VERSION_REVISION);
163 Serial_write(VERSION_PATCH_LEVEL);
165 // Channel order
166 Serial_write(RUDDER<<6|THROTTLE<<4|ELEVATOR<<2|AILERON);
168 if(multi_protocols_index == 0xFF) // selection out of list... send first available protocol
169 {
170 Serial_write(multi_protocols[0].protocol); // begining of list
171 Serial_write(multi_protocols[0].protocol); // begining of list
172 for(uint8_t i=0;i<16;i++)
173 Serial_write(0x00); // everything else is invalid
174 }
175 else
176 {
177 // Protocol next/prev
178 if(multi_protocols[multi_protocols_index+1].protocol != 0xFF)
179 {
180 if(multi_protocols[multi_protocols_index+1].protocol == PROTO_SCANNER )//|| multi_protocols[multi_protocols_index+1].protocol == PROTO_CONFIG )
181 {// if next is scanner
182 if(multi_protocols[multi_protocols_index+2].protocol != 0xFF)
183 Serial_write(multi_protocols[multi_protocols_index+2].protocol); // skip to next protocol number
184 else
185 Serial_write(multi_protocols[multi_protocols_index].protocol); // or end of list
186 }
187 else
188 Serial_write(multi_protocols[multi_protocols_index+1].protocol); // next protocol number
189 }
190 else
191 Serial_write(multi_protocols[multi_protocols_index].protocol); // end of list
192 if(multi_protocols_index>0 && multi_protocols[multi_protocols_index-1].protocol != 0)
193 {
194 if(multi_protocols[multi_protocols_index-1].protocol == PROTO_SCANNER )//|| multi_protocols[multi_protocols_index-1].protocol == PROTO_CONFIG )
195 {// if prev is scanner
196 if(multi_protocols_index > 1)
197 Serial_write(multi_protocols[multi_protocols_index-2].protocol); // skip to prev protocol number
198 else
199 Serial_write(multi_protocols[multi_protocols_index].protocol); // begining of list
200 }
201 else
202 Serial_write(multi_protocols[multi_protocols_index-1].protocol); // prev protocol number
203 }
204 else
205 Serial_write(multi_protocols[multi_protocols_index].protocol); // begining of list
206 // Protocol
207 for(uint8_t i=0;i<7;i++)
208 Serial_write(multi_protocols[multi_protocols_index].ProtoString[i]); // protocol name
209 // Sub-protocol
210 uint8_t nbr=multi_protocols[multi_protocols_index].nbrSubProto;
211 if(option_override>0x0F)
212 Serial_write(nbr | (multi_protocols[multi_protocols_index].optionType<<4)); // number of sub protocols && option type
213 else
214 Serial_write(nbr | (option_override<<4)); // number of sub protocols && option_override type
215 uint8_t j=0;
216 if(IS_SUB_PROTO_VALID)
217 {
218 uint8_t len=multi_protocols[multi_protocols_index].SubProtoString[0];
219 uint8_t offset=len*(sub_protocol&0x07)+1;
220 for(;j<len;j++)
221 Serial_write(multi_protocols[multi_protocols_index].SubProtoString[j+offset]); // current sub protocol name
222 }
223 for(;j<8;j++)
224 Serial_write(0x00);
225 }
226 }
228 #ifdef MULTI_CONFIG_INO
229 void CONFIG_frame()
230 {
231 multi_send_header(MULTI_TELEMETRY_CONFIG, 21);
232 for (uint8_t i = 0; i < 21; i++) // Config data
233 Serial_write(packet_in[i]);
234 }
235 #endif
237 #ifdef MLINK_FW_TELEMETRY
238 void MLINK_frame()
239 {
240 multi_send_header(MULTI_TELEMETRY_MLINK, 10);
241 Serial_write(TX_RSSI); // RSSI
242 Serial_write(TX_LQI); // LQI
243 for (uint8_t i = 0; i < 8; i++) // followed by 8 bytes of telemetry data
244 Serial_write(packet_in[i]);
245 }
246 #endif
248 #ifdef DSM_TELEMETRY
249 void DSM_frame()
250 {
251 if (packet_in[0] == 0x80)
252 {
253 multi_send_header(MULTI_TELEMETRY_DSMBIND, 10);
254 for (uint8_t i = 1; i < 11; i++) // 10 bytes of DSM bind response
255 Serial_write(packet_in[i]);
257 }
258 else
259 {
260 multi_send_header(MULTI_TELEMETRY_DSM, 17);
261 for (uint8_t i = 0; i < 17; i++) // RSSI value followed by 16 bytes of telemetry data
262 Serial_write(packet_in[i]);
263 }
264 }
265 #endif
267 #ifdef SCANNER_TELEMETRY
268 void spectrum_scanner_frame()
269 {
270 multi_send_header(MULTI_TELEMETRY_SCANNER, SCAN_CHANS_PER_PACKET + 1);
271 Serial_write(packet_in[0]); // start channel
272 for(uint8_t ch = 0; ch < SCAN_CHANS_PER_PACKET; ch++)
273 Serial_write(packet_in[ch+1]); // RSSI power levels
274 }
275 #endif
277 #if defined (FRSKY_RX_TELEMETRY) || defined (AFHDS2A_RX_TELEMETRY) || defined (BAYANG_RX_TELEMETRY) || defined (DSM_RX_CYRF6936_INO)
278 void receiver_channels_frame()
279 {
280 uint16_t len = packet_in[3] * 11; // 11 bit per channel
281 if (len % 8 == 0)
282 len = 4 + (len / 8);
283 else
284 len = 5 + (len / 8);
285 multi_send_header(MULTI_TELEMETRY_RX_CHANNELS, len);
286 for (uint8_t i = 0; i < len; i++)
287 Serial_write(packet_in[i]); // pps, rssi, ch start, ch count, 16x ch data
288 }
289 #endif
291 #ifdef AFHDS2A_FW_TELEMETRY
292 void AFHDSA_short_frame()
293 {
294 multi_send_header(packet_in[29]==0xAA?MULTI_TELEMETRY_AFHDS2A:MULTI_TELEMETRY_AFHDS2A_AC, 29);
295 for (uint8_t i = 0; i < 29; i++) // RSSI value followed by 4*7 bytes of telemetry data
296 Serial_write(packet_in[i]);
297 }
298 #endif
300 #ifdef HITEC_FW_TELEMETRY
301 void HITEC_short_frame()
302 {
303 multi_send_header(MULTI_TELEMETRY_HITEC, 8);
304 for (uint8_t i = 0; i < 8; i++) // TX RSSI and TX LQI values followed by frame number and 5 bytes of telemetry data
305 Serial_write(packet_in[i]);
306 }
307 #endif
309 #ifdef HOTT_FW_TELEMETRY
310 void HOTT_short_frame()
311 {
312 multi_send_header(MULTI_TELEMETRY_HOTT, 15);
313 for (uint8_t i = 0; i < 15; i++) // TX RSSI and TX LQI values followed by frame number and telemetry data
314 Serial_write(packet_in[i]);
315 }
316 #endif
318 static void multi_send_frskyhub()
319 {
320 multi_send_header(MULTI_TELEMETRY_HUB, 9);
321 for (uint8_t i = 0; i < 9; i++)
322 Serial_write(frame[i]);
323 }
325 #endif //MULTI_TELEMETRY
327 void frskySendStuffed()
328 {
329 Serial_write(START_STOP);
330 for (uint8_t i = 0; i < 9; i++)
331 {
332 if ((frame[i] == START_STOP) || (frame[i] == BYTESTUFF))
333 {
334 Serial_write(BYTESTUFF);
335 frame[i] ^= STUFF_MASK;
336 }
337 Serial_write(frame[i]);
338 }
339 Serial_write(START_STOP);
340 }
342 bool frsky_process_telemetry(uint8_t *buffer,uint8_t len)
343 {
344 if(protocol!=PROTO_FRSKY_R9)
345 {
346 if(buffer[1] != rx_tx_addr[3] || buffer[2] != rx_tx_addr[2] || len != buffer[0] + 3 )
347 return false; // Bad address or length...
348 // RSSI and LQI are the 2 last bytes
349 TX_RSSI = buffer[len-2];
350 if(TX_RSSI >=128)
351 TX_RSSI -= 128;
352 else
353 TX_RSSI += 128;
354 }
355 telemetry_link|=1; // Telemetry data is available
357 #if defined FRSKYD_CC2500_INO
358 if (protocol==PROTO_FRSKYD)
359 {
360 TX_LQI = buffer[len-1]&0x7F;
361 //Save current buffer
362 for (uint8_t i=3;i<len-2;i++)
363 telemetry_in_buffer[i]=buffer[i]; // Buffer telemetry values to be sent
365 //Check incoming telemetry sequence
366 if(telemetry_in_buffer[6]>0 && telemetry_in_buffer[6]<=10)
367 { //Telemetry length ok
368 if ( ( telemetry_in_buffer[7] & 0x1F ) == (telemetry_counter & 0x1F) )
369 {//Sequence is ok
370 uint8_t topBit = 0 ;
371 if ( telemetry_counter & 0x80 )
372 if ( ( telemetry_counter & 0x1F ) != RetrySequence )
373 topBit = 0x80 ;
374 telemetry_counter = ( (telemetry_counter+1)%32 ) | topBit ; // Request next telemetry frame
375 }
376 else
377 {//Incorrect sequence
378 RetrySequence = telemetry_in_buffer[7] & 0x1F ;
379 telemetry_counter |= 0x80 ;
380 telemetry_in_buffer[6]=0 ; // Discard current packet and wait for retransmit
381 }
382 }
383 else
384 telemetry_in_buffer[6]=0; // Discard packet
385 }
386 #endif
388 #if defined SPORT_TELEMETRY && (defined FRSKYX_CC2500_INO || defined FRSKYR9_SX1276_INO)
389 if (protocol==PROTO_FRSKYX||protocol==PROTO_FRSKYX2)
390 {
391 /*Telemetry frames(RF) SPORT info
392 15 bytes payload
393 SPORT frame valid 6+3 bytes
394 [00] PKLEN 0E 0E 0E 0E
395 [01] TXID1 DD DD DD DD
396 [02] TXID2 6D 6D 6D 6D
397 [03] CONST 02 02 02 02
398 [04] RS/RB 2C D0 2C CE //D0;CE=2*RSSI;....2C = RX battery voltage(5V from Bec)
399 [05] HD-SK 03 10 21 32 //TX/RX telemetry hand-shake bytes
400 [06] NO.BT 00 00 06 03 //No.of valid SPORT frame bytes in the frame
401 [07] STRM1 00 00 7E 00
402 [08] STRM2 00 00 1A 00
403 [09] STRM3 00 00 10 00
404 [10] STRM4 03 03 03 03
405 [11] STRM5 F1 F1 F1 F1
406 [12] STRM6 D1 D1 D0 D0
407 [13] CHKSUM1 --|2 CRC bytes sent by RX (calculated on RX side crc16/table)
408 [14] CHKSUM2 --|*/
409 //len=17 -> len-7=10 -> 3..12
410 uint16_t lcrc = FrSkyX_crc(&buffer[3], len-7 ) ;
411 if ( ( (lcrc >> 8) != buffer[len-4]) || ( (lcrc & 0x00FF ) != buffer[len-3]) )
412 return false; // Bad CRC
414 if(buffer[4] & 0x80)
415 RX_RSSI=buffer[4] & 0x7F ;
416 else
417 v_lipo1 = (buffer[4]<<1) + 1 ;
418 #if defined(TELEMETRY_FRSKYX_TO_FRSKYD) && defined(ENABLE_PPM)
419 if(mode_select != MODE_SERIAL)
420 return true;
421 #endif
422 }
423 if (protocol==PROTO_FRSKYX||protocol==PROTO_FRSKYX2||protocol==PROTO_FRSKY_R9)
424 {
425 telemetry_lost=0;
427 //Save outgoing telemetry sequence
428 FrSkyX_TX_IN_Seq=buffer[5] >> 4;
430 //Check incoming telemetry sequence
431 uint8_t packet_seq=buffer[5] & 0x03;
432 if ( buffer[5] & 0x08 )
433 {//Request init
434 FrSkyX_RX_Seq = 0x08 ;
435 FrSkyX_RX_NextFrame = 0x00 ;
436 FrSkyX_RX_Frames[0].valid = false ;
437 FrSkyX_RX_Frames[1].valid = false ;
438 FrSkyX_RX_Frames[2].valid = false ;
439 FrSkyX_RX_Frames[3].valid = false ;
440 }
441 else if ( packet_seq == (FrSkyX_RX_Seq & 0x03 ) )
442 {//In sequence
443 struct t_FrSkyX_RX_Frame *p ;
444 uint8_t count ;
445 // buffer[4] RSSI
446 // buffer[5] sequence control
447 // buffer[6] payload count
448 // buffer[7-12] payload
449 p = &FrSkyX_RX_Frames[packet_seq] ;
450 count = buffer[6]; // Payload length
451 if ( count <= 6 )
452 {//Store payload
453 p->count = count ;
454 for ( uint8_t i = 0 ; i < count ; i++ )
455 p->payload[i] = buffer[i+7] ;
456 }
457 else
458 p->count = 0 ; // Discard
459 p->valid = true ;
461 FrSkyX_RX_Seq = ( FrSkyX_RX_Seq + 1 ) & 0x03 ; // Move to next sequence
463 if ( FrSkyX_RX_ValidSeq & 0x80 )
464 {
465 FrSkyX_RX_Seq = ( FrSkyX_RX_ValidSeq + 1 ) & 3 ;
466 FrSkyX_RX_ValidSeq &= 0x7F ;
467 }
469 }
470 else
471 {//Not in sequence
472 struct t_FrSkyX_RX_Frame *q ;
473 uint8_t count ;
474 // buffer[4] RSSI
475 // buffer[5] sequence control
476 // buffer[6] payload count
477 // buffer[7-12] payload
478 if ( packet_seq == ( ( FrSkyX_RX_Seq +1 ) & 3 ) )
479 {//Received next sequence -> save it
480 q = &FrSkyX_RX_Frames[packet_seq] ;
481 count = buffer[6]; // Payload length
482 if ( count <= 6 )
483 {//Store payload
484 q->count = count ;
485 for ( uint8_t i = 0 ; i < count ; i++ )
486 q->payload[i] = buffer[i+7] ;
487 }
488 else
489 q->count = 0 ;
490 q->valid = true ;
492 FrSkyX_RX_ValidSeq = 0x80 | packet_seq ;
493 }
494 FrSkyX_RX_Seq = ( FrSkyX_RX_Seq & 0x03 ) | 0x04 ; // Request re-transmission of original sequence
495 }
496 }
497 #endif
498 return true;
499 }
501 void init_frskyd_link_telemetry()
502 {
503 telemetry_link=0;
504 telemetry_counter=0;
505 telemetry_lost=1;
506 v_lipo1=0;
507 v_lipo2=0;
508 RX_RSSI=0;
509 TX_RSSI=0;
510 RX_LQI=0;
511 TX_LQI=0;
512 #if defined HUB_TELEMETRY
513 FrSkyD_User_Frame_Start=FrSkyD_User_Frame_End=0;
514 #endif
515 }
517 void frsky_link_frame()
518 {
519 frame[0] = 0xFE; // Link frame
520 if (protocol==PROTO_FRSKYD)
521 {
522 frame[1] = telemetry_in_buffer[3]; // A1
523 frame[2] = telemetry_in_buffer[4]; // A2
524 frame[3] = telemetry_in_buffer[5]; // RX_RSSI
525 telemetry_link &= ~1 ; // Sent
526 telemetry_link |= 2 ; // Send hub if available
527 }
528 else
529 {//PROTO_HUBSAN, PROTO_AFHDS2A, PROTO_BAYANG, PROTO_NCC1701, PROTO_CABELL, PROTO_HITEC, PROTO_BUGS, PROTO_BUGSMINI, PROTO_FRSKYX, PROTO_FRSKYX2, PROTO_PROPEL, PROTO_DEVO, PROTO_RLINK, PROTO_OMP, PROTO_WFLY2, PROTO_LOLI, PROTO_MLINK, PROTO_MT99XX
530 frame[1] = v_lipo1;
531 frame[2] = v_lipo2;
532 frame[3] = RX_RSSI;
533 telemetry_link &= ~1 ; // Sent
534 }
535 frame[4] = TX_RSSI;
536 frame[5] = RX_LQI;
537 frame[6] = TX_LQI;
538 frame[7] = frame[8] = 0;
539 #if defined MULTI_TELEMETRY
540 multi_send_frskyhub();
541 #else
542 frskySendStuffed();
543 #endif
544 }
546 #if defined HUB_TELEMETRY
547 void frsky_user_frame()
548 {
549 if(telemetry_in_buffer[6])
550 {//only send valid hub frames
551 frame[0] = 0xFD; // user frame
552 if(telemetry_in_buffer[6]>USER_MAX_BYTES)
553 {
554 frame[1]=USER_MAX_BYTES; // packet size
555 telemetry_in_buffer[6]-=USER_MAX_BYTES;
556 telemetry_link |= 2 ; // 2 packets need to be sent
557 }
558 else
559 {
560 frame[1]=telemetry_in_buffer[6]; // packet size
561 telemetry_link &= ~2; // only 1 packet or processing second packet
562 }
563 frame[2] = telemetry_in_buffer[7];
564 for(uint8_t i=0;i<USER_MAX_BYTES;i++)
565 frame[i+3]=telemetry_in_buffer[i+8];
566 if(telemetry_link & 2) // prepare the content of second packet
567 for(uint8_t i=8;i<USER_MAX_BYTES+8;i++)
568 telemetry_in_buffer[i]=telemetry_in_buffer[i+USER_MAX_BYTES];
569 #if defined MULTI_TELEMETRY
570 multi_send_frskyhub();
571 #else
572 frskySendStuffed();
573 #endif
574 }
575 else
576 telemetry_link &= ~2;
577 }
578 /*
579 HuB RX packets.
580 packet_in[6]|(counter++)|00 01 02 03 04 05 06 07 08 09
581 %32
582 01 08 5E 28 12 00 5E 5E 3A 06 00 5E
583 0A 09 28 12 00 5E 5E 3A 06 00 5E 5E
584 09 0A 3B 09 00 5E 5E 06 36 7D 5E 5E
585 03 0B 5E 28 11 00 5E 5E 06 06 6C 5E
586 0A 0C 00 5E 5E 3A 06 00 5E 5E 3B 09
587 07 0D 00 5E 5E 06 06 6C 5E 16 72 5E
588 05 0E 5E 28 11 00 5E 5E 3A 06 00 5E
589 0A 0F 5E 3A 06 00 5E 5E 3B 09 00 5E
590 05 10 5E 06 16 72 5E 5E 3A 06 00 5E
591 */
592 static void __attribute__((unused)) frsky_write_user_frame(uint8_t ID, uint8_t low, uint8_t high)
593 {
594 telemetry_in_buffer[6] = 0x04; // number of bytes in the payload
595 telemetry_in_buffer[7] = 0x00; // unknown?
596 telemetry_in_buffer[8] = 0x5E; // start of payload
597 telemetry_in_buffer[9] = ID; // ID must be less than 0x40
598 uint8_t pos=10;
599 uint8_t value = low;
600 for(uint8_t i=0;i<2;i++)
601 {// Byte stuffing
602 if(value == 0x5D || value == 0x5E)
603 {// Byte stuffing
604 telemetry_in_buffer[pos+1] = value ^ 0x60;
605 telemetry_in_buffer[pos] = 0x5D;
606 telemetry_in_buffer[6]++; // 1 more byte in the payload
607 pos += 2;
608 }
609 else
610 telemetry_in_buffer[pos++] = value;
611 value = high;
612 }
613 telemetry_link |= 2; // request to send frame
614 }
616 static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t low, uint8_t high)
617 {
618 if(telemetry_link&2)
619 { // add to buffer
620 uint8_t test = (FrSkyD_User_Frame_End + 1) & 0x07;
621 if(test == FrSkyD_User_Frame_Start)
622 return; // buffer full...
623 FrSkyD_User_Frame_End = test;
624 FrSkyD_User_Frame[FrSkyD_User_Frame_End].ID = ID;
625 FrSkyD_User_Frame[FrSkyD_User_Frame_End].low = low;
626 FrSkyD_User_Frame[FrSkyD_User_Frame_End].high = high;
627 }
628 else // send to TX direct
629 frsky_write_user_frame(ID, low, high);
630 }
632 static void __attribute__((unused)) frsky_check_user_frame()
633 {
634 if((telemetry_link&2) || FrSkyD_User_Frame_Start == FrSkyD_User_Frame_End)
635 return; // need to wait that the last frame is sent or buffer is empty
636 frsky_write_user_frame(FrSkyD_User_Frame[FrSkyD_User_Frame_Start].ID, FrSkyD_User_Frame[FrSkyD_User_Frame_Start].low, FrSkyD_User_Frame[FrSkyD_User_Frame_Start].high);
637 FrSkyD_User_Frame_Start++;
638 FrSkyD_User_Frame_Start &= 0x07;
639 }
640 #endif
643 #if defined SPORT_TELEMETRY
644 /* SPORT details serial
645 100K 8E2 normal-multiprotocol
646 -every 12ms-or multiple of 12; %36
647 1 2 3 4 5 6 7 8 9 CRC DESCR
648 7E 98 10 05 F1 20 23 0F 00 A6 SWR_ID
649 7E 98 10 01 F1 33 00 00 00 C9 RSSI_ID
650 7E 98 10 04 F1 58 00 00 00 A1 BATT_ID
651 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
652 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
653 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
654 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
655 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
656 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
659 Telemetry frames(RF) SPORT info
660 15 bytes payload
661 SPORT frame valid 6+3 bytes
662 [00] PKLEN 0E 0E 0E 0E
663 [01] TXID1 DD DD DD DD
664 [02] TXID2 6D 6D 6D 6D
665 [03] CONST 02 02 02 02
666 [04] RS/RB 2C D0 2C CE //D0;CE=2*RSSI;....2C = RX battery voltage(5V from Bec)
667 [05] HD-SK 03 10 21 32 //TX/RX telemetry hand-shake bytes
668 [06] NO.BT 00 00 06 03 //No.of valid SPORT frame bytes in the frame
669 [07] STRM1 00 00 7E 00
670 [08] STRM2 00 00 1A 00
671 [09] STRM3 00 00 10 00
672 [10] STRM4 03 03 03 03
673 [11] STRM5 F1 F1 F1 F1
674 [12] STRM6 D1 D1 D0 D0
675 [13] CHKSUM1 --|2 CRC bytes sent by RX (calculated on RX side crc16/table)
676 [14] CHKSUM2 --|
677 +2 appended bytes automatically RSSI and LQI/CRC bytes(len=0x0E+3);
679 0x06 0x06 0x06 0x06 0x06
681 0x7E 0x00 0x03 0x7E 0x00
682 0x1A 0x00 0xF1 0x1A 0x00
683 0x10 0x00 0xD7 0x10 0x00
684 0x03 0x7E 0x00 0x03 0x7E
685 0xF1 0x1A 0x00 0xF1 0x1A
686 0xD7 0x10 0x00 0xD7 0x10
688 0xE1 0x1C 0xD0 0xEE 0x33
689 0x34 0x0A 0xC3 0x56 0xF3
690 */
692 #if defined MULTI_TELEMETRY
693 const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45,
694 0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB,
695 0xAC, 0x0D, 0x8E, 0x2F, 0xD0, 0x71,
696 0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7,
697 0x98, 0x39, 0xBA, 0x1B } ;
698 #endif
700 #ifdef MULTI_TELEMETRY
701 void sportSend(uint8_t *p)
702 {
703 multi_send_header(MULTI_TELEMETRY_SPORT, 9);
704 uint16_t crc_s = 0;
705 uint8_t x = p[0] ;
706 if ( x <= 0x1B )
707 x = pgm_read_byte_near( &Indices[x] ) ;
708 Serial_write(x) ;
709 for (uint8_t i = 1; i < 8; i++)
710 {
711 Serial_write(p[i]);
712 crc_s += p[i]; //0-1FF
713 crc_s += crc_s >> 8; //0-100
714 crc_s &= 0x00ff;
715 }
716 Serial_write(0xff - crc_s);
717 }
718 #else
719 void sportSend(uint8_t *p)
720 {
721 uint16_t crc_s = 0;
722 Serial_write(START_STOP);//+9
723 Serial_write(p[0]) ;
724 for (uint8_t i = 1; i < 9; i++)
725 {
726 if (i == 8)
727 p[i] = 0xff - crc_s;
729 if ((p[i] == START_STOP) || (p[i] == BYTESTUFF))
730 {
731 Serial_write(BYTESTUFF);//stuff again
732 Serial_write(STUFF_MASK ^ p[i]);
733 }
734 else
735 Serial_write(p[i]);
737 crc_s += p[i]; //0-1FF
738 crc_s += crc_s >> 8; //0-100
739 crc_s &= 0x00ff;
740 }
741 }
742 #endif
744 void sportIdle()
745 {
746 #if !defined MULTI_TELEMETRY
747 Serial_write(START_STOP);
748 #endif
749 }
751 void sportSendFrame()
752 {
753 static uint8_t sport_counter=0;
754 uint8_t i;
756 sport_counter = (sport_counter + 1) %36;
757 if(telemetry_lost)
758 {
759 sportIdle();
760 return;
761 }
762 if(sport_counter<6)
763 {
764 frame[0] = 0x98;
765 frame[1] = 0x10;
766 for (i=5;i<8;i++)
767 frame[i]=0;
768 }
769 switch (sport_counter)
770 {
771 case 0:
772 frame[2] = 0x05;
773 frame[3] = 0xf1;
774 frame[4] = 0x02; //dummy values if swr 20230f00
775 frame[5] = 0x23;
776 frame[6] = 0x0F;
777 break;
778 case 2: // RSSI
779 frame[2] = 0x01;
780 frame[3] = 0xf1;
781 frame[4] = RX_RSSI;
782 frame[5] = TX_RSSI;
783 frame[6] = RX_LQI;
784 frame[7] = TX_LQI;
785 break;
786 case 4: //BATT
787 frame[2] = 0x04;
788 frame[3] = 0xf1;
789 frame[4] = v_lipo1; //a1;
790 break;
791 default:
792 if(Sport_Data)
793 {
794 for (i=0;i<FRSKY_SPORT_PACKET_SIZE;i++)
795 frame[i]=pktx1[i];
796 Sport_Data -- ;
797 if ( Sport_Data )
798 {
799 uint8_t j = Sport_Data * FRSKY_SPORT_PACKET_SIZE ;
800 for (i=0;i<j;i++)
801 pktx1[i] = pktx1[i+FRSKY_SPORT_PACKET_SIZE] ;
802 }
803 break;
804 }
805 else
806 {
807 sportIdle();
808 return;
809 }
810 }
811 sportSend(frame);
812 }
814 void proces_sport_data(uint8_t data)
815 {
816 static uint8_t pass = 0, indx = 0;
817 switch (pass)
818 {
819 case 0:
820 if (data == START_STOP)
821 {//waiting for 0x7e
822 indx = 0;
823 pass = 1;
824 }
825 break;
826 case 1:
827 if (data == START_STOP) // Happens if missed packet
828 {//waiting for 0x7e
829 indx = 0;
830 pass = 1;
831 break;
832 }
833 if(data == BYTESTUFF) //if they are stuffed
834 pass=2;
835 else
836 if (indx < MAX_PKTX)
837 pktx[indx++] = data;
838 break;
839 case 2:
840 if (indx < MAX_PKTX)
841 pktx[indx++] = data ^ STUFF_MASK; //unstuff bytes
842 pass=1;
843 break;
844 } // end switch
845 if (indx >= FRSKY_SPORT_PACKET_SIZE)
846 {//8 bytes no crc
847 if ( Sport_Data < FX_BUFFERS )
848 {
849 uint8_t dest = Sport_Data * FRSKY_SPORT_PACKET_SIZE ;
850 uint8_t i ;
851 for ( i = 0 ; i < FRSKY_SPORT_PACKET_SIZE ; i++ )
852 pktx1[dest++] = pktx[i] ; // Triple buffer
853 Sport_Data += 1 ;//ok to send
854 }
855 // else
856 // {
857 // // Overrun
858 // }
859 pass = 0;//reset
860 }
861 }
863 #endif
865 void TelemetryUpdate()
866 {
867 // check for space in tx buffer
868 #ifdef BASH_SERIAL
869 uint8_t h ;
870 uint8_t t ;
871 h = SerialControl.head ;
872 t = SerialControl.tail ;
873 if ( h >= t )
874 t += TXBUFFER_SIZE - h ;
875 else
876 t -= h ;
877 if ( t < 64 )
878 {
879 return ;
880 }
881 #else
882 uint8_t h ;
883 uint8_t t ;
884 h = tx_head ;
885 t = tx_tail ;
886 if ( h >= t )
887 t += TXBUFFER_SIZE - h ;
888 else
889 t -= h ;
890 if ( t < 32 )
891 {
892 debugln("TEL_BUF_FULL %d",t);
893 return ;
894 }
895 /* else
896 if(t!=96)
897 debugln("TEL_BUF %d",t);
898 */
899 #endif
900 #ifdef MULTI_TELEMETRY
901 uint32_t now = millis();
902 if ((IS_SEND_MULTI_STATUS_on || ((now - lastMulti) > MULTI_TIME))&& protocol != PROTO_SCANNER)
903 {
904 multi_send_status();
905 SEND_MULTI_STATUS_off;
906 lastMulti = now;
907 return;
908 }
909 #ifdef MULTI_SYNC
910 if ( inputRefreshRate && (now - lastInputSync) > INPUT_SYNC_TIME )
911 {
912 mult_send_inputsync();
913 lastInputSync = now;
914 return;
915 }
916 #endif
917 #endif
918 #if defined SPORT_TELEMETRY
919 if ((protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2||protocol==PROTO_FRSKY_R9) && telemetry_link
920 #ifdef TELEMETRY_FRSKYX_TO_FRSKYD
921 && mode_select==MODE_SERIAL
922 #endif
923 )
924 { // FrSkyX
925 for(;;)
926 { //Empty buffer
927 struct t_FrSkyX_RX_Frame *p ;
928 uint8_t count ;
929 p = &FrSkyX_RX_Frames[FrSkyX_RX_NextFrame] ;
930 if ( p->valid )
931 {
932 count = p->count ;
933 for (uint8_t i=0; i < count ; i++)
934 proces_sport_data(p->payload[i]) ;
935 p->valid = false ; // Sent
936 FrSkyX_RX_NextFrame = ( FrSkyX_RX_NextFrame + 1 ) & 3 ;
937 }
938 else
939 break ;
940 }
941 telemetry_link=0;
942 sportSendFrame();
943 }
944 #endif // SPORT_TELEMETRY
946 #ifdef MULTI_TELEMETRY
947 #if defined MULTI_CONFIG_INO
948 if(telemetry_link && protocol == PROTO_CONFIG)
949 {
950 CONFIG_frame();
951 telemetry_link=0;
952 return;
953 }
954 #endif
955 #if defined MLINK_FW_TELEMETRY
956 if(telemetry_link && protocol == PROTO_MLINK)
957 {
958 MLINK_frame();
959 telemetry_link=0;
960 return;
961 }
962 #endif
963 #if defined DSM_TELEMETRY
964 if(telemetry_link && protocol == PROTO_DSM)
965 { // DSM
966 DSM_frame();
967 telemetry_link=0;
968 return;
969 }
970 #endif
971 #if defined AFHDS2A_FW_TELEMETRY
972 if(telemetry_link == 2 && protocol == PROTO_AFHDS2A)
973 {
974 AFHDSA_short_frame();
975 telemetry_link=0;
976 return;
977 }
978 #endif
979 #if defined HITEC_FW_TELEMETRY
980 if(telemetry_link == 2 && protocol == PROTO_HITEC)
981 {
982 HITEC_short_frame();
983 telemetry_link=0;
984 return;
985 }
986 #endif
987 #if defined HOTT_FW_TELEMETRY
988 if(telemetry_link == 2 && protocol == PROTO_HOTT)
989 {
990 HOTT_short_frame();
991 telemetry_link=0;
992 return;
993 }
994 #endif
995 #if defined SCANNER_TELEMETRY
996 if (telemetry_link && protocol == PROTO_SCANNER)
997 {
998 spectrum_scanner_frame();
999 telemetry_link = 0;
1000 return;
1001 }
1002 #endif
1003 #if defined (FRSKY_RX_TELEMETRY) || defined(AFHDS2A_RX_TELEMETRY) || defined (BAYANG_RX_TELEMETRY) || defined (DSM_RX_CYRF6936_INO)
1004 if ((telemetry_link & 1) && (protocol == PROTO_FRSKY_RX || protocol == PROTO_AFHDS2A_RX || protocol == PROTO_BAYANG_RX || protocol == PROTO_DSM_RX) )
1005 {
1006 receiver_channels_frame();
1007 telemetry_link &= ~1;
1008 return;
1009 }
1010 #endif
1011 #endif //MULTI_TELEMETRY
1013 if( telemetry_link & 1 )
1014 { // FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell + Hitec + Bugs + BugsMini + NCC1701 + PROPEL + RLINK + OMP + MLINK + DEVO
1015 // FrSkyX telemetry if in PPM
1016 frsky_link_frame();
1017 return;
1018 }
1019 #if defined HUB_TELEMETRY
1020 if((telemetry_link & 2) && ( protocol == PROTO_FRSKYD || protocol == PROTO_BAYANG || protocol == PROTO_MLINK || protocol == PROTO_DEVO ) )
1021 { // FrSkyD + Bayang + MLINK + DEVO
1022 frsky_user_frame();
1023 frsky_check_user_frame();
1024 return;
1025 }
1026 #endif
1027 }
1030 /**************************/
1031 /**************************/
1032 /** Serial TX routines **/
1033 /**************************/
1034 /**************************/
1036 #ifndef BASH_SERIAL
1037 // Routines for normal serial output
1038 void Serial_write(uint8_t data)
1039 {
1040 uint8_t nextHead ;
1041 nextHead = tx_head + 1 ;
1042 if ( nextHead >= TXBUFFER_SIZE )
1043 nextHead = 0 ;
1044 tx_buff[nextHead]=data;
1045 tx_head = nextHead ;
1046 tx_resume();
1047 }
1049 void initTXSerial( uint8_t speed)
1050 {
1051 #ifdef ENABLE_PPM
1052 if(speed==SPEED_9600)
1053 { // 9600
1054 #ifdef ORANGE_TX
1055 USARTC0.BAUDCTRLA = 207 ;
1056 USARTC0.BAUDCTRLB = 0 ;
1057 USARTC0.CTRLB = 0x18 ;
1058 USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
1059 USARTC0.CTRLC = 0x03 ;
1060 #else
1061 #ifdef STM32_BOARD
1062 usart3_begin(9600,SERIAL_8N1); //USART3
1063 #else
1064 UBRR0H = 0x00;
1065 UBRR0L = 0x67;
1066 UCSR0A = 0 ; // Clear X2 bit
1067 //Set frame format to 8 data bits, none, 1 stop bit
1068 UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
1069 #endif
1070 #endif
1071 }
1072 else if(speed==SPEED_57600)
1073 { // 57600
1074 #ifdef ORANGE_TX
1075 /*USARTC0.BAUDCTRLA = 207 ;
1076 USARTC0.BAUDCTRLB = 0 ;
1077 USARTC0.CTRLB = 0x18 ;
1078 USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
1079 USARTC0.CTRLC = 0x03 ;*/
1080 #else
1081 #ifdef STM32_BOARD
1082 usart3_begin(57600,SERIAL_8N1); //USART3
1083 #else
1084 UBRR0H = 0x00;
1085 UBRR0L = 0x22;
1086 UCSR0A = 0x02 ; // Set X2 bit
1087 //Set frame format to 8 data bits, none, 1 stop bit
1088 UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
1089 #endif
1090 #endif
1091 }
1092 else if(speed==SPEED_125K)
1093 { // 125000
1094 #ifdef ORANGE_TX
1095 /*USARTC0.BAUDCTRLA = 207 ;
1096 USARTC0.BAUDCTRLB = 0 ;
1097 USARTC0.CTRLB = 0x18 ;
1098 USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
1099 USARTC0.CTRLC = 0x03 ;*/
1100 #else
1101 #ifdef STM32_BOARD
1102 usart3_begin(125000,SERIAL_8N1); //USART3
1103 #else
1104 UBRR0H = 0x00;
1105 UBRR0L = 0x07;
1106 UCSR0A = 0x00 ; // Clear X2 bit
1107 //Set frame format to 8 data bits, none, 1 stop bit
1108 UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
1109 #endif
1110 #endif
1111 }
1112 #else
1113 (void)speed;
1114 #endif
1115 #ifndef ORANGE_TX
1116 #ifndef STM32_BOARD
1117 UCSR0B |= (1<<TXEN0);//tx enable
1118 #endif
1119 #endif
1120 }
1122 //Serial TX
1123 #ifdef ORANGE_TX
1124 ISR(USARTC0_DRE_vect)
1125 #else
1126 #ifdef STM32_BOARD
1127 void __irq_usart3()
1128 #else
1129 ISR(USART_UDRE_vect)
1130 #endif
1131 #endif
1132 { // Transmit interrupt
1133 #ifdef STM32_BOARD
1134 if(USART3_BASE->SR & USART_SR_TXE)
1135 {
1136 #endif
1137 if(tx_head!=tx_tail)
1138 {
1139 if(++tx_tail>=TXBUFFER_SIZE)//head
1140 tx_tail=0;
1141 #ifdef STM32_BOARD
1142 USART3_BASE->DR=tx_buff[tx_tail];//clears TXE bit
1143 #else
1144 UDR0=tx_buff[tx_tail];
1145 #endif
1146 }
1147 if (tx_tail == tx_head)
1148 {
1149 tx_pause(); // Check if all data is transmitted. If yes disable transmitter UDRE interrupt.
1150 }
1151 #ifdef STM32_BOARD
1152 }
1153 #endif
1154 }
1155 #else //BASH_SERIAL
1156 // Routines for bit-bashed serial output
1158 // Speed is 0 for 100K and 1 for 9600
1159 void initTXSerial( uint8_t speed)
1160 {
1161 TIMSK0 = 0 ; // Stop all timer 0 interrupts
1162 #ifdef INVERT_SERIAL
1163 SERIAL_TX_off;
1164 #else
1165 SERIAL_TX_on;
1166 #endif
1167 UCSR0B &= ~(1<<TXEN0) ;
1169 SerialControl.speed = speed ;
1170 if ( speed == SPEED_9600 )
1171 {
1172 OCR0A = 207 ; // 104uS period
1173 TCCR0A = 3 ;
1174 TCCR0B = 0x0A ; // Fast PMM, 2MHz
1175 }
1176 else // 100K
1177 {
1178 TCCR0A = 0 ;
1179 TCCR0B = 2 ; // Clock/8 (0.5uS)
1180 }
1181 }
1183 void Serial_write( uint8_t byte )
1184 {
1185 uint8_t temp ;
1186 uint8_t temp1 ;
1187 uint8_t byteLo ;
1189 #ifdef INVERT_SERIAL
1190 byte = ~byte ;
1191 #endif
1193 byteLo = byte ;
1194 byteLo >>= 7 ; // Top bit
1195 if ( SerialControl.speed == SPEED_100K )
1196 {
1197 #ifdef INVERT_SERIAL
1198 byteLo |= 0x02 ; // Parity bit
1199 #else
1200 byteLo |= 0xFC ; // Stop bits
1201 #endif
1202 // calc parity
1203 temp = byte ;
1204 temp >>= 4 ;
1205 temp = byte ^ temp ;
1206 temp1 = temp ;
1207 temp1 >>= 2 ;
1208 temp = temp ^ temp1 ;
1209 temp1 = temp ;
1210 temp1 <<= 1 ;
1211 temp ^= temp1 ;
1212 temp &= 0x02 ;
1213 #ifdef INVERT_SERIAL
1214 byteLo ^= temp ;
1215 #else
1216 byteLo |= temp ;
1217 #endif
1218 }
1219 else
1220 {
1221 byteLo |= 0xFE ; // Stop bit
1222 }
1223 byte <<= 1 ;
1224 #ifdef INVERT_SERIAL
1225 byte |= 1 ; // Start bit
1226 #endif
1227 uint8_t next = SerialControl.head + 2;
1228 if(next>=TXBUFFER_SIZE)
1229 next=0;
1230 if ( next != SerialControl.tail )
1231 {
1232 SerialControl.data[SerialControl.head] = byte ;
1233 SerialControl.data[SerialControl.head+1] = byteLo ;
1234 SerialControl.head = next ;
1235 }
1236 if(!IS_TX_PAUSE_on)
1237 tx_resume();
1238 }
1240 void resumeBashSerial()
1241 {
1242 cli() ;
1243 if ( SerialControl.busy == 0 )
1244 {
1245 sei() ;
1246 // Start the transmission here
1247 #ifdef INVERT_SERIAL
1248 GPIOR2 = 0 ;
1249 #else
1250 GPIOR2 = 0x01 ;
1251 #endif
1252 if ( SerialControl.speed == SPEED_100K )
1253 {
1254 GPIOR1 = 1 ;
1255 OCR0B = TCNT0 + 40 ;
1256 OCR0A = OCR0B + 210 ;
1257 TIFR0 = (1<<OCF0A) | (1<<OCF0B) ;
1258 TIMSK0 |= (1<<OCIE0B) ;
1259 SerialControl.busy = 1 ;
1260 }
1261 else
1262 {
1263 GPIOR1 = 1 ;
1264 TIFR0 = (1<<TOV0) ;
1265 TIMSK0 |= (1<<TOIE0) ;
1266 SerialControl.busy = 1 ;
1267 }
1268 }
1269 else
1270 {
1271 sei() ;
1272 }
1273 }
1275 // Assume timer0 at 0.5uS clock
1277 ISR(TIMER0_COMPA_vect)
1278 {
1279 uint8_t byte ;
1280 byte = GPIOR0 ;
1281 if ( byte & 0x01 )
1282 SERIAL_TX_on;
1283 else
1284 SERIAL_TX_off;
1285 byte /= 2 ; // Generates shorter code than byte >>= 1
1286 GPIOR0 = byte ;
1287 if ( --GPIOR1 == 0 )
1288 {
1289 TIMSK0 &= ~(1<<OCIE0A) ;
1290 GPIOR1 = 3 ;
1291 }
1292 else
1293 OCR0A += 20 ;
1294 }
1296 ISR(TIMER0_COMPB_vect)
1297 {
1298 uint8_t byte ;
1299 byte = GPIOR2 ;
1300 if ( byte & 0x01 )
1301 SERIAL_TX_on;
1302 else
1303 SERIAL_TX_off;
1304 byte /= 2 ; // Generates shorter code than byte >>= 1
1305 GPIOR2 = byte ;
1306 if ( --GPIOR1 == 0 )
1307 {
1308 if ( IS_TX_PAUSE_on )
1309 {
1310 SerialControl.busy = 0 ;
1311 TIMSK0 &= ~(1<<OCIE0B) ;
1312 }
1313 else
1314 {
1315 // prepare next byte and allow for 2 stop bits
1316 volatile struct t_serial_bash *ptr = &SerialControl ;
1317 if ( ptr->head != ptr->tail )
1318 {
1319 GPIOR0 = ptr->data[ptr->tail] ;
1320 GPIOR2 = ptr->data[ptr->tail+1] ;
1321 uint8_t nextTail = ptr->tail + 2 ;
1322 if ( nextTail >= TXBUFFER_SIZE )
1323 nextTail = 0 ;
1324 ptr->tail = nextTail ;
1325 GPIOR1 = 8 ;
1326 OCR0A = OCR0B + 40 ;
1327 OCR0B = OCR0A + 8 * 20 ;
1328 TIMSK0 |= (1<<OCIE0A) ;
1329 }
1330 else
1331 {
1332 SerialControl.busy = 0 ;
1333 TIMSK0 &= ~(1<<OCIE0B) ;
1334 }
1335 }
1336 }
1337 else
1338 OCR0B += 20 ;
1339 }
1341 ISR(TIMER0_OVF_vect)
1342 {
1343 uint8_t byte ;
1344 if ( GPIOR1 > 2 )
1345 byte = GPIOR0 ;
1346 else
1347 byte = GPIOR2 ;
1348 if ( byte & 0x01 )
1349 SERIAL_TX_on;
1350 else
1351 SERIAL_TX_off;
1352 byte /= 2 ; // Generates shorter code than byte >>= 1
1353 if ( GPIOR1 > 2 )
1354 GPIOR0 = byte ;
1355 else
1356 GPIOR2 = byte ;
1357 if ( --GPIOR1 == 0 )
1358 { // prepare next byte
1359 volatile struct t_serial_bash *ptr = &SerialControl ;
1360 if ( ptr->head != ptr->tail )
1361 {
1362 GPIOR0 = ptr->data[ptr->tail] ;
1363 GPIOR2 = ptr->data[ptr->tail+1] ;
1364 uint8_t nextTail = ptr->tail + 2 ;
1365 if ( nextTail >= TXBUFFER_SIZE )
1366 nextTail = 0 ;
1367 ptr->tail = nextTail ;
1368 GPIOR1 = 10 ;
1369 }
1370 else
1371 {
1372 SerialControl.busy = 0 ;
1373 TIMSK0 &= ~(1<<TOIE0) ;
1374 }
1375 }
1376 }
1379 #endif // BASH_SERIAL
1381 #endif // TELEMETRY