Multiprotocol/WFLY2_a7105.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
  16 #if defined(WFLY2_A7105_INO)
  17
  18 #include "iface_a7105.h"
  19
  20 //#define WFLY2_FORCE_ID
  21
  22 //WFLY2 constants & variables
  23 #define WFLY2_BIND_COUNT                2777    // abort bind after 10sec
  24 #define WFLY2_PACKET_SIZE               32
  25
  26 enum{
  27         WFLY2_DATA,
  28         WFLY2_PLL_TX,
  29         WFLY2_RX,
  30 };
  31
  32 static void __attribute__((unused)) WFLY2_build_packet()
  33 {
  34         static uint16_t pseudo=0;
  35
  36         //End bind
  37         if(IS_BIND_IN_PROGRESS && bind_counter)
  38         {
  39                 bind_counter--;
  40                 if (bind_counter==0)
  41                 {
  42                         BIND_DONE;
  43                         A7105_WriteID(MProtocol_id);
  44                         rf_ch_num = 0;
  45                 }
  46         }
  47
  48         memset(packet,0x00,WFLY2_PACKET_SIZE);
  49
  50         if(IS_BIND_IN_PROGRESS)
  51         {
  52                 //Header
  53                 packet[0] = 0x0F;                       // Bind packet
  54
  55                 //ID
  56                 packet[1] = rx_tx_addr[3];
  57                 packet[2] = rx_tx_addr[2];
  58                 packet[3] = rx_tx_addr[1];
  59                 //packet[4] = 0x00;                     // Should be rx_tx_addr[0]&0x0F but bind is already using 0x00 so ....
  60
  61                 //Unknown
  62                 packet[5] = 0x01;
  63
  64                 //Freq
  65                 rf_ch_num = (hopping_frequency_no<<1)+0x08;
  66                 packet[6] = rf_ch_num;
  67                 hopping_frequency_no++;
  68                 if(hopping_frequency_no > 0x17) hopping_frequency_no=0x00;
  69
  70                 //Unknown bytes 7..31
  71         }
  72         else
  73         {
  74                 //Pseudo
  75                 uint16_t high_bit=(pseudo & 0x8000) ^ 0x8000;                                                   // toggle 0x8000 every other line
  76                 pseudo <<= 1;                                                                                                                   // *2
  77                 if( (pseudo & 0x8000) || pseudo == 0 ) pseudo ^= 0x8A87;                                // Randomisation, pseudo==0 is a guess but would give the start value seen on the dump when P[2]P[1]=0 at init and will prevent a lock up
  78                 pseudo |= high_bit;                                                                                                             // include toggle
  79                 packet[1] = pseudo;
  80                 packet[2] = pseudo>>8;
  81
  82                 //RF channel
  83                 int8_t prev = rf_ch_num & 0x1F;
  84                 rf_ch_num = (pseudo ^ (pseudo >> 7));
  85                 rf_ch_num = ((rf_ch_num>>1)&0x08) | (rf_ch_num & 0x47);
  86                 rf_ch_num = ((rf_ch_num>>2)&0x10) | (rf_ch_num & 0x1F);
  87                 rf_ch_num ^= rx_tx_addr[3] & 0x1F;
  88                 if(abs((int8_t)rf_ch_num-prev) <= 9)
  89                 {
  90                         if(high_bit)
  91                                 rf_ch_num |= 0x20;
  92                 }
  93                 else
  94                         if(!high_bit)
  95                                 rf_ch_num |= 0x20;
  96
  97                 //Partial ID
  98                 packet[3] = rx_tx_addr[3];
  99                 packet[4] = rx_tx_addr[2] & 0x03;
 100
 101                 //Header
 102                 if(prev_option!=option)
 103                 {//Set the RX PPM/WBUS on change
 104                         packet[0] = 0x05;       //PPM/WBUS packet
 105                         packet[5] = 0x01;
 106                         if(option)
 107                                 packet[6] = 0x01;       // PPM
 108                         else
 109                                 packet[6] = 0x00;       // WBUS
 110                         prev_option = option;
 111                 }
 112                 else
 113                 {//Normal or Failsafe packets
 114                         uint8_t offset=0;
 115
 116                         //packet[0] = 0x00;     // Normal packet
 117
 118                         #ifdef FAILSAFE_ENABLE
 119                                 #define WFLY2_NUM_FS_PKTS 2     //the original TX sends 4 but that's not needed...
 120                                 if(IS_FAILSAFE_VALUES_on && packet_sent >= WFLY2_NUM_FS_PKTS)   //Failsafe packet arrived from radio
 121                                         packet_sent = 0;                // send FS config packets
 122
 123                                 if(packet_sent < WFLY2_NUM_FS_PKTS)
 124                                 {// Send failsafe packets
 125                                         packet[0] = 0x01;               //Failsafe packet
 126                                         packet[5] = 0x08 | packet_sent;
 127                                         /*if(packet_sent > 1)   // needed when more than 2 FS packets are sent
 128                                         {
 129                                                 packet[5] |= 0x50;      // all channels in failsafe
 130                                                 packet[6]  = 0x55;      // all channels in failsafe
 131                                         }
 132                                         else*/
 133                                         {
 134                                                 uint8_t val=0;
 135                                                 for(uint8_t i = 0; i < 6; i++)
 136                                                 {
 137                                                         val >>= 2;
 138                                                         if(Failsafe_data[i+packet_sent*6] == FAILSAFE_CHANNEL_NOPULSES)         //no pulse value = 2
 139                                                                 val |= 0x80;
 140                                                         else if(Failsafe_data[i+packet_sent*6] != FAILSAFE_CHANNEL_HOLD)        //hold value = 0
 141                                                                 val |= 0x40;                                                                                                    //fs value = 1
 142                                                         //debug("ch%d=%04X, val=%02X | ",i+1+packet_sent*6, Failsafe_data[i+packet_sent*6],val);
 143                                                         if(i==1)
 144                                                                 packet[5] |= val;
 145                                                 }
 146                                                 packet[6] = val;
 147                                         }
 148                                         offset=2;
 149                                         packet_sent++;
 150                                         //debugln("5=%02X, 6=%02X", packet[5], packet[6]);
 151                                 }
 152                         #endif
 153
 154                         //10 channels -100%=0x2C1...0%=0x800...+100%=0xD3F
 155                         for(uint8_t i = 0; i < 5; i++)
 156                         {
 157                                 uint16_t temp=convert_channel_16b_nolimit(i*2 , 0x2C1, 0xD3F, IS_FAILSAFE_VALUES_on);
 158                                 packet[5 + offset + i*3]  = temp&0xFF;          // low byte
 159                                 packet[7 + offset + i*3]  = (temp>>8)&0x0F;     // high byte
 160                                 temp=convert_channel_16b_nolimit(i*2+1, 0x2C1, 0xD3F, IS_FAILSAFE_VALUES_on);
 161                                 packet[6 + offset + i*3]  = temp&0xFF;          // low byte
 162                                 packet[7 + offset + i*3] |= (temp>>4)&0xF0;     // high byte
 163                         }
 164
 165                         #ifdef FAILSAFE_ENABLE
 166                                 if(packet_sent >= WFLY2_NUM_FS_PKTS)
 167                                         FAILSAFE_VALUES_off;
 168                         #endif
 169
 170                         //Unknown bytes 20+offset..31
 171                 }
 172         }
 173
 174         //Debug
 175         #if 0
 176                 debug("ch=%02X,%02X P=",rf_ch_num,(rf_ch_num<<1)+0x10);
 177                 for(uint8_t i=0; i<WFLY2_PACKET_SIZE; i++)
 178                         debug("%02X ", packet[i]);
 179                 debugln("");
 180         #endif
 181 }
 182
 183 #ifdef WFLY2_HUB_TELEMETRY
 184         static void __attribute__((unused)) WFLY2_Send_Telemetry()
 185         {
 186                 //Incoming packet values
 187                 v_lipo1=packet[3]<<1;           // RX_batt *10 in V
 188                 v_lipo2=packet[5]<<1;           // Ext_batt*10 in V
 189                 RX_RSSI=(255-packet[7])>>1;     // Looks to be the RX RSSI value direct from A7105
 190
 191                 // Read TX RSSI
 192                 TX_RSSI=255-A7105_ReadReg(A7105_1D_RSSI_THOLD);
 193
 194                 telemetry_counter++;                    // LQI counter
 195                 telemetry_link=1;
 196                 if(telemetry_lost)
 197                 {
 198                         telemetry_lost = 0;
 199                         packet_count = 100;
 200                         telemetry_counter = 100;
 201                 }
 202         }
 203 #endif
 204
 205 #define WFLY2_PACKET_PERIOD             3600    //3600
 206 #define WFLY2_BUFFER_TIME               1500    //1500
 207 #define WFLY2_WRITE_TIME                800             //942
 208
 209 uint16_t WFLY2_callback()
 210 {
 211         uint16_t start;
 212         uint8_t status;
 213
 214         #ifndef FORCE_WFLY2_TUNING
 215                 A7105_AdjustLOBaseFreq(1);
 216         #endif
 217         switch(phase)
 218         {
 219                 case WFLY2_DATA:
 220                         #ifdef MULTI_SYNC
 221                                 telemetry_set_input_sync(WFLY2_PACKET_PERIOD);
 222                         #endif
 223                         //Build data packet
 224                         WFLY2_build_packet();
 225
 226                         //Fill the TX buffer without sending
 227                         A7105_WriteData(WFLY2_PACKET_SIZE,0);
 228
 229                         #ifdef WFLY2_HUB_TELEMETRY
 230                                 //LQI calculation
 231                                 packet_count++;
 232                                 if(packet_count>=100)
 233                                 {
 234                                         packet_count=0;
 235                                         TX_LQI=telemetry_counter;
 236                                         if(telemetry_counter==0)
 237                                                 telemetry_lost = 1;
 238                                         telemetry_counter = 0;
 239                                 }
 240                         #endif
 241
 242                         phase++;        // WFLY2_PLL_TX
 243                         return WFLY2_BUFFER_TIME;
 244
 245                 case WFLY2_PLL_TX:
 246                         //Check RX status
 247                         status=A7105_ReadReg(A7105_00_MODE);
 248                         //debugln("S:%02X", status);
 249
 250                         //PLL
 251                         A7105_Strobe(A7105_PLL);
 252
 253                         //Read incoming packet even if bad/not present to not change too much the TX timing, might want to reorg the code...
 254                         A7105_ReadData(WFLY2_PACKET_SIZE);
 255
 256                         //Read telemetry
 257                         if((status & 0x21)==0)
 258                         { // Packet received and CRC OK
 259                                 //Debug
 260                                 #if 0
 261                                         debug("T:");
 262                                         for(uint8_t i=0; i<WFLY2_PACKET_SIZE-20; i++)           // Can't send the full telemetry at full speed
 263                                                 debug(" %02X", packet[i]);
 264                                         debugln("");
 265                                 #endif
 266
 267                                 if(IS_BIND_IN_PROGRESS)
 268                                 {
 269                                         if(packet[0]==0x0F && packet[1]==rx_tx_addr[3] && packet[2]==rx_tx_addr[2] && packet[3]==rx_tx_addr[1] && packet[4]==0x00)
 270                                         {
 271                                                 bind_counter=1;                                                                 // End bind
 272                                                 debugln("Bind done");
 273                                                 //packet[5..7] contains the RXID
 274                                         }
 275                                 }
 276                                 #ifdef WFLY2_HUB_TELEMETRY
 277                                         else
 278                                                 if(packet[0]==0 && packet[1]==rx_tx_addr[3] && packet[2]==(rx_tx_addr[2] & 0x03))
 279                                                 {//Packet match the ID
 280                                                                 WFLY2_Send_Telemetry();                                                 // Packet looks good do send telem to the radio
 281                                                 }
 282                                 #endif
 283                         }
 284
 285                         //Change RF channel
 286                         A7105_WriteReg(A7105_0F_PLL_I, (rf_ch_num<<1)+0x10);
 287
 288                         //Switch to TX
 289                         A7105_SetPower();
 290                         A7105_SetTxRxMode(TX_EN);
 291                         A7105_Strobe(A7105_TX);
 292
 293                         phase++;        // WFLY2_RX
 294                         return WFLY2_WRITE_TIME;
 295
 296                 case WFLY2_RX:
 297                         //Wait for TX completion
 298                         start=micros();
 299                         while ((uint16_t)((uint16_t)micros()-start) < 700)                              // Wait max 700µs
 300                                 if(!(A7105_ReadReg(A7105_00_MODE) & 0x01))
 301                                         break;
 302
 303                         //Switch to RX
 304                         A7105_SetTxRxMode(RX_EN);
 305                         A7105_Strobe(A7105_RX);
 306
 307                         phase = WFLY2_DATA;
 308                         return WFLY2_PACKET_PERIOD-WFLY2_WRITE_TIME-WFLY2_BUFFER_TIME;
 309         }
 310         return WFLY2_PACKET_PERIOD; // never reached, please the compiler
 311 }
 312
 313 void WFLY2_init()
 314 {
 315         A7105_Init();
 316
 317         #ifdef WFLY2_FORCE_ID
 318                 MProtocol_id = 0x50002313;      //Richard
 319                 //MProtocol_id = 0x50000223;    //Pascal
 320         #endif
 321         MProtocol_id &= 0x00FFFFFF;             // Since the bind ID starts with 50, let's keep only the last 3 bytes of the ID
 322         MProtocol_id |= 0x50000000;             // As recommended on the A7105 datasheet
 323         set_rx_tx_addr(MProtocol_id);   // Update the ID
 324
 325         if(IS_BIND_IN_PROGRESS)
 326                 A7105_WriteID(0x50FFFFFE);      // Bind ID
 327         else
 328                 A7105_WriteID(MProtocol_id);
 329
 330         hopping_frequency_no=0;
 331         rf_ch_num = 0;
 332         bind_counter = WFLY2_BIND_COUNT;
 333         packet_sent = 255;
 334         phase = WFLY2_DATA;
 335         #ifdef WFLY2_HUB_TELEMETRY
 336                 packet_count = 0;
 337                 telemetry_lost = 1;
 338         #endif
 339 }
 340 #endif