set packet[13] |= 0x03;

flysky_a7105.c

/*
 This project is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 Deviation is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with Deviation.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifdef MODULAR
  //Allows the linker to properly relocate
  #define FLYSKY_Cmds PROTO_Cmds
  #pragma long_calls
#endif

#include "common.h"
#include "interface.h"
#include "mixer.h"
#include "config/model.h"
#include "config/tx.h"
#include "telemetry.h"

#ifdef MODULAR
  #pragma long_calls_off
  extern unsigned _data_loadaddr;
  const unsigned long protocol_type = (unsigned long)&_data_loadaddr;
#endif

#ifdef PROTO_HAS_A7105

//Fewer bind packets in the emulator so we can get right to the important bits
#ifdef EMULATOR
#define BIND_COUNT 3
#else
#define BIND_COUNT 2500
#endif

static const char * const flysky_opts[] = {
  "WLToys ext.",  _tr_noop("Off"), _tr_noop("V9x9"), _tr_noop("V6x6"), _tr_noop("V91x"), NULL,
  NULL
};
enum {
    PROTOOPTS_WLTOYS = 0,
    LAST_PROTO_OPT,
};
ctassert(LAST_PROTO_OPT <= NUM_PROTO_OPTS, too_many_protocol_opts);

#define WLTOYS_EXT_OFF 0
#define WLTOYS_EXT_V9X9 1
#define WLTOYS_EXT_V6X6 2
#define WLTOYS_EXT_V91X 3

static const u8 A7105_regs[] = {
     -1,  0x42, 0x00, 0x14, 0x00,  -1 ,  -1 , 0x00, 0x00, 0x00, 0x00, 0x01, 0x21, 0x05, 0x00, 0x50,
    0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x00, 0x62, 0x80, 0x80, 0x00, 0x0a, 0x32, 0xc3, 0x0f,
    0x13, 0xc3, 0x00,  -1,  0x00, 0x00, 0x3b, 0x00, 0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00,
    0x01, 0x0f,  -1,
};
static const u8 tx_channels[16][16] = {
  {0x0a, 0x5a, 0x14, 0x64, 0x1e, 0x6e, 0x28, 0x78, 0x32, 0x82, 0x3c, 0x8c, 0x46, 0x96, 0x50, 0xa0},
  {0xa0, 0x50, 0x96, 0x46, 0x8c, 0x3c, 0x82, 0x32, 0x78, 0x28, 0x6e, 0x1e, 0x64, 0x14, 0x5a, 0x0a},
  {0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x46, 0x96, 0x1e, 0x6e, 0x3c, 0x8c, 0x28, 0x78, 0x32, 0x82},
  {0x82, 0x32, 0x78, 0x28, 0x8c, 0x3c, 0x6e, 0x1e, 0x96, 0x46, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a},
  {0x28, 0x78, 0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96},
  {0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a, 0x78, 0x28},
  {0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96, 0x14, 0x64},
  {0x64, 0x14, 0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50},
  {0x50, 0xa0, 0x46, 0x96, 0x3c, 0x8c, 0x28, 0x78, 0x0a, 0x5a, 0x32, 0x82, 0x1e, 0x6e, 0x14, 0x64},
  {0x64, 0x14, 0x6e, 0x1e, 0x82, 0x32, 0x5a, 0x0a, 0x78, 0x28, 0x8c, 0x3c, 0x96, 0x46, 0xa0, 0x50},
  {0x46, 0x96, 0x3c, 0x8c, 0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64},
  {0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50, 0x8c, 0x3c, 0x96, 0x46},
  {0x46, 0x96, 0x0a, 0x5a, 0x3c, 0x8c, 0x14, 0x64, 0x50, 0xa0, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82},
  {0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0xa0, 0x50, 0x64, 0x14, 0x8c, 0x3c, 0x5a, 0x0a, 0x96, 0x46},
  {0x46, 0x96, 0x0a, 0x5a, 0x50, 0xa0, 0x3c, 0x8c, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64},
  {0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0x8c, 0x3c, 0xa0, 0x50, 0x5a, 0x0a, 0x96, 0x46},
};
static u32 id;
//static const u8 id[] = { 0x02, 0x00, 0x00, 0x70 };
static u8 chanrow;
static u8 chancol;
static u8 chanoffset;
static u8 packet[21];
static u16 counter;

static int flysky_init()
{
    printf("Hello goebish !\n");
    int i;
    u8 if_calibration1;
    u8 vco_calibration0;
    u8 vco_calibration1;

    A7105_WriteID(0x5475c52a);
    for (i = 0; i < 0x33; i++)
        if((s8)A7105_regs[i] != -1)
            A7105_WriteReg(i, A7105_regs[i]);

    A7105_Strobe(A7105_STANDBY);

    //IF Filter Bank Calibration
    A7105_WriteReg(0x02, 1);
    A7105_ReadReg(0x02);
    u32 ms = CLOCK_getms();
    CLOCK_ResetWatchdog();
    while(CLOCK_getms()  - ms < 500) {
        if(! A7105_ReadReg(0x02))
            break;
    }
    if (CLOCK_getms() - ms >= 500)
        return 0;
    if_calibration1 = A7105_ReadReg(0x22);
    if(if_calibration1 & A7105_MASK_FBCF) {
        //Calibration failed...what do we do?
        return 0;
    }

    //VCO Current Calibration
    A7105_WriteReg(0x24, 0x13); //Recomended calibration from A7105 Datasheet

    //VCO Bank Calibration
    A7105_WriteReg(0x26, 0x3b); //Recomended limits from A7105 Datasheet

    //VCO Bank Calibrate channel 0?
    //Set Channel
    A7105_WriteReg(0x0f, 0); //Should we choose a different channel?
    //VCO Calibration
    A7105_WriteReg(0x02, 2);
    ms = CLOCK_getms();
    CLOCK_ResetWatchdog();
    while(CLOCK_getms()  - ms < 500) {
        if(! A7105_ReadReg(0x02))
            break;
    }
    if (CLOCK_getms() - ms >= 500)
        return 0;
    vco_calibration0 = A7105_ReadReg(0x25);
    if (vco_calibration0 & A7105_MASK_VBCF) {
        //Calibration failed...what do we do?
        return 0;
    }

    //Calibrate channel 0xa0?
    //Set Channel
    A7105_WriteReg(0x0f, 0xa0); //Should we choose a different channel?
    //VCO Calibration
    A7105_WriteReg(0x02, 2);
    ms = CLOCK_getms();
    CLOCK_ResetWatchdog();
    while(CLOCK_getms()  - ms < 500) {
        if(! A7105_ReadReg(A7105_02_CALC))
            break;
    }
    if (CLOCK_getms() - ms >= 500)
        return 0;
    vco_calibration1 = A7105_ReadReg(0x25);
    if (vco_calibration1 & A7105_MASK_VBCF) {
        //Calibration failed...what do we do?
        return 0;
    }

    //Reset VCO Band calibration
    A7105_WriteReg(0x25, 0x08);

    A7105_SetTxRxMode(TX_EN);
    A7105_SetPower(Model.tx_power);

    A7105_Strobe(A7105_STANDBY);
    return 1;
}

static void flysky_build_packet(u8 init)
{
    int i;
    //-100% =~ 0x03e8
    //+100% =~ 0x07ca
    //Calculate:
    //Center = 0x5d9
    //1 %    = 5
    packet[0] = init ? 0xaa : 0x55;
    packet[1] = (id >>  0) & 0xff; // 41 68 74 46 
    packet[2] = (id >>  8) & 0xff;
    packet[3] = (id >> 16) & 0xff;
    packet[4] = (id >> 24) & 0xff;
    for (i = 0; i < 8; i++) {
        if (i > Model.num_channels) {
            packet[5 + i*2] = 0;
            packet[6 + i*2] = 0;
            continue;
        }
        s32 value = (s32)Channels[i] * 0x1f1 / CHAN_MAX_VALUE + 0x5d9;
        if (value < 0)
            value = 0;
        packet[5 + i*2] = value & 0xff;
        packet[6 + i*2] = (value >> 8) & 0xff;
    }
    if (Model.proto_opts[PROTOOPTS_WLTOYS] == WLTOYS_EXT_V9X9) {
        if(Channels[4] > 0)
            packet[12] |= 0x20;
        if(Channels[5] > 0)
            packet[10] |= 0x40;
        if(Channels[6] > 0)
            packet[10] |= 0x80;
        if(Channels[7] > 0)
            packet[12] |= 0x10;
    }
    else if (Model.proto_opts[PROTOOPTS_WLTOYS] == WLTOYS_EXT_V6X6) {
        packet[13] = 0x00;
        packet[13] |= 0x03; // not always there ?
        packet[14] = 0x00;
        if(Channels[4] > 0) // Lights
            packet[14] |= 0x40;
        if(Channels[5] > 0) // flip button
            packet[14] |= 0x80;
        if(Channels[6] > 0) // take picture button
            packet[14] |= 0x10;
        if(Channels[7] > 0) // video record
            packet[14] |= 0x01;
        if(Channels[8] > 0) { // headless mode
            packet[14] |= 0x20;
            packet[13] |= 0x80;
        }
        if(Channels[9] > 0) // RTH button
            packet[14] |= 0x08;
        packet[15] = 0x10; // unknown
        packet[16] = 0x10; // unknown
        packet[17] = 0xAA; // unknown
        packet[18] = 0xAA; // unknown
        packet[19] = 0xA7; // unknown, changes at irregular interval in stock TX, looks like a floating ADC
        packet[20] = 0x02; // unknown
    }
    else if (Model.proto_opts[PROTOOPTS_WLTOYS] == WLTOYS_EXT_V91X) {
        const u8 X17_SEQ[10] = { 0x14, 0x31, 0x40, 0x49, 0x49,    // sometime first byte is 0x15
                                 0x49, 0x49, 0x49, 0x49, 0x49, }; 
        static u32 seq_counter;
        seq_counter++;
        packet[12] |= 0x20; // "channel 4" bit 6 always HIGH ?
        packet[13] = 0x00;  // unknown
        packet[14] = 0x00;
        if(Channels[5] > 0)
            packet[14] |= 0x80; // bottom button
        if(Channels[6] > 0)
            packet[14] |= 0x40; // top button
        packet[15] = 0x27; // [15] and [16] apparently hold an analog channel with a value lower than 1000
        packet[16] = 0x03; // maybe it's there for a pitch channel for a CP copter ?
        packet[17] = X17_SEQ[seq_counter % 10]; // not sure what [17] & [18] are for, not even if they're useful
        if(seq_counter % 10 == 0)
            packet[18] = 0x02;
        else
            packet[18] = 0x00;
        packet[19] = 0x00; // unknown
        packet[20] = 0x00; // unknown
    }
}

MODULE_CALLTYPE
static u16 flysky_cb()
{
    if (counter) {
        flysky_build_packet(1);
        A7105_WriteData(packet, 21, 1);
        counter--;
        if (! counter)
            PROTOCOL_SetBindState(0);
    } else {
        flysky_build_packet(0);
        printf("%02d  ",chanrow);
        printf("%02d\t",chancol);
        
        u8 rf_channel = (u8)(tx_channels[chanrow][chancol]);
        //if (Model.proto_opts[PROTOOPTS_WLTOYS] == WLTOYS_EXT_V91X && rf_channel == 0x1A)
        //    rf_channel = 0x2A;
        rf_channel -= chanoffset;
        printf("0x%02X\n", (u8)(rf_channel));
        A7105_WriteData(packet, 21, rf_channel);
        chancol = (chancol + 1) % 16;
        if (! chancol) //Keep transmit power updated
            A7105_SetPower(Model.tx_power);
    }
    return 1460;
}

static void initialize(u8 bind) {
    CLOCK_StopTimer();
    while(1) {
        A7105_Reset();
        CLOCK_ResetWatchdog();
        if (flysky_init())
            break;
    }
    if (Model.fixed_id) {
        id = Model.fixed_id;
    } else {
        id = (Crc(&Model, sizeof(Model)) + Crc(&Transmitter, sizeof(Transmitter))) % 999999;
    }
    
    // 0x023154CE; // v686 sebydocky
    //id =  0x46746841; // 0x41687446; // v912 Deal57
    //id = 0x46746841;
    //id = 0x03F2; //0x2775;
    printf("\nid=0x%X\n",id);
    chanrow = id % 16;
    chancol = 0;
    chanoffset = (id & 0xff) / 16;
    printf("chanrow = %d\n", chanrow);
    printf("chanoffset = %d\n", chanoffset);
    printf("\nrow col\tchan\n");
    if (bind || ! Model.fixed_id) {
        counter = BIND_COUNT;
        PROTOCOL_SetBindState(2500 * 1460 / 1000); //msec
    } else {
        counter = 0;
    }
    CLOCK_StartTimer(2400, flysky_cb);
}

const void *FLYSKY_Cmds(enum ProtoCmds cmd)
{
    switch(cmd) {
        case PROTOCMD_INIT:  initialize(0); return 0;
        case PROTOCMD_DEINIT:
        case PROTOCMD_RESET:
            CLOCK_StopTimer();
            return (void *)(A7105_Reset() ? 1L : -1L);
        case PROTOCMD_CHECK_AUTOBIND: return Model.fixed_id ? 0 : (void *)1L;
        case PROTOCMD_BIND:  initialize(1); return 0;
        case PROTOCMD_NUMCHAN: return (void *)10L;
        case PROTOCMD_DEFAULT_NUMCHAN: return (void *)8L;
        case PROTOCMD_CURRENT_ID: return (void *)((unsigned long)id);
        case PROTOCMD_GETOPTIONS:
            return flysky_opts;
        case PROTOCMD_TELEMETRYSTATE: return (void *)(long)PROTO_TELEM_UNSUPPORTED;
        default: break;
    }
    return 0;
}
#endif