Multiprotocol/Hubsan_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 // compatible with Hubsan H102D, H107/L/C/D and H107P/C+/D+
  16 // Last sync with hexfet new_protocols/hubsan_a7105.c dated 2015-12-11
  17
  18 #if defined(HUBSAN_A7105_INO)
  19
  20 #include "iface_a7105.h"
  21
  22 enum{
  23         // flags going to packet[9] (H107)
  24         HUBSAN_FLAG_VIDEO= 0x01,   // record video
  25         HUBSAN_FLAG_FLIP = 0x08,   // enable flips
  26         HUBSAN_FLAG_LED  = 0x04    // enable LEDs
  27 };
  28
  29 enum{
  30         // flags going to packet[9] (H107 Plus series)
  31         HUBSAN_FLAG_HEADLESS = 0x08, // headless mode
  32 };
  33
  34 enum{
  35         // flags going to packet[9] (H301)
  36         FLAG_H301_VIDEO = 0x01,
  37         FLAG_H301_STAB  = 0x02,
  38         FLAG_H301_LED   = 0x10,
  39         FLAG_H301_RTH   = 0x40,
  40 };
  41
  42 enum{
  43         // flags going to packet[13] (H107 Plus series)
  44         HUBSAN_FLAG_SNAPSHOT  = 0x01,
  45         HUBSAN_FLAG_FLIP_PLUS = 0x80,
  46 };
  47
  48 enum{
  49         // flags going to packet[9] (H501S)
  50         FLAG_H501_VIDEO     = 0x01,
  51         FLAG_H501_LED       = 0x04,
  52         FLAG_H122D_FLIP     = 0x08,     //H122D
  53         FLAG_H501_RTH       = 0x20,
  54         FLAG_H501_HEADLESS1 = 0x40,
  55         FLAG_H501_GPS_HOLD  = 0x80,
  56         };
  57
  58 enum{
  59         // flags going to packet[11] (H122D & H123D)
  60         FLAG_H123D_FMODES   = 0x03,     //H123D 3 FMODES: Sport mode 1, Sport mode 2, Acro
  61         FLAG_H122D_OSD      = 0x20,     //H122D OSD
  62 };
  63
  64 enum{
  65         // flags going to packet[13] (H501S)
  66         FLAG_H501_SNAPSHOT  = 0x01,
  67         FLAG_H501_HEADLESS2 = 0x02,
  68         FLAG_H501_ALT_HOLD  = 0x08,
  69 };
  70
  71 uint32_t hubsan_id_data;
  72
  73 enum {
  74         BIND_1,
  75         BIND_2,
  76         BIND_3,
  77         BIND_4,
  78         BIND_5,
  79         BIND_6,
  80         BIND_7,
  81         BIND_8,
  82         DATA_1,
  83         DATA_2,
  84         DATA_3,
  85         DATA_4,
  86         DATA_5,
  87 };
  88 #define HUBSAN_WAIT_WRITE 0x80
  89
  90 static void __attribute__((unused)) hubsan_update_crc()
  91 {
  92         uint8_t sum = 0;
  93         for(uint8_t i = 0; i < 15; i++)
  94                 sum += packet[i];
  95         packet[15] = (256 - (sum % 256)) & 0xFF;
  96 }
  97
  98 static void __attribute__((unused)) hubsan_build_bind_packet(uint8_t bind_state)
  99 {
 100         static uint8_t handshake_counter;
 101         if(phase < BIND_7)
 102                 handshake_counter = 0;
 103         memset(packet, 0, 16);
 104         packet[0] = bind_state;
 105         packet[1] = channel;
 106         packet[2] = (MProtocol_id >> 24) & 0xFF;
 107         packet[3] = (MProtocol_id >> 16) & 0xFF;
 108         packet[4] = (MProtocol_id >>  8) & 0xFF;
 109         packet[5] = (MProtocol_id >>  0) & 0xFF;
 110         if(hubsan_id_data == ID_NORMAL && sub_protocol != H501)
 111         {
 112                 packet[6] = 0x08;
 113                 packet[7] = 0xe4;
 114                 packet[8] = 0xea;
 115                 packet[9] = 0x9e;
 116                 packet[10] = 0x50;
 117                 //const uint32_t txid = 0xdb042679;
 118                 packet[11] = 0xDB;
 119                 packet[12] = 0x04;
 120                 packet[13] = 0x26;
 121                 packet[14] = 0x79;
 122         }
 123         else
 124         { //ID_PLUS
 125                 if(phase >= BIND_3)
 126                 {
 127                         packet[7] = 0x62;
 128                         packet[8] = 0x16;
 129                 }
 130                 if(phase == BIND_7)
 131                         packet[2] = handshake_counter++;
 132         }
 133         hubsan_update_crc();
 134 }
 135
 136 //cc : throttle  observed range: 0x00 - 0xFF (smaller is down)
 137 //ee : rudder    observed range: 0x34 - 0xcc (smaller is right)52-204-60%
 138 //gg : elevator  observed range: 0x3e - 0xbc (smaller is up)62-188 -50%
 139 //ii : aileron   observed range: 0x45 - 0xc3 (smaller is right)69-195-50%
 140 static void __attribute__((unused)) hubsan_build_packet()
 141 {
 142         static uint8_t vtx_freq = 0, h501_packet = 0;
 143         memset(packet, 0, 16);
 144         if(vtx_freq != option || packet_count==100) // set vTX frequency (H107D)
 145         {
 146                 vtx_freq = option;
 147                 packet[0] = 0x40;       // vtx data packet
 148                 packet[1] = (vtx_freq>0xF2)?0x17:0x16;
 149                 packet[2] = vtx_freq+0x0D;      // 5645 - 5900 MHz
 150                 packet[3] = 0x82;
 151                 packet_count++;
 152         }
 153         else //20 00 00 00 80 00 7d 00 84 02 64 db 04 26 79 7b
 154         {
 155                 packet[0] = 0x20;       // normal data packet
 156                 packet[2] = convert_channel_8b(THROTTLE);               //Throtle
 157         }
 158         packet[4] = 0xFF - convert_channel_8b(RUDDER);          //Rudder is reversed
 159         packet[6] = 0xFF - convert_channel_8b(ELEVATOR);        //Elevator is reversed
 160         packet[8] = convert_channel_8b(AILERON);                        //Aileron
 161         if(hubsan_id_data == ID_NORMAL && sub_protocol==H107)
 162         {// H107/L/C/D, H102D
 163                 if( packet_count < 100)
 164                 {
 165                         packet[9] = 0x02 | HUBSAN_FLAG_LED | HUBSAN_FLAG_FLIP; // sends default value for the 100 first packets
 166                         packet_count++;
 167                 }
 168                 else
 169                 {
 170                         packet[9] = 0x02;
 171                         // Channel 5
 172                         if(CH5_SW)      packet[9] |= HUBSAN_FLAG_FLIP;
 173                         // Channel 6
 174                         if(CH6_SW)      packet[9] |= HUBSAN_FLAG_LED;
 175                         // Channel 8
 176                         if(CH8_SW)      packet[9] |= HUBSAN_FLAG_VIDEO; // H102D
 177                 }
 178                 packet[10] = 0x64;
 179                 //const uint32_t txid = 0xdb042679;
 180                 packet[11] = 0xDB;
 181                 packet[12] = 0x04;
 182                 packet[13] = 0x26;
 183                 packet[14] = 0x79;
 184         } else  if(sub_protocol==H301)
 185         {// H301
 186                 if( packet_count < 100)
 187                 {
 188                         packet[9] = FLAG_H301_STAB; // sends default value for the 100 first packets
 189                         packet_count++;
 190                 }
 191                 else
 192                 {
 193             packet[9] = GET_FLAG(CH6_SW, FLAG_H301_LED)
 194                       | GET_FLAG(CH7_SW, FLAG_H301_STAB)
 195                       | GET_FLAG(CH8_SW, FLAG_H301_VIDEO)
 196                       | GET_FLAG(CH5_SW, FLAG_H301_RTH);
 197                 }
 198                 packet[10] = 0x18; // ?
 199                 packet[12] = 0x5c; // ?
 200                 packet[14] = 0xf6; // ?
 201         }
 202         else
 203         { //ID_PLUS && H501
 204                 packet[3] = sub_protocol==H501 ? 0x00:0x64;
 205                 packet[5] = sub_protocol==H501 ? 0x00:0x64;
 206                 packet[7] = sub_protocol==H501 ? 0x00:0x64;
 207
 208                 if(sub_protocol==H501)
 209                 { // H501S
 210                         packet[9] = 0x02
 211                                            | GET_FLAG(CH6_SW, FLAG_H501_LED)
 212                                            | GET_FLAG(CH8_SW, FLAG_H501_VIDEO)
 213                                            | GET_FLAG(CH12_SW, FLAG_H122D_FLIP) // H122D specific -> flip
 214                                            | GET_FLAG(CH5_SW, FLAG_H501_RTH)
 215                                            | GET_FLAG(CH10_SW, FLAG_H501_GPS_HOLD)
 216                                            | GET_FLAG(CH9_SW, FLAG_H501_HEADLESS1);
 217                         //packet[10]= 0x1A;
 218
 219                         //packet[11] content 0x00 is default
 220                         //H123D specific -> Flight modes
 221                         packet[11] = 0x41;      // Sport mode 1
 222                         if(Channel_data[CH13]>CHANNEL_MAX_COMMAND)
 223                                 packet[11]=0x43;        // Acro
 224                         else if(Channel_data[CH13]>CHANNEL_MIN_COMMAND)
 225                                 packet[11]=0x42;        // Sport mode 2
 226                         //H122D specific -> OSD but useless...
 227                         //packet[11]|= 0x80
 228                         //                | GET_FLAG(CHXX_SW,FLAG_H122D_OSD);
 229
 230                         packet[13] = GET_FLAG(CH9_SW, FLAG_H501_HEADLESS2)
 231                                            | GET_FLAG(CH11_SW, FLAG_H501_ALT_HOLD)
 232                                            | GET_FLAG(CH7_SW, FLAG_H501_SNAPSHOT);
 233                 }
 234                 else
 235                 { // H107P/C+/D+
 236                         packet[9] = 0x06;
 237                         //FLIP|LIGHT|PICTURE|VIDEO|HEADLESS
 238                         if(CH8_SW)      packet[9] |= HUBSAN_FLAG_VIDEO;
 239                         if(CH9_SW)      packet[9] |= HUBSAN_FLAG_HEADLESS;
 240                         packet[10]= 0x19;
 241                         packet[12]= 0x5C; // ghost channel ?
 242                         packet[13] = 0;
 243                         if(CH7_SW)      packet[13]  = HUBSAN_FLAG_SNAPSHOT;
 244                         if(CH5_SW)      packet[13] |= HUBSAN_FLAG_FLIP_PLUS;
 245                         packet[14]= 0x49; // ghost channel ?
 246                 }
 247                 if(packet_count < 100)
 248                 { // set channels to neutral for first 100 packets
 249                         packet[2] = 0x80; // throttle neutral is at mid stick on plus series
 250                         packet[4] = 0x80;
 251                         packet[6] = 0x80;
 252                         packet[8] = 0x80;
 253                         packet[9] = 0x06;
 254                         packet[13]= 0x00;
 255                         packet_count++;
 256                 }
 257                 if(sub_protocol==H501)
 258                 { // H501S
 259                         h501_packet++;
 260                         if(h501_packet == 10)
 261                         {
 262                                 memset(packet, 0, 16);
 263                                 packet[0] = 0xe8;
 264                         }
 265                         else if(h501_packet == 20)
 266                         {
 267                                 memset(packet, 0, 16);
 268                                 packet[0] = 0xe9;
 269                         }
 270                         if(h501_packet >= 20) h501_packet = 0;
 271                 }
 272         }
 273         hubsan_update_crc();
 274 }
 275
 276 #ifdef HUBSAN_HUB_TELEMETRY
 277 static uint8_t __attribute__((unused)) hubsan_check_integrity()
 278 {
 279     if( (packet[0]&0xFE) != 0xE0 )
 280                 return 0;
 281         uint8_t sum = 0;
 282     for(uint8_t i = 0; i < 15; i++)
 283         sum += packet[i];
 284         return ( packet[15] == (uint8_t)(-sum) );
 285 }
 286 #endif
 287
 288 uint16_t HUBSAN_callback()
 289 {
 290 #ifdef HUBSAN_HUB_TELEMETRY
 291         static uint8_t rfMode=0;
 292 #endif
 293         static uint8_t txState=0;
 294         uint16_t delay;
 295         uint8_t i;
 296
 297         #ifndef FORCE_HUBSAN_TUNING
 298                 A7105_AdjustLOBaseFreq(1);
 299         #endif
 300         switch(phase)
 301         {
 302                 case BIND_1:
 303                         bind_phase++;
 304                         if(bind_phase >= 20 && sub_protocol != H501)
 305                         {
 306                                 if(hubsan_id_data == ID_NORMAL)
 307                                         hubsan_id_data = ID_PLUS;
 308                                 else
 309                                         hubsan_id_data = ID_NORMAL;
 310                                 A7105_WriteID(hubsan_id_data);
 311                                 bind_phase = 0;
 312                         }
 313                 case BIND_3:
 314                 case BIND_5:
 315                 case BIND_7:
 316                         hubsan_build_bind_packet(phase == BIND_7 ? 9 : (phase == BIND_5 ? 1 : phase + 1 - BIND_1));
 317                         A7105_Strobe(A7105_STANDBY);
 318                         A7105_WriteData(16, channel);
 319                         phase |= HUBSAN_WAIT_WRITE;
 320                         return 3000;
 321                 case BIND_1 | HUBSAN_WAIT_WRITE:
 322                 case BIND_3 | HUBSAN_WAIT_WRITE:
 323                 case BIND_5 | HUBSAN_WAIT_WRITE:
 324                 case BIND_7 | HUBSAN_WAIT_WRITE:
 325                         //wait for completion
 326                         for(i = 0; i< 20; i++)
 327                                 if(! (A7105_ReadReg(A7105_00_MODE) & 0x01))
 328                                         break;
 329                         A7105_SetTxRxMode(RX_EN);
 330                         A7105_Strobe(A7105_RX);
 331                         phase &= ~HUBSAN_WAIT_WRITE;
 332                         if(hubsan_id_data == ID_PLUS)
 333                         {
 334                                 if(phase == BIND_7 && packet[2] == 9)
 335                                 {
 336                                         phase = DATA_1;
 337                                         A7105_WriteReg(A7105_1F_CODE_I, 0x0F);
 338                                         BIND_DONE;
 339                                         return 4500;
 340                                 }
 341                         }
 342                         phase++;
 343                         return 4500; //7.5msec elapsed since last write
 344                 case BIND_2:
 345                 case BIND_4:
 346                 case BIND_6:
 347                         A7105_SetTxRxMode(TX_EN);
 348                         if(A7105_ReadReg(A7105_00_MODE) & 0x01) {
 349                                 phase = BIND_1;
 350                                 return 4500; //No signal, restart binding procedure.  12msec elapsed since last write
 351                         }
 352                         A7105_ReadData(16);
 353                         phase++;
 354                         if (phase == BIND_5)
 355                                 A7105_WriteID(((uint32_t)packet[2] << 24) | ((uint32_t)packet[3] << 16) | ((uint32_t)packet[4] << 8) | packet[5]);
 356                         return 500;  //8msec elapsed time since last write;
 357                 case BIND_8:
 358                         A7105_SetTxRxMode(TX_EN);
 359                         if(A7105_ReadReg(A7105_00_MODE) & 0x01) {
 360                                 phase = BIND_7;
 361                                 return 15000; //22.5msec elapsed since last write
 362                         }
 363                         A7105_ReadData(16);
 364                         if(packet[1] == 9 && hubsan_id_data == ID_NORMAL) {
 365                                 phase = DATA_1;
 366                                 A7105_WriteReg(A7105_1F_CODE_I, 0x0F);
 367                                 BIND_DONE;
 368                                 return 28000; //35.5msec elapsed since last write
 369                         } else {
 370                                 phase = BIND_7;
 371                                 return 15000; //22.5 msec elapsed since last write
 372                         }
 373                 case DATA_1:
 374                 case DATA_2:
 375                 case DATA_3:
 376                 case DATA_4:
 377                 case DATA_5:
 378                         if( txState == 0) { // send packet
 379                         #ifdef MULTI_SYNC
 380                                 telemetry_set_input_sync(10000);
 381                         #endif
 382 #ifdef HUBSAN_HUB_TELEMETRY
 383                                 rfMode = A7105_TX;
 384 #endif
 385                                 if( phase == DATA_1)
 386                                                 A7105_SetPower(); //Keep transmit power in sync
 387                                 hubsan_build_packet();
 388                                 A7105_Strobe(A7105_STANDBY);
 389                                 uint8_t ch;
 390                                 if((phase == DATA_5 && hubsan_id_data == ID_NORMAL) && sub_protocol == H107)
 391                                         ch = channel + 0x23;
 392                                 else
 393                                         ch = channel;
 394                                 A7105_WriteData(16, ch);
 395                                 if (phase == DATA_5)
 396                                         phase = DATA_1;
 397                                 else
 398                                         phase++;
 399                                 delay=3000;
 400                         }
 401                         else {
 402 #ifdef HUBSAN_HUB_TELEMETRY
 403                                 if( rfMode == A7105_TX)
 404                                 {// switch to rx mode 3ms after packet sent
 405                                         for( i=0; i<10; i++)
 406                                         {
 407                                                 if( !(A7105_ReadReg(A7105_00_MODE) & 0x01)) {// wait for tx completion
 408                                                         A7105_SetTxRxMode(RX_EN);
 409                                                         A7105_Strobe(A7105_RX);
 410                                                         rfMode = A7105_RX;
 411                                                         break;
 412                                                 }
 413                                         }
 414                                 }
 415                                 if( rfMode == A7105_RX)
 416                                 { // check for telemetry frame
 417                                         for( i=0; i<10; i++)
 418                                         {
 419                                                 if( !(A7105_ReadReg(A7105_00_MODE) & 0x01))
 420                                                 { // data received
 421                                                         A7105_ReadData(16);
 422                                                         if( hubsan_check_integrity() )
 423                                                         {
 424                                                                 v_lipo1=packet[13]*2;// hubsan lipo voltage 8bits the real value is h_lipo/10(0x2A=42 -> 4.2V)
 425                                                                 telemetry_link=1;
 426                                                         }
 427                                                         A7105_Strobe(A7105_RX);
 428                                                         // Read TX RSSI
 429                                                         int16_t temp=256-(A7105_ReadReg(A7105_1D_RSSI_THOLD)*8)/5;              // value from A7105 is between 8 for maximum signal strength to 160 or less
 430                                                         if(temp<0) temp=0;
 431                                                         else if(temp>255) temp=255;
 432                                                         TX_RSSI=temp;
 433                                                         break;
 434                                                 }
 435                                         }
 436                                 }
 437 #endif
 438                                 delay=1000;
 439                         }
 440                         if (++txState == 8) { // 3ms + 7*1ms
 441                                 A7105_SetTxRxMode(TX_EN);
 442                                 txState = 0;
 443                         }
 444                         return delay;
 445         }
 446         return 0;
 447 }
 448
 449 void HUBSAN_init()
 450 {
 451         const uint8_t allowed_ch[] = {0x14, 0x1e, 0x28, 0x32, 0x3c, 0x46, 0x50, 0x5a, 0x64, 0x6e, 0x78, 0x82};
 452         A7105_Init();
 453
 454         channel = allowed_ch[MProtocol_id % sizeof(allowed_ch)];
 455         hubsan_id_data=ID_NORMAL;
 456
 457         if(IS_BIND_IN_PROGRESS || sub_protocol==H107)
 458         {
 459                 BIND_IN_PROGRESS;       // autobind protocol
 460                 phase = BIND_1;
 461         }
 462         else
 463         {
 464                 phase = DATA_1;
 465                 A7105_WriteID(MProtocol_id);
 466                 A7105_WriteReg(A7105_1F_CODE_I, 0x0F);
 467         }
 468         packet_count=0;
 469         bind_phase=0;
 470 }
 471
 472 #endif