Multiprotocol/SLT_ccnrf.ino

   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.
   6
   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.
  11
  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 // Last sync with deviation main github branch
  16
  17 #if defined(SLT_CCNRF_INO)
  18
  19 #include "iface_nrf250k.h"
  20
  21 //#define SLT_Q200_FORCE_ID
  22
  23 // For code readability
  24 #define SLT_PAYLOADSIZE_V1              7
  25 #define SLT_PAYLOADSIZE_V1_4    5
  26 #define SLT_PAYLOADSIZE_V2              11
  27 #define SLT_NFREQCHANNELS               15
  28 #define SLT_TXID_SIZE                   4
  29 #define SLT_BIND_CHANNEL                0x50
  30
  31 enum{
  32         // flags going to packet[6] (Q200)
  33         FLAG_Q200_FMODE = 0x20,
  34         FLAG_Q200_VIDON = 0x10,
  35         FLAG_Q200_FLIP  = 0x08,
  36         FLAG_Q200_VIDOFF= 0x04,
  37 };
  38
  39 enum{
  40         // flags going to packet[6] (MR100 & Q100)
  41         FLAG_MR100_FMODE        = 0x20,
  42         FLAG_MR100_FLIP         = 0x04,
  43         FLAG_MR100_VIDEO        = 0x02,
  44         FLAG_MR100_PICTURE      = 0x01,
  45 };
  46
  47 enum {
  48         SLT_BUILD=0,
  49         SLT_DATA1,
  50         SLT_DATA2,
  51         SLT_DATA3,
  52         SLT_BIND1,
  53         SLT_BIND2,
  54 };
  55
  56 static void __attribute__((unused)) SLT_RF_init()
  57 {
  58         NRF250K_Init();
  59         NRF250K_SetTXAddr(rx_tx_addr, SLT_TXID_SIZE);
  60 }
  61
  62 static void __attribute__((unused)) SLT_set_freq(void)
  63 {
  64         // Frequency hopping sequence generation
  65         for (uint8_t i = 0; i < SLT_TXID_SIZE; ++i)
  66         {
  67                 uint8_t next_i = (i+1) % SLT_TXID_SIZE; // is & 3 better than % 4 ?
  68                 uint8_t base = i < 2 ? 0x03 : 0x10;
  69                 hopping_frequency[i*4 + 0]  = (rx_tx_addr[i] & 0x3f) + base;
  70                 hopping_frequency[i*4 + 1]  = (rx_tx_addr[i] >> 2) + base;
  71                 hopping_frequency[i*4 + 2]  = (rx_tx_addr[i] >> 4) + (rx_tx_addr[next_i] & 0x03)*0x10 + base;
  72                 hopping_frequency[i*4 + 3]  = (rx_tx_addr[i] >> 6) + (rx_tx_addr[next_i] & 0x0f)*0x04 + base;
  73         }
  74
  75         // Unique freq
  76         uint8_t max_freq=0x50;  //V1 and V2
  77         if(sub_protocol==Q200)
  78                 max_freq=45;
  79         for (uint8_t i = 0; i < SLT_NFREQCHANNELS; ++i)
  80         {
  81                 if(sub_protocol==Q200 && hopping_frequency[i] >= max_freq)
  82                         hopping_frequency[i] = hopping_frequency[i] - max_freq + 0x03;
  83                 uint8_t done = 0;
  84                 while (!done)
  85                 {
  86                         done = 1;
  87                         for (uint8_t j = 0; j < i; ++j)
  88                                 if (hopping_frequency[i] == hopping_frequency[j])
  89                                 {
  90                                         done = 0;
  91                                         hopping_frequency[i] += 7;
  92                                         if (hopping_frequency[i] >= max_freq)
  93                                                 hopping_frequency[i] = hopping_frequency[i] - max_freq + 0x03;
  94                                 }
  95                 }
  96         }
  97         #if 0
  98                 debug("CH:");
  99                 for (uint8_t i = 0; i < SLT_NFREQCHANNELS; ++i)
 100                         debug(" %02X", hopping_frequency[i]);
 101                 debugln();
 102         #endif
 103
 104         //Bind channel
 105         hopping_frequency[SLT_NFREQCHANNELS]=SLT_BIND_CHANNEL;
 106
 107         //Calib all channels
 108         NRF250K_HoppingCalib(SLT_NFREQCHANNELS+1);
 109 }
 110
 111 static void __attribute__((unused)) SLT_wait_radio()
 112 {
 113         if (packet_sent)
 114                 while (!NRF250K_IsPacketSent());
 115         packet_sent = 0;
 116 }
 117
 118 static void __attribute__((unused)) SLT_send_packet(uint8_t len)
 119 {
 120         SLT_wait_radio();
 121         NRF250K_WritePayload(packet, len);
 122         packet_sent = 1;
 123 }
 124
 125 static void __attribute__((unused)) SLT_build_packet()
 126 {
 127         static uint8_t calib_counter=0;
 128
 129         // Set radio channel - once per packet batch
 130         NRF250K_SetFreqOffset();        // Set frequency offset
 131         NRF250K_Hopping(hopping_frequency_no);
 132         if (++hopping_frequency_no >= SLT_NFREQCHANNELS)
 133                 hopping_frequency_no = 0;
 134
 135         // aileron, elevator, throttle, rudder, gear, pitch
 136         uint8_t e = 0; // byte where extension 2 bits for every 10-bit channel are packed
 137         for (uint8_t i = 0; i < 4; ++i)
 138         {
 139                 uint16_t v = convert_channel_10b(sub_protocol != SLT_V1_4 ? CH_AETR[i] : i, false);
 140                 if(sub_protocol>SLT_V2 && (i==CH2 || i==CH3) && sub_protocol != SLT_V1_4)
 141                         v=1023-v;       // reverse throttle and elevator channels for Q100/Q200/MR100 protocols
 142                 packet[i] = v;
 143                 e = (e >> 2) | (uint8_t) ((v >> 2) & 0xC0);
 144         }
 145         // Extra bits for AETR
 146         packet[4] = e;
 147
 148         //->V1_4CH stops here
 149
 150         // 8-bit channels
 151         packet[5] = convert_channel_8b(CH5);
 152         packet[6] = convert_channel_8b(CH6);
 153
 154         //->V1 stops here
 155
 156         if(sub_protocol==Q200)
 157                 packet[6] =  GET_FLAG(CH9_SW , FLAG_Q200_FMODE)
 158                                         |GET_FLAG(CH10_SW, FLAG_Q200_FLIP)
 159                                         |GET_FLAG(CH11_SW, FLAG_Q200_VIDON)
 160                                         |GET_FLAG(CH12_SW, FLAG_Q200_VIDOFF);
 161         else if(sub_protocol==MR100 || sub_protocol==Q100)
 162                 packet[6] =  GET_FLAG(CH9_SW , FLAG_MR100_FMODE)
 163                                         |GET_FLAG(CH10_SW, FLAG_MR100_FLIP)
 164                                         |GET_FLAG(CH11_SW, FLAG_MR100_VIDEO)    // Does not exist on the Q100 but...
 165                                         |GET_FLAG(CH12_SW, FLAG_MR100_PICTURE); // Does not exist on the Q100 but...
 166         packet[7]=convert_channel_8b(CH7);
 167         packet[8]=convert_channel_8b(CH8);
 168         packet[9]=0xAA;                         //normal mode for Q100/Q200, unknown for V2/MR100
 169         packet[10]=0x00;                        //normal mode for Q100/Q200, unknown for V2/MR100
 170         if((sub_protocol==Q100 || sub_protocol==Q200) && CH13_SW)
 171         {//Calibrate
 172                 packet[9]=0x77;                 //enter calibration
 173                 if(calib_counter>=20 && calib_counter<=25)      // 7 packets for Q100 / 3 packets for Q200
 174                         packet[10]=0x20;        //launch calibration
 175                 calib_counter++;
 176                 if(calib_counter>250) calib_counter=250;
 177         }
 178         else
 179                 calib_counter=0;
 180 }
 181
 182 static void __attribute__((unused)) SLT_send_bind_packet()
 183 {
 184         SLT_wait_radio();
 185         NRF250K_Hopping(SLT_NFREQCHANNELS);     //Bind channel
 186         BIND_IN_PROGRESS;                                       //Limit TX power to bind level
 187         NRF250K_SetPower();
 188         BIND_DONE;
 189         NRF250K_SetTXAddr((uint8_t *)"\x7E\xB8\x63\xA9", SLT_TXID_SIZE);
 190         memcpy((void*)packet,(void*)rx_tx_addr,SLT_TXID_SIZE);
 191         if(phase==SLT_BIND2)
 192                 SLT_send_packet(SLT_TXID_SIZE);
 193         else // SLT_BIND1
 194                 SLT_send_packet(SLT_PAYLOADSIZE_V2);
 195 }
 196
 197 #define SLT_TIMING_BUILD                1000
 198 #define SLT_V1_TIMING_PACKET    1000
 199 #define SLT_V1_4_TIMING_PACKET  1643
 200 #define SLT_V2_TIMING_PACKET    2042
 201 #define SLT_V1_TIMING_BIND2             1000
 202 #define SLT_V2_TIMING_BIND1             6507
 203 #define SLT_V2_TIMING_BIND2             2112
 204 uint16_t SLT_callback()
 205 {
 206         switch (phase)
 207         {
 208                 case SLT_BUILD:
 209                         //debugln_time("b ");
 210                         #ifdef MULTI_SYNC
 211                                 telemetry_set_input_sync(packet_period);
 212                         #endif
 213                         SLT_build_packet();
 214                         NRF250K_SetPower();                                     //Change power level
 215                         NRF250K_SetTXAddr(rx_tx_addr, SLT_TXID_SIZE);
 216                         phase++;
 217                         return SLT_TIMING_BUILD;
 218                 case SLT_DATA1:
 219                 case SLT_DATA2:
 220                         phase++;
 221                         SLT_send_packet(packet_length);
 222                         if(sub_protocol==SLT_V1)
 223                                 return SLT_V1_TIMING_PACKET;
 224                         if(sub_protocol==SLT_V1_4)
 225                         {
 226                                 phase++;                                                //Packets are sent two times only
 227                                 return SLT_V1_4_TIMING_PACKET;
 228                         }
 229                         //V2
 230                         return SLT_V2_TIMING_PACKET;
 231                 case SLT_DATA3:
 232                         SLT_send_packet(packet_length);
 233                         if (++packet_count >= 100)
 234                         {// Send bind packet
 235                                 packet_count = 0;
 236                                 if(sub_protocol==SLT_V1||sub_protocol==SLT_V1_4)
 237                                 {
 238                                         phase=SLT_BIND2;
 239                                         return SLT_V1_TIMING_BIND2;
 240                                 }
 241                                 //V2
 242                                 phase=SLT_BIND1;
 243                                 return SLT_V2_TIMING_BIND1;
 244                         }
 245                         else
 246                         {// Continue to send normal packets
 247                                 phase = SLT_BUILD;
 248                                 if(sub_protocol==SLT_V1)
 249                                         return 20000-SLT_TIMING_BUILD;
 250                                 if(sub_protocol==SLT_V1_4)
 251                                         return 18000-SLT_TIMING_BUILD-SLT_V1_4_TIMING_PACKET;
 252                                 //V2
 253                                 return 13730-SLT_TIMING_BUILD;
 254                         }
 255                 case SLT_BIND1:
 256                         SLT_send_bind_packet();
 257                         phase++;
 258                         return SLT_V2_TIMING_BIND2;
 259                 case SLT_BIND2:
 260                         SLT_send_bind_packet();
 261                         phase = SLT_BUILD;
 262                         if(sub_protocol==SLT_V1)
 263                                 return 20000-SLT_TIMING_BUILD-SLT_V1_TIMING_BIND2;
 264                         if(sub_protocol==SLT_V1_4)
 265                                 return 18000-SLT_TIMING_BUILD-SLT_V1_TIMING_BIND2-SLT_V1_4_TIMING_PACKET;
 266                         //V2
 267                         return 13730-SLT_TIMING_BUILD-SLT_V2_TIMING_BIND1-SLT_V2_TIMING_BIND2;
 268         }
 269         return 19000;
 270 }
 271
 272 void SLT_init()
 273 {
 274         BIND_DONE;      // Not a TX bind protocol
 275         packet_count = 0;
 276         packet_sent = 0;
 277         hopping_frequency_no = 0;
 278         if(sub_protocol==Q200)
 279         { //Q200: Force high part of the ID otherwise it won't bind
 280                 rx_tx_addr[0]=0x01;
 281                 rx_tx_addr[1]=0x02;
 282                 #ifdef SLT_Q200_FORCE_ID        // ID taken from TX dumps
 283                         rx_tx_addr[0]=0x01;rx_tx_addr[1]=0x02;rx_tx_addr[2]=0x6A;rx_tx_addr[3]=0x31;
 284                 /*      rx_tx_addr[0]=0x01;rx_tx_addr[1]=0x02;rx_tx_addr[2]=0x0B;rx_tx_addr[3]=0x57;*/
 285                 #endif
 286         }
 287         SLT_RF_init();
 288         SLT_set_freq();
 289         phase = SLT_BUILD;
 290         if(sub_protocol==SLT_V1)
 291         {
 292                 packet_length = SLT_PAYLOADSIZE_V1;
 293                 #ifdef MULTI_SYNC
 294                         packet_period = 20000+2*SLT_V1_TIMING_PACKET;           //22ms
 295                 #endif
 296         }
 297         else if(sub_protocol==SLT_V1_4)
 298         {
 299                 packet_length = SLT_PAYLOADSIZE_V1_4;
 300                 #ifdef MULTI_SYNC
 301                         packet_period = 18000;                                                          //18ms
 302                 #endif
 303                 //Test IDs
 304                 MProtocol_id = MProtocol_id_master ^ (1<<RX_num);
 305                 set_rx_tx_addr(MProtocol_id);
 306                 debugln("Try ID: %lx", MProtocol_id);
 307         }
 308         else //V2
 309         {
 310                 packet_length = SLT_PAYLOADSIZE_V2;
 311                 #ifdef MULTI_SYNC
 312                         packet_period = 13730+2*SLT_V2_TIMING_PACKET;           //~18ms
 313                 #endif
 314         }
 315 }
 316
 317 #endif
 318 //SLT v1_4ch timing
 319 //268363 + 1643 / 15 = 18000