radio/src/pulses/pulses_avr.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22
  23 uint16_t nextMixerEndTime = 0;
  24 #define SCHEDULE_MIXER_END(delay) nextMixerEndTime = getTmr16KHz() + (delay) - 2*16/*2ms*/
  25
  26 #if defined(DSM2)
  27   // DSM2 control bits
  28   #define DSM2_CHANS                         6
  29   #define FRANCE_BIT                         0x10
  30   #define DSMX_BIT                           0x08
  31   #define BAD_DATA                           0x47
  32   #define DSM2_SEND_BIND                     (1 << 7)
  33   #define DSM2_SEND_RANGECHECK               (1 << 5)
  34   uint8_t  dsm2BindTimer = DSM2_BIND_TIMEOUT;
  35 #endif
  36
  37 #if defined(PXX)
  38   #define PXX_SEND_BIND                      0x01
  39   #define PXX_SEND_FAILSAFE                  (1 << 4)
  40   #define PXX_SEND_RANGECHECK                (1 << 5)
  41 #endif
  42
  43 #if defined(DSM2) || defined(PXX)
  44 uint8_t moduleFlag[NUM_MODULES] = { 0 };
  45 #endif
  46
  47 uint8_t s_current_protocol[1] = { 255 };
  48 uint8_t s_pulses_paused = 0;
  49
  50 uint16_t B3_comp_value;
  51
  52 #if defined(DSM2_SERIAL)
  53 inline void DSM2_EnableTXD(void)
  54 {
  55   UCSR0B |= (1 << TXEN0); // enable TX
  56   // don't enable UDRE0 interrupt now, it will be enabled during next setupPulses
  57 }
  58 #endif
  59
  60 void set_timer3_capture(void);
  61 void set_timer3_ppm(void);
  62
  63 void startPulses()
  64 {
  65 #if defined(CPUM2560)
  66 #if defined(DSM2_SERIAL)
  67   if (!IS_DSM2_PROTOCOL(g_model.protocol))
  68 #endif
  69   {
  70     // TODO g: There has to be a better place for this bug fix
  71     OCR1B = 0xffff; /* Prevent any PPM_OUT pin toggle before the TCNT1 interrupt
  72                        fires for the first time and sets up the pulse period.
  73                        *** Prevents WDT reset loop. */
  74   }
  75 #endif
  76
  77 #if defined(SIMU)
  78   s_current_protocol[0] = g_model.protocol;
  79 #else
  80   setupPulses();
  81 #endif // SIMU
  82 }
  83
  84 #define PULSES_SIZE       144
  85 uint8_t pulses2MHz[PULSES_SIZE] = {0}; // TODO check this length, pulled from er9x, perhaps too big.
  86 uint8_t *pulses2MHzRPtr = pulses2MHz;
  87
  88 #if defined(DSM2) || defined(PXX) || defined(IRPROTOS)
  89 uint8_t *pulses2MHzWPtr = pulses2MHz;
  90 #endif
  91
  92 #define CTRL_END 0
  93 #define CTRL_CNT 1
  94 #define CTRL_REP_1CMD -3
  95 #define CTRL_REP_2CMD -6
  96
  97 #define SETUP_PULSES_DURATION 1000 // 500us
  98 uint8_t g_ppmPulsePolarity = 0; // Needed for Bit-bang PPM.
  99
 100 // TIMER1_COMPA_vect used for PPM and DSM2=SERIAL.
 101 ISR(TIMER1_COMPA_vect) // 2MHz pulse generation (BLOCKING ISR).
 102 {
 103   uint8_t dt = TCNT1L; // Record Timer1 latency for DEBUG stats display.
 104
 105   // Call setupPulses only after "rest" period has elapsed.
 106   // Must do this before toggle PORTB to keep timing accurate.
 107   if (IS_DSM2_SERIAL_PROTOCOL(s_current_protocol[0]) || *((uint16_t*)pulses2MHzRPtr) == 0) {
 108     if (!IS_DSM2_SERIAL_PROTOCOL(s_current_protocol[0])) {
 109       OCR1A = SETUP_PULSES_DURATION;
 110 #if defined(CPUM2560) // CPUM2560 hardware toggled PPM out.
 111       OCR1B = OCR1A;
 112       if (g_model.pulsePol) {
 113         TCCR1A = (TCCR1A | (1<<COM1B1)) & ~(1<<COM1B0); // Set idle level.
 114       }
 115       else {
 116         TCCR1A |= 3<<COM1B0;
 117       }
 118       TCCR1C = 1<<FOC1B; // Strobe FOC1B.
 119       TCCR1A = (TCCR1A | (1<<COM1B0)) & ~(1<<COM1B1); // Toggle OC1B on next match.
 120 #endif
 121     }
 122     setupPulses(); // Does not sei() for setupPulsesPPM.
 123     heartbeat |= HEART_TIMER_PULSES;
 124     return;
 125   }
 126
 127   if (s_current_protocol[0] != PROTO_NONE) {
 128 #if !defined(CPUM2560)
 129     // Original Bit-bang for PPM.
 130     if (g_ppmPulsePolarity) {
 131       PORTB |= (1<<OUT_B_PPM); // GCC optimisation should result in a single SBI instruction
 132       g_ppmPulsePolarity = 0;
 133     }
 134     else {
 135       PORTB &= ~(1<<OUT_B_PPM);
 136       g_ppmPulsePolarity = 1;
 137     }
 138 #else // defined(CPUM2560)
 139     // CPUM2560 hardware toggled PPM out.
 140     if (*(uint16_t*)(pulses2MHzRPtr + sizeof(uint16_t)) == 0) {
 141       // Look one step ahead to see if we are currently the "rest" period.
 142       OCR1B = 0xffff; // Prevent next compare match hence toggle.
 143     }
 144     else {
 145       OCR1B = *((uint16_t*) pulses2MHzRPtr);
 146     }
 147 #endif
 148   }
 149
 150   OCR1A = *((uint16_t*) pulses2MHzRPtr); // Schedule next Timer1 interrupt vector (to this function).
 151   pulses2MHzRPtr += sizeof(uint16_t); // Non PPM protocols use uint8_t pulse buffer.
 152
 153   if (dt > g_tmr1Latency_max) g_tmr1Latency_max = dt;
 154   if (dt < g_tmr1Latency_min) g_tmr1Latency_min = dt;
 155 }
 156
 157 void setupPulsesPPM(uint8_t proto)
 158 {
 159   // Total frame length is a fixed 22.5msec (more than 9 channels is non-standard and requires this to be extended.)
 160   // Each channel's pulse is 0.7 to 1.7ms long, with a 0.3ms stop tail, making each compelte cycle 1 to 2ms.
 161
 162   int16_t PPM_range = g_model.extendedLimits ? 640*2 : 512*2;   //range of 0.7..1.7msec
 163
 164   uint16_t *ptr = (proto == PROTO_PPM ? (uint16_t *)pulses2MHz : (uint16_t *) &pulses2MHz[PULSES_SIZE/2]);
 165
 166   //The pulse ISR is 2mhz that's why everything is multiplied by 2
 167   uint8_t p = (proto == PROTO_PPM16 ? 16 : 8) + (g_model.ppmNCH * 2); //Channels *2
 168   uint16_t q = (g_model.ppmDelay*50+300)*2; // Stoplen *2
 169   int32_t rest = 22500u*2 - q;
 170
 171   rest += (int32_t(g_model.ppmFrameLength))*1000;
 172   for (uint8_t i=(proto==PROTO_PPM16) ? p-8 : 0; i<p; i++) {
 173     int16_t v = limit((int16_t)-PPM_range, channelOutputs[i], (int16_t)PPM_range) + 2*PPM_CH_CENTER(i);
 174     rest -= v;
 175     *ptr++ = q;
 176     *ptr++ = v - q; // total pulse width includes stop phase
 177   }
 178
 179   *ptr++ = q;
 180   if (rest > 65535) rest = 65535; /* prevents overflows */
 181   if (rest < 9000)  rest = 9000;
 182
 183   if (proto == PROTO_PPM) {
 184     *ptr++ = rest - SETUP_PULSES_DURATION;
 185     pulses2MHzRPtr = pulses2MHz;
 186   }
 187   else {
 188     *ptr++ = rest;
 189     B3_comp_value = rest - SETUP_PULSES_DURATION;               // 500uS before end of sync pulse
 190   }
 191
 192   *ptr = 0;
 193 }
 194
 195
 196 #if defined(PXX)
 197 const pm_uint16_t CRCTable[] PROGMEM =
 198 {
 199   0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf,
 200   0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7,
 201   0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e,
 202   0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876,
 203   0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd,
 204   0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5,
 205   0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c,
 206   0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974,
 207   0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb,
 208   0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3,
 209   0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a,
 210   0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72,
 211   0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9,
 212   0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1,
 213   0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738,
 214   0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70,
 215   0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7,
 216   0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff,
 217   0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036,
 218   0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e,
 219   0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5,
 220   0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd,
 221   0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134,
 222   0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c,
 223   0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3,
 224   0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb,
 225   0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232,
 226   0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a,
 227   0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1,
 228   0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9,
 229   0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330,
 230   0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78
 231 };
 232
 233 uint8_t PcmByte;
 234 uint8_t PcmBitCount;
 235 uint16_t PcmCrc;
 236 uint8_t PcmOnesCount;
 237
 238 void crc(uint8_t data)
 239 {
 240   PcmCrc = (PcmCrc<<8) ^ pgm_read_word(&CRCTable[((PcmCrc >> 8) ^ data) & 0xFF]);
 241 }
 242
 243
 244 void putPcmPart(uint8_t value)
 245 {
 246   PcmByte >>= 2;
 247   PcmByte |= value;
 248   if (++PcmBitCount >= 4) {
 249     *pulses2MHzWPtr++ = PcmByte;
 250     PcmBitCount = PcmByte = 0;
 251   }
 252 }
 253
 254
 255 void putPcmFlush()
 256 {
 257   while (PcmBitCount != 0) {
 258     putPcmPart(0); // Empty
 259   }
 260   *pulses2MHzWPtr = 0; // Mark end
 261 }
 262
 263 void putPcmBit(uint8_t bit)
 264 {
 265   if (bit) {
 266     PcmOnesCount += 1;
 267     putPcmPart(0x80);
 268   }
 269   else {
 270     PcmOnesCount = 0;
 271     putPcmPart(0xC0);
 272   }
 273   if (PcmOnesCount >= 5) {
 274     putPcmBit(0);                                // Stuff a 0 bit in
 275   }
 276 }
 277
 278 void putPcmByte(uint8_t byte)
 279 {
 280   uint8_t i;
 281
 282   crc(byte);
 283
 284   for (i=0; i<8; i++) {
 285     putPcmBit(byte & 0x80);
 286     byte <<= 1;
 287   }
 288 }
 289
 290
 291 void putPcmHead()
 292 {
 293   // send 7E, do not CRC
 294   // 01111110
 295   putPcmPart(0xC0);
 296   putPcmPart(0x80);
 297   putPcmPart(0x80);
 298   putPcmPart(0x80);
 299   putPcmPart(0x80);
 300   putPcmPart(0x80);
 301   putPcmPart(0x80);
 302   putPcmPart(0xC0);
 303 }
 304
 305 uint16_t scaleForPXX(uint8_t i)
 306 {
 307   int16_t value = ((i < 16) ? channelOutputs[i] * 3 / 4 : 0) + 1024;
 308   return limit<uint16_t>(1, value, 2046);
 309 }
 310
 311 void setupPulsesPXX()
 312 {
 313   uint16_t chan;
 314   uint16_t chan_1;
 315
 316   pulses2MHzWPtr = pulses2MHz;
 317   pulses2MHzRPtr = pulses2MHz;
 318
 319   PcmCrc = 0;
 320   PcmBitCount = PcmByte = 0;
 321   PcmOnesCount = 0;
 322   putPcmHead();
 323   putPcmByte(g_model.header.modelId[0]);
 324   uint8_t flag1 = 0;
 325   if (moduleFlag[0] == MODULE_BIND) {
 326     flag1 |= (g_eeGeneral.countryCode << 1) | PXX_SEND_BIND;
 327   }
 328   else if (moduleFlag[0] == MODULE_RANGECHECK) {
 329     flag1 |= PXX_SEND_RANGECHECK;
 330   }
 331   putPcmByte(flag1);     // First byte of flags
 332   putPcmByte(0);     // Second byte of flags
 333   for (uint8_t i=0; i<8; i+=2) {              // First 8 channels only
 334     chan = scaleForPXX(i);
 335     chan_1 = scaleForPXX(i+1);
 336     putPcmByte(chan); // Low byte of channel
 337     putPcmByte(((chan >> 8) & 0x0F) | (chan_1 << 4));  // 4 bits each from 2 channels
 338     putPcmByte(chan_1 >> 4);  // High byte of channel
 339   }
 340   putPcmByte(0);
 341   chan = PcmCrc;                     // get the crc
 342   putPcmByte(chan>>8);                        // Checksum lo
 343   putPcmByte(chan); // Checksum hi
 344   putPcmHead();
 345   putPcmFlush();
 346   OCR1C += 40000;            // 20mS on
 347   PORTB |= (1<<OUT_B_PPM);
 348 }
 349 #endif
 350
 351 #if defined(DSM2_SERIAL)
 352
 353 // DSM2 protocol pulled from th9x - Thanks thus!!!
 354
 355 //http://www.rclineforum.de/forum/board49-zubeh-r-elektronik-usw/fernsteuerungen-sender-und-emp/neuer-9-kanal-sender-f-r-70-au/Beitrag_3897736#post3897736
 356 //(dsm2(LP4DSM aus den RTF (Ready To Fly) Sendern von Spektrum.
 357 //http://www.rcgroups.com/forums/showpost.php?p=18554028&postcount=237
 358 // /home/thus/txt/flieger/PPMtoDSM.c
 359 /*
 360   125000 Baud 8n1      _ xxxx xxxx - ---
 361 #define DSM2_CHANNELS      6                // Max number of DSM2 Channels transmitted
 362 #define DSM2_BIT (8*2)
 363 bind:
 364   DSM2_Header = 0x80,0
 365 static byte DSM2_Channel[DSM2_CHANNELS*2] = {
 366                 ch
 367   0x00,0xAA,     0 0aa
 368   0x05,0xFF,     1 1ff
 369   0x09,0xFF,     2 1ff
 370   0x0D,0xFF,     3 1ff
 371   0x13,0x54,     4 354
 372   0x14,0xAA      5 0aa
 373 };
 374
 375 normal:
 376   DSM2_Header = 0,0;
 377   DSM2_Channel[i*2]   = (byte)(i<<2) | highByte(pulse);
 378   DSM2_Channel[i*2+1] = lowByte(pulse);
 379
 380  */
 381
 382 // DSM2=SERIAL mode
 383 FORCEINLINE void setupPulsesDSM2()
 384 {
 385   uint16_t *ptr = (uint16_t *)pulses2MHz;
 386   switch (s_current_protocol[0])
 387   {
 388     case PROTO_DSM2_LP45:
 389       *ptr = 0x00;
 390       break;
 391     case PROTO_DSM2_DSM2:
 392       *ptr = 0x10;
 393       break;
 394     default: // DSMX
 395       *ptr = 0x18;
 396       break;
 397   }
 398
 399   if (dsm2BindTimer > 0) {
 400     dsm2BindTimer--;
 401     if (switchState(SW_DSM2_BIND)) {
 402       moduleFlag[0] = MODULE_BIND;
 403       *ptr |= DSM2_SEND_BIND;
 404     }
 405   }
 406   else if (moduleFlag[0] == MODULE_RANGECHECK) {
 407     *ptr |= DSM2_SEND_RANGECHECK;
 408   }
 409   else {
 410     moduleFlag[0] = 0;
 411   }
 412
 413   ptr++;
 414   *ptr++ = g_model.header.modelId[0];
 415
 416   for (uint8_t i=0; i<DSM2_CHANS; i++) {
 417     uint16_t pulse = limit(0, ((channelOutputs[i]*13)>>5)+512,1023);
 418     *ptr++ = (i<<2) | ((pulse>>8)&0x03); // encoded channel + upper 2 bits pulse width
 419     *ptr++ = pulse & 0xff; // low byte
 420   }
 421
 422   pulses2MHzWPtr = (uint8_t *)ptr;
 423   pulses2MHzRPtr = pulses2MHz;
 424
 425   UCSR0B |= (1 << UDRIE0); // enable UDRE0 interrupt to start transmitting DSM2 data bytes from buffer
 426 }
 427
 428 // DSM2=SERIAL mode
 429 void DSM2_Done()
 430 {
 431   UCSR0B &= ~((1 << TXEN0) | (1 << UDRIE0)); // disable UART TX and interrupt
 432 }
 433
 434 // DSM2=SERIAL mode
 435 void DSM2_Init(void)
 436 {
 437 #ifndef SIMU
 438
 439 #undef BAUD
 440 #define BAUD 125000
 441
 442 #include <util/setbaud.h>
 443
 444   UBRR0H = UBRRH_VALUE;
 445   UBRR0L = UBRRL_VALUE;
 446   UCSR0A &= ~(1 << U2X0); // disable double speed operation.
 447
 448   // set 8N1 (leave TX and RX disabled for now)
 449   UCSR0B = 0 | (0 << RXCIE0) | (0 << TXCIE0) | (0 << UDRIE0) | (0 << RXEN0) | (0 << TXEN0) | (0 << UCSZ02);
 450   UCSR0C = 0 | (1 << UCSZ01) | (1 << UCSZ00);
 451
 452   while (UCSR0A & (1 << RXC0)) {
 453     UDR0;  // flush receive buffer
 454   }
 455
 456   // These should be running right from power up on a FrSky enabled '9X.
 457   DSM2_EnableTXD(); // enable DSM2 UART transmitter
 458
 459 #endif // SIMU
 460 }
 461 #endif // defined(DSM2_SERIAL)
 462
 463 /****** END DSM2=SERIAL ********/
 464
 465
 466
 467 /////////////////////////////////////////////////////////////
 468
 469
 470
 471 /******* BEGIN DSM2=PPM ********/
 472
 473 #if defined(DSM2_PPM)
 474 inline void _send_1(uint8_t v)
 475 {
 476   *pulses2MHzWPtr++ = v;
 477 }
 478
 479 #define BITLEN_DSM2 (8*2) //125000 Baud
 480 void sendByteDsm2(uint8_t b) //max 10changes 0 10 10 10 10 1
 481 {
 482   bool lev = 0;
 483   uint8_t len = BITLEN_DSM2; //max val: 9*16 < 256
 484   for (uint8_t i=0; i<=8; i++) { //8Bits + Stop=1
 485     bool nlev = b & 1; //lsb first
 486     if (lev == nlev) {
 487       len += BITLEN_DSM2;
 488     }
 489     else {
 490 #if defined(CPUM2560)
 491       // G: Compensate for main clock synchronisation -- to get accurate 8us bit length
 492       // NOTE: This has now been tested as NOT required on the stock board, with the ATmega64A chip.
 493       _send_1(nlev ? len-5 : len+3);
 494 #else
 495       _send_1(len-1);
 496 #endif
 497       len  = BITLEN_DSM2;
 498       lev  = nlev;
 499     }
 500     b = (b>>1) | 0x80; //shift in stop bit
 501   }
 502 #if defined (CPUM2560)
 503   _send_1(len+BITLEN_DSM2+3); // 2 stop bits
 504 #else
 505   _send_1(len+BITLEN_DSM2-1); // 2 stop bits
 506 #endif
 507 }
 508
 509 // DSM2=PPM mode
 510 void setupPulsesDSM2()
 511 {
 512   static uint8_t dsmDat[2+6*2] = {0xFF,0x00, 0x00,0xAA, 0x05,0xFF, 0x09,0xFF, 0x0D,0xFF, 0x13,0x54, 0x14,0xAA};
 513
 514   pulses2MHzWPtr = pulses2MHz;
 515
 516   // If more channels needed make sure the pulses union/array is large enough
 517   switch (s_current_protocol[0]) {
 518     case PROTO_DSM2_LP45:
 519       dsmDat[0] = 0x00;
 520       break;
 521     case PROTO_DSM2_DSM2:
 522       dsmDat[0] = 0x10;
 523       break;
 524     default: // DSMX
 525       dsmDat[0] = 0x18;
 526       break;
 527   }
 528
 529   if (dsm2BindTimer > 0) {
 530     dsm2BindTimer--;
 531     if (switchState(SW_DSM2_BIND)) {
 532       moduleFlag[0] = MODULE_BIND;
 533       dsmDat[0] |= DSM2_SEND_BIND;
 534     }
 535   }
 536   else if (moduleFlag[0] == MODULE_RANGECHECK) {
 537     dsmDat[0] |= DSM2_SEND_RANGECHECK;
 538   }
 539   else {
 540     moduleFlag[0] = 0;
 541   }
 542
 543   dsmDat[1] = g_model.header.modelId[0];
 544
 545   for (uint8_t i=0; i<DSM2_CHANS; i++) {
 546     uint16_t pulse = limit(0, ((channelOutputs[i]*13)>>5)+512, 1023);
 547     dsmDat[2+2*i] = (i<<2) | ((pulse>>8)&0x03); // encoded channel + upper 2 bits pulse width
 548     dsmDat[3+2*i] = pulse & 0xff; // low byte
 549   }
 550
 551   for (uint8_t counter=0; counter<14; counter++) {
 552     sendByteDsm2(dsmDat[counter]);
 553   }
 554
 555   pulses2MHzWPtr -= 1; //remove last stopbits and
 556
 557 #if !defined(CPUM2560)
 558 //G: Removed to get waveform correct on analyser. Leave in for stock board until tests can be done.
 559   _send_1(255); // prolong them
 560 #endif
 561   _send_1(0); //end of pulse stream
 562
 563   pulses2MHzRPtr = pulses2MHz;
 564 }
 565 #endif
 566
 567 /****** END DSM2=PPM ********/
 568
 569 #if defined(IRPROTOS)
 570 static void _send_u8(uint8_t u8)
 571 {
 572 #ifdef SIMU
 573   *(pulses2MHzPtr)++ = u8;
 574 #else
 575   asm volatile(
 576     " st   Z+,%A[u]        \n\t"
 577
 578     : [p]"=z"(pulses2MHzWPtr)
 579     :    "%[p]"(pulses2MHzWPtr)
 580     , [u]"r"(u8)
 581  );
 582
 583 #endif
 584 }
 585
 586 static void _send_u16(uint16_t u16)
 587 {
 588 #ifdef  SIMU
 589   *(*(uint16_t**)&pulses2MHzWPtr)++ = u16;
 590 #else
 591   asm volatile(
 592     " st   Z+,%A[t0]        \n\t"
 593     " st   Z+,%B[t0]        \n\t"
 594
 595     : [p]"=z"(pulses2MHzWPtr)
 596     :    "%[p]"(pulses2MHzWPtr)
 597     , [t0]"r"(u16)
 598  );
 599
 600 #endif
 601 }
 602
 603 static void _send_1(uint16_t t0)
 604 {
 605   //  *(*(uint16_t**)&pulses2MHzPtr)++=t0;
 606   _send_u16(t0);
 607   *pulses2MHzWPtr++ = CTRL_CNT;
 608   //_send_u8(CTRL_CNT);
 609 }
 610
 611 static void _send_rep1(uint16_t t0, uint8_t cnt)
 612 {
 613   //  *(*(uint16_t**)&pulses2MHzPtr)++=t0;
 614   _send_u16(t0);
 615   _send_u8(CTRL_REP_1CMD);
 616   _send_u8(cnt);
 617   _send_u8(CTRL_CNT);
 618 }
 619
 620 //picco z
 621 //http://home.versanet.de/~b-konze/uni_fb/uni_fb.htm
 622 // /home/husteret/txt/flieger/protokolle/m168fb_ufo_v08/picooz.c
 623 //
 624 // 1900  650 650         1226             650       1226           Stop
 625 // ----   __  -- __ -- __----__----__     --__      ----____       --__
 626 /*
 627   chn:2 a=00 b=01 c=10
 628   pow:4u msb first
 629   trim:4s  -2,0,1
 630   direction:3s
 631   -chk[0]
 632   -chk[1]
 633   0
 634 -------
 635 2-bit sum = 0
 636
 637 chk:2 = chn:2 + pow>>2:2 +pow:2 +  trim>>2:2 +trim:2 dir + direction>>1:2 + direction<<1:2
 638
 639 */
 640
 641 #define PICOOZ_RC_HIGH   93 //(1226/13)
 642 #define PICOOZ_RC_LOW    49 //(650/13)
 643
 644 #define LEN_38KHZ (13*2) //= 38,46KHz
 645
 646 void picco_sendB1(bool bit)
 647 {
 648   if (bit) {
 649     _send_rep1(LEN_38KHZ-1, PICOOZ_RC_HIGH);     //ungerade anzahl 10101
 650     _send_1   (1226*2 - 1);   //lange 0
 651   }
 652   else {
 653     _send_rep1(LEN_38KHZ-1, PICOOZ_RC_LOW);      //ungerade anzahl 10101
 654     _send_1   (650*2  - 1);   //lange 0
 655   }
 656 }
 657
 658 void picco_sendBn(uint8_t bits, uint8_t n)
 659 {
 660   while (n--) {
 661     picco_sendB1(bits & (1<<n));
 662   }
 663 }
 664
 665 #define BITS  10
 666 #define BITS2 (BITS-1)
 667
 668 NOINLINE uint8_t reduce7u(int16_t v, uint8_t sfr)
 669 {
 670   v += (1<<BITS2);
 671   if (v < 0) v = 0;
 672   if (v >= (1<<BITS)) v=(1<<BITS)-1;
 673   return v >> sfr;
 674 }
 675
 676 NOINLINE int8_t reduce7s(int16_t v, uint8_t sfr, uint8_t sf2, int8_t ofs2)
 677 {
 678   v += (1<<BITS2) + sf2;
 679   if (v & (1<<BITS)) {
 680     v = (1<<BITS)-1; // no overflow
 681   }
 682   int8_t  i8 = (uint16_t)v>>sfr;
 683   if (i8 <= 0) {
 684     i8 = 1;
 685   }
 686   i8 -= ofs2;
 687   if (i8 >= ofs2) {
 688     i8 = ofs2-1; //no overflow
 689   }
 690   return i8;
 691 }
 692
 693 //these defines allow the compiler to preclculate constants
 694 #define getChan7u(i,bitsRes) reduce7u(channelOutputs[i],(BITS-bitsRes))
 695 #define getChan7s(i,bitsRes) reduce7s(channelOutputs[i],(BITS-bitsRes),1<<((BITS-bitsRes)-1),1<<(bitsRes-1))
 696
 697 static void setupPulsesPiccoZ(uint8_t chn)
 698 {
 699   // 1900  650 650         1226             650 0bit  1226 1bit      Stop
 700   // ----   __  -- __ -- __----__----__     --__      ----____       --__
 701   static bool    state = 0;
 702   static uint8_t pow;
 703   static int8_t  trim;
 704   static int8_t  dir;
 705   static uint8_t chk;
 706   if (state == 0) {
 707     _send_rep1(LEN_38KHZ-1, 147);   // 1900/13  !! must be odd
 708     _send_1   (650*2 - 1);//
 709     picco_sendBn(0,2);
 710     _send_rep1(LEN_38KHZ-1, PICOOZ_RC_HIGH);
 711     _send_1   (650*2 - 1);//
 712     _send_rep1(LEN_38KHZ-1, PICOOZ_RC_HIGH);
 713     _send_1   (650*2 - 1);//
 714     //   chn:2 a=00 b=01 c=10
 715     //   pow:4u msb first
 716     //   trim:4s  -2,0,1
 717     //   dir:3s
 718     //   -chk[0]
 719     //   -chk[1]
 720     //   0
 721     pow  = getChan7u(2,4);
 722     trim = getChan7s(1,4);
 723     dir  = getChan7s(0,3);
 724     chk    = - (chn+ (pow>>2) + pow + (trim>>2) + trim + (dir>>1) + (dir<<1));
 725   }
 726   else {
 727     picco_sendBn(chn,2);
 728     picco_sendBn(pow,4);
 729     picco_sendBn(trim,4);
 730     picco_sendBn(dir,3);
 731     picco_sendB1(chk & (1<<0)); //lsb first, because we are here on a odd bit (dir is only 3 bits)
 732     picco_sendB1(chk & (1<<1));
 733     _send_rep1(LEN_38KHZ-1, PICOOZ_RC_LOW); //0-bit pulses
 734     _send_1   (20000u*2  - 1); //20ms gap ?
 735   }
 736   state = !state;
 737 }
 738 #endif
 739
 740 void setupPulses()
 741 {
 742   uint8_t required_protocol = g_model.protocol;
 743
 744 #if defined(DEBUG) && !defined(VOICE)
 745   PORTH |= 0x80; // PORTH:7 LOW->HIGH signals start of setupPulses()
 746 #endif
 747
 748   if (s_pulses_paused) {
 749     required_protocol = PROTO_NONE;
 750   }
 751
 752 #if defined(CPUM2560) && defined(DSM2_PPM) && defined(TX_CADDY)
 753 // This should be here, executed on every loop, to ensure re-setting of the
 754 // TX moudle power control output register, in case of electrical glitch.
 755 // (Following advice of Atmel for MCU's used  in industrial / mission cricital
 756 // applications.)
 757   if (IS_DSM2_PROTOCOL(required_protocol))
 758     PORTH &= ~0x80;
 759   else
 760     PORTH |= 0x80;
 761 #endif
 762
 763   if (s_current_protocol[0] != required_protocol) {
 764
 765 #if defined(DSM2_SERIAL) && defined(TELEMETRY_FRSKY)
 766     if (s_current_protocol[0] == 255 || IS_DSM2_PROTOCOL(s_current_protocol[0])) {
 767       telemetryInit();
 768     }
 769 #endif
 770
 771     s_current_protocol[0] = required_protocol;
 772
 773     TCCR1B = 0;                           // Stop counter
 774     TCNT1 = 0;
 775
 776 #if defined(CPUM2560) || defined(CPUM2561)
 777     TIMSK1 &= ~0x2F;                      // All Timer1 interrupts off
 778     TIMSK1 &= ~(1<<OCIE1C);               // COMPC1 off
 779     TIFR1 = 0x2F;
 780 #else
 781     TIMSK &= ~0x3C;            // All interrupts off
 782     ETIMSK &= ~(1<<OCIE1C);    // COMPC1 off
 783     TIFR = 0x3C;               // Clear all pending interrupts
 784     ETIFR = 0x3F;              // Clear all pending interrupts
 785 #endif
 786
 787     switch (required_protocol) {
 788 #if defined(DSM2_PPM) // For DSM2=SERIAL, the default: case is executed, below
 789       case PROTO_DSM2_LP45:
 790       case PROTO_DSM2_DSM2:
 791       case PROTO_DSM2_DSMX:
 792         set_timer3_capture();
 793         OCR1C = 200;                          // 100 uS
 794         TCNT1 = 300;                          // Past the OCR1C value
 795         ICR1 = 44000;                         // Next frame starts in 22 mS
 796 #if defined(CPUM2560) || defined(CPUM2561)
 797         TIMSK1 |= 0x28;                       // Enable Timer1 COMPC and CAPT interrupts
 798         TCCR1A = (0 << WGM10);                // Set output waveform mode to normal, for now. Note that
 799                                               // WGM will be changed to toggle OCR1B pin on compare capture,
 800                                               // in next switch(required_protocol) {...}, below
 801 #else
 802         TIMSK |= 0x20;                        // Enable CAPT
 803         ETIMSK |= (1<<OCIE1C);                // Enable COMPC
 804         TCCR1A = (0 << WGM10);
 805 #endif
 806         TCCR1B = (3 << WGM12) | (2 << CS10);  // CTC ICR, 16MHz / 8
 807         break;
 808 #endif // defined(DSM2_PPM)
 809
 810 #if defined(PXX)
 811       case PROTO_PXX:
 812         set_timer3_capture();
 813         OCR1B = 6000;                         // Next frame starts in 3 mS
 814         OCR1C = 4000;                         // Next frame setup in 2 mS
 815 #if defined(CPUM2560) || defined(CPUM2561)
 816         TIMSK1 |= (1<<OCIE1B);                // Enable COMPB
 817         TIMSK1 |= (1<<OCIE1C);                // Enable COMPC
 818         TCCR1A = (3 << COM1B0);               // Connect OC1B for hardware PPM switching
 819 #else
 820         TIMSK |= (1<<OCIE1B);                 // Enable COMPB
 821         ETIMSK |= (1<<OCIE1C);                // Enable COMPC
 822         TCCR1A  = 0;
 823 #endif
 824         TCCR1B  = (2<<CS10);                  // ICNC3 16MHz / 8
 825         break;
 826 #endif
 827
 828       case PROTO_PPM16:
 829         OCR1A = 40000;                        // Next frame starts in 20 mS
 830 #if defined(CPUM2560) || defined(CPUM2561)
 831         TIMSK1 |= (1<<OCIE1A);                // Enable COMPA
 832         TCCR1A = (3 << COM1B0);               // Connect OC1B for hardware PPM switching
 833 #else
 834         TIMSK |= 0x10;                        // Enable COMPA
 835         TCCR1A = (0<<WGM10);
 836 #endif
 837         TCCR1B = (1 << WGM12) | (2<<CS10);   // CTC OCRA, 16MHz / 8
 838         setupPulsesPPM(PROTO_PPM16);
 839         OCR3A = 50000;
 840         OCR3B = 5000;
 841         set_timer3_ppm();
 842         break;
 843
 844       case PROTO_PPMSIM:
 845 #if defined(CPUM2560) || defined(CPUM2561)
 846         TCCR1A = 0;                           // Disconnect OC1B for bit-bang PPM switching
 847 #endif
 848         setupPulsesPPM(PROTO_PPMSIM);
 849         OCR3A = 50000;
 850         OCR3B = 5000;
 851         set_timer3_ppm();
 852         PORTB &= ~(1<<OUT_B_PPM);             // Hold PPM output low
 853         break;
 854
 855 #if defined(DSM2_SERIAL) && defined(TELEMETRY_FRSKY)
 856       case PROTO_DSM2_LP45:
 857       case PROTO_DSM2_DSM2:
 858       case PROTO_DSM2_DSMX:
 859         DSM2_Init();
 860         // no break
 861 #endif
 862
 863       default: // PPM and DSM2=SERIAL modes
 864         set_timer3_capture();
 865         OCR1A = 44000;                        // Next frame starts in 22ms -- DSM mode.
 866                                               //    This is arbitrary and for the first frame only. In fact, ...
 867                                               //    DSM2 mode will set frame timing again at each ISR(TIMER1_COMPC_vect)
 868                                               //       and
 869                                               //    PPM mode will dynamically adjust to the frame rate set in model SETUP menu,
 870                                               //    from within setupPulsesPPM().
 871 #if defined(CPUM2560) || defined(CPUM2561)
 872         TIMSK1 |= (1<<OCIE1A);                // Enable COMPA
 873         TCCR1A = (3 << COM1B0);               // Connect OC1B for hardware PPM switching. G: Not needed
 874                                               // for DSM2=SERIAL. But OK.
 875 #else
 876         TIMSK |= 0x10;                        // Enable COMPA
 877         TCCR1A = (0 << WGM10);
 878 #endif
 879         TCCR1B = (1 << WGM12) | (2 << CS10);  // CTC OCRA, 16MHz / 8
 880         break;
 881     }
 882   }
 883
 884   switch(required_protocol) {
 885 #if defined(PXX)
 886     case PROTO_PXX:
 887       // schedule next Mixer calculations
 888       SCHEDULE_MIXER_END(20*16);
 889       sei();
 890       setupPulsesPXX();
 891       break;
 892 #endif
 893
 894 #if defined(DSM2)
 895     case PROTO_DSM2_LP45:
 896     case PROTO_DSM2_DSM2:
 897     case PROTO_DSM2_DSMX:
 898       // schedule next Mixer calculations
 899       SCHEDULE_MIXER_END(22*16);
 900 #if defined(DSM2_PPM)
 901       sei();
 902 #endif
 903       setupPulsesDSM2(); // Different versions for DSM2=SERIAL vs. DSM2=PPM
 904 #if defined(CPUM2560) && defined(DSM2_PPM)
 905       // Ensure each DSM2=PPM serial packet starts out with the correct bit polarity
 906       TCCR1A = (0 << WGM10) | (3<<COM1B1);  // Make Waveform Generator 'SET' OCR1B pin on next compare event and ...
 907       TCCR1C = (1<<FOC1B);                  // ... force compare event, to set OCR1B pin high.
 908       TCCR1A = (1<<COM1B0);                 // Output is ready. Now configure OCR1B pin into 'TOGGLE' mode.
 909 #endif
 910       break;
 911 #endif
 912
 913 #if defined(IRPROTOS)
 914     case PROTO_PICZ:
 915       setupPulsesPiccoZ(g_model.ppmNCH);
 916       // TODO BSS stbyLevel = 0; //start with 1
 917       break;
 918 #endif
 919
 920     default: // standard PPM protocol
 921 #if !defined(SIMU)
 922       g_ppmPulsePolarity = g_model.pulsePol;
 923 #endif
 924       // schedule next Mixer calculations
 925       SCHEDULE_MIXER_END(45*8+g_model.ppmFrameLength*8);
 926       // no sei here
 927       setupPulsesPPM(PROTO_PPM);
 928       // if PPM16, PPM16 pulses are set up automatically within the interrupts
 929       break;
 930   }
 931
 932 #if defined(DEBUG) && !defined(VOICE)
 933   PORTH &= ~0x80; // PORTH:7 HIGH->LOW signals end of setupPulses()
 934 #endif
 935 }
 936
 937 #ifndef SIMU
 938
 939 #if defined(DSM2_PPM) || defined(PXX)
 940 ISR(TIMER1_CAPT_vect) // 2MHz pulse generation
 941 {
 942 #if defined (CPUM2560)
 943   /*** G9X V4 hardware toggled PPM_out avoids any chance of output timing jitter ***/
 944
 945   // OCR1B output pin (PPM_OUT) is pre-SET in setupPulses -- on every new
 946   // frame, for safety -- and then configured to toggle on each OCR1B compare match.
 947   // Thus, all we need do here is update the compare regisiter(s) ...
 948   uint8_t x;
 949   x = *pulses2MHzRPtr++;  // Byte size
 950   ICR1 = x;
 951   OCR1B = (uint16_t)x;    // Duplicate capture compare value for OCR1B, because Timer1 is in CTC mode
 952                           // and thus we cannot use the OCR1B int. vector. (Should have put PPM_OUT
 953                           // pin on OCR1A. Oh well.)
 954
 955 #else // manual bit-bang mode
 956   uint8_t x;
 957   PORTB ^= (1<<OUT_B_PPM);    // Toggle PPM_OUT
 958   x = *pulses2MHzRPtr++;      // Byte size
 959   ICR1 = x;
 960   if (x > 200) PORTB |= (1<<OUT_B_PPM); // Make sure pulses are the correct way up.
 961 #endif
 962 }
 963
 964 #if defined(PXX)
 965 ISR(TIMER1_COMPB_vect) // PXX main interrupt
 966 {
 967   uint8_t x;
 968   PORTB ^= (1<<OUT_B_PPM);
 969   x = *pulses2MHzRPtr;      // Byte size
 970   if ((x & 1) == 0) {
 971     OCR1B += 32;
 972   }
 973   else {
 974     OCR1B += 16;
 975   }
 976   if ((x >>= 1) == 0) {
 977     if (*(++pulses2MHzRPtr) == 0) {
 978       OCR1B = OCR1C + 2000;              // 1mS on from OCR1B
 979     }
 980   }
 981   else {
 982     *pulses2MHzRPtr = x;
 983   }
 984
 985   heartbeat |= HEART_TIMER_PULSES;
 986 }
 987 #endif
 988
 989 ISR(TIMER1_COMPC_vect) // DSM2_PPM or PXX end of frame
 990 {
 991 #if defined(DSM2_PPM) && defined(PXX)
 992   if (IS_DSM2_PROTOCOL(s_current_protocol[0])) {
 993 #endif
 994
 995 #if defined(DSM2_PPM)
 996     ICR1 = 41536; // next frame starts in 22ms 41536 = 2*(22000 - 14*11*8)
 997     if (OCR1C < 255) {
 998       OCR1C = 39000;  // delay setup pulses by 19.5ms to reduce system latency
 999     }
1000     else {
1001       OCR1C = 200;
1002       // sei will be called inside setupPulses()
1003       setupPulses();
1004     }
1005
1006     heartbeat |= HEART_TIMER_PULSES;
1007 #endif
1008
1009 #if defined(DSM2_PPM) && defined(PXX)
1010   }
1011   else {
1012 #endif
1013
1014 #if defined(PXX)
1015     // must be PXX
1016     setupPulses();
1017 #endif
1018
1019 #if defined(DSM2_PPM) && defined(PXX)
1020   }
1021 #endif
1022 }
1023 #endif // defined(DSM2_PPM) || defined(PXX)
1024
1025 #endif // ifndef SIMU
1026
1027
1028 void set_timer3_capture()
1029 {
1030 #ifndef SIMU
1031 #if defined (CPUM2560) || defined(CPUM2561) // TODO TIMSK3 in #define!
1032   TIMSK3 &= ~((1<<OCIE3A) | (1<<OCIE3B) | (1<<OCIE3C));    // Stop compare interrupts
1033 #else
1034   ETIMSK &= ~((1<<OCIE3A) | (1<<OCIE3B) | (1<<OCIE3C));    // Stop compare interrupts
1035 #endif
1036   // TODO G: This can't work with V3.2/4.x boards. Select and use a different pin
1037   //         for secondary 8-ch PPM output (PORTH or Spare1 or Spare2 maybe?)
1038   DDRE &= ~0x80;  PORTE |= 0x80;     // Bit 7 input + pullup
1039
1040   TCCR3B = 0;                        // Stop counter
1041   TCCR3A = 0;
1042   // Noise Canceller enabled, neg. edge, clock at 16MHz / 8 (2MHz) (Correct for PCB V4.x+ also)
1043   TCCR3B  = (1<<ICNC3) | (0b010 << CS30);
1044
1045   RESUME_PPMIN_INTERRUPT();
1046 #endif
1047 }
1048
1049 void set_timer3_ppm()
1050 {
1051 #ifndef SIMU
1052   PAUSE_PPMIN_INTERRUPT();
1053
1054   DDRE |= 0x80;                                       // Bit 7 output
1055
1056   TCCR3B = 0;                        // Stop counter
1057   TCCR3A = (0<<WGM10);
1058   TCCR3B = (1 << WGM12) | (2<<CS10); // CTC OCR1A, 16MHz / 8
1059
1060 #if defined (CPUM2560) || defined(CPUM2561)
1061   TIMSK3 |= ((1<<OCIE3A) | (1<<OCIE3B));                    // enable immediately before mainloop
1062 #else
1063   ETIMSK |= ((1<<OCIE3A) | (1<<OCIE3B));                    // enable immediately before mainloop
1064 #endif
1065 #endif
1066 }
1067
1068 #ifndef SIMU
1069
1070 // G: TIMER3_COMPA and COMPB int. vectors are used for PPM16 (8+8, actually)
1071 // and PPMSIM modes
1072 ISR(TIMER3_COMPA_vect) //2MHz pulse generation
1073 {
1074   static uint8_t pulsePol;
1075   static uint16_t * pulse2MHzPPM16RPtr = (uint16_t*) &pulses2MHz[PULSES_SIZE/2];
1076
1077   if (pulsePol) {
1078     PORTE |= 0x80; // (1<<OUT_B_PPM);
1079     pulsePol = 0;
1080   }
1081   else {
1082     PORTE &= ~0x80; // (1<<OUT_B_PPM);
1083     pulsePol = 1;
1084   }
1085
1086   OCR3A = *pulse2MHzPPM16RPtr++;
1087   OCR3B = B3_comp_value;
1088
1089   if (*pulse2MHzPPM16RPtr == 0) {
1090     pulse2MHzPPM16RPtr = (uint16_t*) &pulses2MHz[PULSES_SIZE/2];
1091     pulsePol = g_model.pulsePol;
1092   }
1093
1094   heartbeat |= HEART_TIMER_PULSES;
1095 }
1096
1097 ISR(TIMER3_COMPB_vect) //2MHz pulse generation
1098 {
1099   sei();
1100   if (s_current_protocol[0] != g_model.protocol) {
1101     if (s_current_protocol[0] == PROTO_PPMSIM) {
1102       setupPulses();
1103     }
1104   }
1105   else {
1106     setupPulsesPPM(g_model.protocol);
1107   }
1108 }
1109
1110 #endif // SIMU
1111