Multiprotocol/RadioLink_cc2500.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 // Radiolink surface protocol. TXs: RC4GS,RC6GS. Compatible RXs:R7FG(Std),R6FG,R6F,R8EF,R8FM,R8F,R4FGM
  16
  17 #if defined(RLINK_CC2500_INO)
  18
  19 #include "iface_cc2500.h"
  20
  21 //#define RLINK_DEBUG
  22 //#define RLINK_DEBUG_TELEM
  23
  24 //#define RLINK_FORCE_ID
  25 //#define RLINK_RC4G_FORCE_ID
  26
  27 #define RLINK_TX_PACKET_LEN     33
  28 #define RLINK_RX_PACKET_LEN     15
  29 #define RLINK_TX_ID_LEN         4
  30 #define RLINK_HOP                       16
  31
  32 enum {
  33         RLINK_DATA      = 0x00,
  34         RLINK_RX1       = 0x01,
  35         RLINK_RX2       = 0x02,
  36 };
  37
  38 uint32_t RLINK_rand1;
  39 uint32_t RLINK_rand2;
  40
  41 static uint32_t __attribute__((unused)) RLINK_prng_next(uint32_t r)
  42 {
  43         return 0xA5E2A705 * r + 0x754DB79B;
  44 }
  45
  46 static void __attribute__((unused)) RLINK_init_random(uint32_t id)
  47 {
  48         uint32_t result = id;
  49
  50         RLINK_rand2 = result;
  51         for (uint8_t i=0; i<31; i++)
  52                 result = RLINK_prng_next(result);
  53         RLINK_rand1 = result;
  54 }
  55
  56 static uint8_t __attribute__((unused)) RLINK_next_random_swap()
  57 {
  58         uint8_t result = (RLINK_rand2 >> 16) + RLINK_rand2 + (RLINK_rand1 >> 16) + RLINK_rand1;
  59
  60         RLINK_rand2 = RLINK_prng_next(RLINK_rand2);
  61         RLINK_rand1 = RLINK_prng_next(RLINK_rand1);
  62
  63         return result & 0x0F;
  64 }
  65
  66 static uint32_t __attribute__((unused)) RLINK_compute_start_id(uint32_t id)
  67 {
  68         return id * 0xF65EF9F9u + 0x2EDDF6CAu;
  69 }
  70
  71 static void __attribute__((unused)) RLINK_shuffle_freqs(uint32_t seed)
  72 {
  73         RLINK_init_random(seed);
  74
  75         for(uint8_t i=0; i<RLINK_HOP; i++)
  76         {
  77                 uint8_t r   = RLINK_next_random_swap();
  78                 uint8_t tmp = hopping_frequency[r];
  79                 hopping_frequency[r] = hopping_frequency[i];
  80                 hopping_frequency[i] = tmp;
  81         }
  82 }
  83
  84 static void __attribute__((unused)) RLINK_hop()
  85 {
  86         uint8_t inc=3*(rx_tx_addr[0]&3);
  87
  88         // init hop table
  89         for(uint8_t i=0; i<RLINK_HOP; i++)
  90                 hopping_frequency[i] = (12*i) + inc;
  91
  92         // shuffle
  93         RLINK_shuffle_freqs(RLINK_compute_start_id(rx_tx_addr[0] + (rx_tx_addr[1] << 8)));
  94         RLINK_shuffle_freqs(RLINK_compute_start_id(rx_tx_addr[2] + (rx_tx_addr[3] << 8)));
  95
  96         // replace one of the channel randomely
  97         rf_ch_num=random(0xfefefefe)%0x11;              // 0x00..0x10
  98         if(inc==9) inc=6;                                               // frequency exception
  99         hopping_frequency[rf_ch_num]=12*16+inc;
 100 }
 101
 102 static void __attribute__((unused)) RLINK_TXID_init()
 103 {
 104         #ifdef RLINK_RC4G_FORCE_ID
 105                 //TODO: test any ID
 106                 if(sub_protocol==RLINK_RC4G)
 107                 {
 108                         rx_tx_addr[1]=0x77;
 109                         rx_tx_addr[2]=0x00;
 110                         rx_tx_addr[3]=0x00;
 111                 }
 112         #endif
 113         #ifdef RLINK_FORCE_ID
 114                 if(sub_protocol==RLINK_SURFACE)
 115                         memcpy(rx_tx_addr,"\x3A\x99\x22\x3A",RLINK_TX_ID_LEN);  //surface RC6GS
 116                 else
 117                         memcpy(rx_tx_addr,"\xFC\x11\x0D\x20",RLINK_TX_ID_LEN);  //air T8FB
 118         #endif
 119         // channels order depend on ID
 120         if(sub_protocol!=RLINK_RC4G)
 121                 RLINK_hop();
 122         else
 123         {//RLINK_RC4G
 124                 // Find 2 unused channels
 125                 //  first  channel is a multiple of 3 between 00 and 5D
 126                 //  second channel is a multiple of 3 between 63 and BD
 127                 CC2500_Strobe(CC2500_SIDLE);
 128                 CC2500_WriteReg(CC2500_17_MCSM1,0x3C);
 129                 CC2500_Strobe(CC2500_SFRX);
 130                 CC2500_SetTxRxMode(RX_EN);
 131                 CC2500_Strobe(CC2500_SRX);
 132                 delayMilliseconds(1);                                           //wait for RX mode
 133                 uint16_t val;
 134                 uint8_t val_low = 0xFF;
 135                 hopping_frequency[0] = 0x00;
 136                 hopping_frequency[1] = 0x63;
 137                 for(uint8_t ch=0; ch<=0xBD; ch+=3)
 138                 {
 139                         if(ch==0x63)
 140                                 val_low = 0xFF;                                         //init for second block
 141                         if(ch==0x60)
 142                                 continue;                                                       //skip channel
 143                         CC2500_WriteReg(CC2500_0A_CHANNR, ch);  //switch channel
 144                         delayMicroseconds(370);                                 //wait to read
 145                         val = 0;
 146                         for(uint8_t i=0;i<16;i++)
 147                                 val += CC2500_ReadReg(CC2500_34_RSSI | CC2500_READ_BURST);
 148                         val >>= 4;
 149                         debug("C:%02X RSSI:%02X",ch,val);
 150                         if(val_low > val)
 151                         {
 152                                 debug(" OK");
 153                                 val_low = val;
 154                                 hopping_frequency[ch<0x63?0:1]=ch;      //save best channel
 155                         }
 156                         debugln("");
 157                 }
 158                 CC2500_WriteReg(CC2500_17_MCSM1,0x30);
 159                 CC2500_Strobe(CC2500_SIDLE);
 160                 CC2500_SetTxRxMode(TX_EN);
 161                 #ifdef RLINK_RC4G_FORCE_ID
 162                         hopping_frequency[0] = 0x03;
 163                         hopping_frequency[1] = 0x6F;
 164                 #endif
 165         }
 166
 167         #ifdef RLINK_DEBUG
 168                 debug("ID:");
 169                 for(uint8_t i=0;i<RLINK_TX_ID_LEN;i++)
 170                         debug(" 0x%02X",rx_tx_addr[i]);
 171                 debugln("");
 172                 debug("Hop(%d):", rf_ch_num);
 173                 for(uint8_t i=0;i<RLINK_HOP;i++)
 174                         debug(" 0x%02X",hopping_frequency[i]);
 175                 debugln("");
 176         #endif
 177  }
 178
 179 const PROGMEM uint8_t RLINK_init_values[] = {
 180   /* 00 */ 0x5B, 0x06, 0x5C, 0x07, 0xAB, 0xCD, 0x40, 0x04,
 181   /* 08 */ 0x45, 0x00, 0x00, 0x06, 0x00, 0x5C, 0x62, 0x76,
 182   /* 10 */ 0x7A, 0x7F, 0x13, 0x23, 0xF8, 0x44, 0x07, 0x30,
 183   /* 18 */ 0x18, 0x16, 0x6C, 0x43, 0x40, 0x91, 0x87, 0x6B,
 184   /* 20 */ 0xF8, 0x56, 0x10, 0xA9, 0x0A, 0x00, 0x11
 185 };
 186
 187 static void __attribute__((unused)) RLINK_rf_init()
 188 {
 189         CC2500_Strobe(CC2500_SIDLE);
 190
 191         for (uint8_t i = 0; i < 39; ++i)
 192                 CC2500_WriteReg(i, pgm_read_byte_near(&RLINK_init_values[i]));
 193
 194         if(sub_protocol==RLINK_DUMBORC)
 195         {
 196                 CC2500_WriteReg(4, 0xBA);
 197                 CC2500_WriteReg(5, 0xDC);
 198         }
 199         else if(sub_protocol==RLINK_RC4G)
 200                 CC2500_WriteReg(5, 0xA5);
 201
 202         CC2500_WriteReg(CC2500_0C_FSCTRL0, option);
 203
 204         CC2500_SetTxRxMode(TX_EN);
 205 }
 206
 207 static void __attribute__((unused)) RLINK_send_packet()
 208 {
 209         static uint32_t pseudo=0;
 210         uint32_t bits = 0;
 211         uint8_t bitsavailable = 0;
 212         uint8_t idx = 6;
 213
 214         CC2500_Strobe(CC2500_SIDLE);
 215
 216         // packet length
 217         packet[0] = RLINK_TX_PACKET_LEN;
 218         // header
 219         if(packet_count>3)
 220                 packet[1] = 0x02;                                       // 0x02 telemetry request flag
 221         else
 222                 packet[1] = 0x00;                                       // no telemetry
 223
 224         switch(sub_protocol)
 225         {
 226                 case RLINK_SURFACE:
 227                         packet[1] |= 0x01;
 228                         //radiolink additionnal ID which is working only on a small set of RXs
 229                         //if(RX_num) packet[1] |= ((RX_num+2)<<4)+4;    // RX number limited to 10 values, 0 is a wildcard
 230                         break;
 231                 case RLINK_AIR:
 232                         packet[1] |= 0x21;                                      //air 0x21 on dump but it looks to support telemetry at least RSSI
 233                         break;
 234                 case RLINK_DUMBORC:
 235                         packet[1]  = 0x00;                                      //always 0x00 on dump
 236                         break;
 237         }
 238
 239         // ID
 240         memcpy(&packet[2],rx_tx_addr,RLINK_TX_ID_LEN);
 241
 242         // pack 16 channels on 11 bits values between 170 and 1876, 1023 middle. The last 8 channels are failsafe values associated to the first 8 values.
 243         for (uint8_t i = 0; i < 16; i++)
 244         {
 245                 uint32_t val = convert_channel_16b_nolimit(i,170,1876,false);           // allow extended limits
 246                 if (val & 0x8000)
 247                         val = 0;
 248                 else if (val > 2047)
 249                         val=2047;
 250
 251                 bits |= val << bitsavailable;
 252                 bitsavailable += 11;
 253                 while (bitsavailable >= 8) {
 254                         packet[idx++] = bits & 0xff;
 255                         bits >>= 8;
 256                         bitsavailable -= 8;
 257                 }
 258         }
 259
 260         // hop
 261         pseudo=((pseudo * 0xAA) + 0x03) % 0x7673;       // calc next pseudo random value
 262         CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[pseudo & 0x0F]);
 263         packet[28]= pseudo;
 264         packet[29]= pseudo >> 8;
 265         packet[30]= 0x00;                                               // unknown
 266         packet[31]= 0x00;                                               // unknown
 267         packet[32]= rf_ch_num;                                  // index of value changed in the RF table
 268
 269         // check
 270         uint8_t sum=0;
 271         for(uint8_t i=1;i<33;i++)
 272                 sum+=packet[i];
 273         packet[33]=sum;
 274
 275         // send packet
 276         CC2500_WriteData(packet, RLINK_TX_PACKET_LEN+1);
 277
 278         // packets type
 279         packet_count++;
 280         if(packet_count>5) packet_count=0;
 281
 282         #ifdef RLINK_DEBUG
 283                 debugln("C= 0x%02X",hopping_frequency[pseudo & 0x0F]);
 284                 debug("P=");
 285                 for(uint8_t i=1;i<RLINK_TX_PACKET_LEN+1;i++)
 286                         debug(" 0x%02X",packet[i]);
 287                 debugln("");
 288         #endif
 289 }
 290
 291 #ifndef MULTI_AIR
 292 static void __attribute__((unused)) RLINK_RC4G_send_packet()
 293 {
 294         uint32_t val;
 295         //hop
 296         CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[packet_count>>1]);
 297         #ifdef RLINK_DEBUG
 298                 debug("C= 0x%02X ",hopping_frequency[packet_count>>1]);
 299         #endif
 300         // packet length
 301         packet[0] = 0x0F;
 302         //address
 303         memcpy(&packet[1], &rx_tx_addr[1], 3);
 304         //channels
 305         for(uint8_t i=0;i<2;i++)
 306         {
 307                 val = Channel_data[2*i  ] +400 -24;
 308                 packet[4+i*2] = val;
 309                 packet[8+i  ] = val>>8;
 310                 val = Channel_data[2*i+1] +400 -24;
 311                 packet[5+i*2] = val;
 312                 packet[8+i  ] |= (val>>4) & 0xF0;
 313         }
 314         //special channel which is linked to gyro on the orginal TX but allocating it on CH5 here
 315         packet[10] = convert_channel_16b_limit(CH5,0,100);
 316         //failsafe
 317         for(uint8_t i=0;i<4;i++)
 318                 packet[11+i] = convert_channel_16b_limit(CH6+i,0,200);
 319         //next hop
 320         packet_count++;
 321         packet_count &= 0x03;
 322         packet[15] = hopping_frequency[packet_count>>1];
 323         // send packet
 324         CC2500_WriteData(packet, 16);
 325
 326         #ifdef RLINK_DEBUG
 327                 debug("P=");
 328                 for(uint8_t i=1;i<16;i++)
 329                         debug(" 0x%02X",packet[i]);
 330                 debugln("");
 331         #endif
 332 }
 333 #endif
 334
 335 #define RLINK_TIMING_PROTO      20000-100               // -100 for compatibility with R8EF
 336 #define RLINK_TIMING_RFSEND     10500
 337 #define RLINK_TIMING_CHECK      2000
 338 #define RLINK_RC4G_TIMING_PROTO 14460
 339 uint16_t RLINK_callback()
 340 {
 341         if(sub_protocol == RLINK_RC4G)
 342         {
 343                 #ifndef MULTI_AIR
 344                         #ifdef MULTI_SYNC
 345                                 telemetry_set_input_sync(RLINK_RC4G_TIMING_PROTO);
 346                         #endif
 347                         CC2500_SetPower();
 348                         CC2500_SetFreqOffset();
 349                         RLINK_RC4G_send_packet();
 350                 #else
 351                         SUB_PROTO_INVALID;
 352                 #endif
 353                 return RLINK_RC4G_TIMING_PROTO;
 354         }
 355         switch(phase)
 356         {
 357                 case RLINK_DATA:
 358                         #ifdef MULTI_SYNC
 359                                 telemetry_set_input_sync(RLINK_TIMING_PROTO);
 360                         #endif
 361                         CC2500_SetPower();
 362                         CC2500_SetFreqOffset();
 363                         RLINK_send_packet();
 364 #if not defined RLINK_HUB_TELEMETRY
 365                         return RLINK_TIMING_PROTO;
 366 #else
 367                         if(!(packet[1]&0x02))
 368                                 return RLINK_TIMING_PROTO;                                      //Normal packet
 369                                                                                                                         //Telemetry packet
 370                         phase++;                                                                                // RX1
 371                         return RLINK_TIMING_RFSEND;
 372                 case RLINK_RX1:
 373                         CC2500_Strobe(CC2500_SIDLE);
 374                         CC2500_Strobe(CC2500_SFRX);
 375                         CC2500_SetTxRxMode(RX_EN);
 376                         CC2500_Strobe(CC2500_SRX);
 377                         phase++;                                                                                // RX2
 378                         return RLINK_TIMING_PROTO-RLINK_TIMING_RFSEND-RLINK_TIMING_CHECK;
 379                 case RLINK_RX2:
 380                         len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 381                         if (len == RLINK_RX_PACKET_LEN + 1 + 2)                 //Telemetry frame is 15 bytes + 1 byte for length + 2 bytes for RSSI&LQI&CRC
 382                         {
 383                                 #ifdef RLINK_DEBUG_TELEM
 384                                         debug("Telem:");
 385                                 #endif
 386                                 CC2500_ReadData(packet_in, len);
 387                                 if(packet_in[0]==RLINK_RX_PACKET_LEN && (packet_in[len-1] & 0x80) && memcmp(&packet[2],rx_tx_addr,RLINK_TX_ID_LEN)==0 && packet_in[6]==packet[1])
 388                                 {//Correct telemetry received: length, CRC, ID and type
 389                                         #ifdef RLINK_DEBUG_TELEM
 390                                                 for(uint8_t i=0;i<len;i++)
 391                                                         debug(" %02X",packet_in[i]);
 392                                         #endif
 393                                         TX_RSSI = packet_in[len-2];
 394                                         if(TX_RSSI >=128)
 395                                                 TX_RSSI -= 128;
 396                                         else
 397                                                 TX_RSSI += 128;
 398                                         RX_RSSI=packet_in[7]&0x7F;                              //Should be packet_in[7]-256 but since it's an uint8_t...
 399                                         v_lipo1=packet_in[8]<<1;                                //RX Batt
 400                                         v_lipo2=packet_in[9];                                   //Batt
 401                                         telemetry_link=1;                                               //Send telemetry out
 402                                         pps_counter++;
 403                                         packet_count=0;
 404                                 }
 405                                 #ifdef RLINK_DEBUG_TELEM
 406                                         debugln("");
 407                                 #endif
 408                         }
 409                         if (millis() - pps_timer >= 2000)
 410                         {//1 telemetry packet every 100ms
 411                                 pps_timer = millis();
 412                                 if(pps_counter<20)
 413                                         pps_counter*=5;
 414                                 else
 415                                         pps_counter=100;
 416                                 debugln("%d pps", pps_counter);
 417                                 TX_LQI = pps_counter;                                           //0..100%
 418                                 pps_counter = 0;
 419                         }
 420                         CC2500_SetTxRxMode(TX_EN);
 421                         phase=RLINK_DATA;                                                               // DATA
 422                         return RLINK_TIMING_CHECK;
 423 #endif
 424         }
 425         return 0;
 426 }
 427
 428 void RLINK_init()
 429 {
 430         BIND_DONE;      // Not a TX bind protocol
 431         RLINK_TXID_init();
 432         RLINK_rf_init();
 433         packet_count = 0;
 434         phase = RLINK_DATA;
 435 }
 436
 437 #endif