FLYSKY RC receiver compatible with TH9X transmitter. Uses an A7105-CL or A7105-SY 2.4Ghz module.
Dependents: A7105_FLYSKY_RX_WITH_PPM
A7105_FLYSKY_RX.cpp
- Committer:
- pebayle
- Date:
- 2015-06-11
- Revision:
- 2:57dfa1f32dad
- Parent:
- 1:b08e4695ffb7
File content as of revision 2:57dfa1f32dad:
///////////////////////////////////////////////////////////////////////////////////////////////////// //FLYSKY RC receiver library with A7105 2.4Ghz module //compatible with TH9X ///////////////////////////////////////////////////////////////////////////////////////////////////// /* // example program: 8 channels receiver with a FRDM-KL05Z platform // // If you want to make a small RC receiver, you can do a custom board with a cheap KL05Z32VLC4 (LQPF32 0.8mm pitch package ), // an A7105CL or A7105SY module and a 3.3V regulator. It works with target FRDM-KL05Z and no modification of mbed library! // // In order to program an external KL05Z32VLC4, I take a FRDM-KL25Z board (don't take FRDM-KL05Z board, there's a bug on layout !!!), // cut wire under JP11 connector to disconnect onboard KL25Z chip, do a special cable (see FRDM-KL25Z schematic, J6 SWD CONNECTOR, // you just need to solder 5 wires: 3.3V, GND, SWD_DIO, SWD_CLK, RST_TGT) to link your custom KL05Z32VLC4 board, and program FRDM-KL25Z // board with an FRDM-KL05Z firmware and it works fine as a cheap programmer interface! // #include "mbed.h" #include "A7105_FLYSKY_RX.h" //compiler option #define DEBUG 1 //display status through serial //A7105 <-> KL05Z pin assignment #define PIN_SDIO PTA7 //SPI mosi #define PIN_GIO1 PTA6 //SPI miso #define PIN_SCK PTB0 //SPI sck #define PIN_CS PTA10 //CS #define PIN_GIO2 PTA11 //wait rx //servo <-> KL05Z pin assignment #define SERVO1 PTB5 #define SERVO2 PTA12 #define SERVO3 PTB6 #define SERVO4 PTB7 #define SERVO5 PTB11 #define SERVO6 PTA5 #define SERVO7 PTA9 #define SERVO8 PTA8 //serial debug #if DEBUG Serial pc(USBTX, USBRX); #endif //servo pwm output PwmOut servoPwmOut[6] = { PwmOut(PTB5), PwmOut(PTA12), PwmOut(PTB6), PwmOut(PTB7), PwmOut(PTB11), PwmOut(PTA5) }; //A7105 flysky RX A7105_flysky_RX rx(PIN_SDIO, PIN_GIO1, PIN_SCK, PIN_CS, PIN_GIO2, LED_GREEN); //mosi, miso, sck, cs, wait_rx, status_led) ///////////// // M A I N ///////////// int main() { int i; bool ok; #if DEBUG //say hello! pc.printf("\n\rHello!"); #endif //init servo PWM output ( 1.5ms pulse width = center position ) for (i=0; i<6; i++) { servoPwmOut[i].period_us(20000); //50hz servoPwmOut[i].pulsewidth_us(1500); //1.5ms } //init A7105 ok = rx.init(); if (ok) { //bind TX ok = rx.bind(); //if bind failed, take default tx id #if DEBUG //display bind status if (!ok) printf("\r\nbind failed !!!"); //display tx id (default tx id if bind failed) printf("\r\ntx id = %x %x %x %x", rx.txId[3], rx.txId[2], rx.txId[1], rx.txId[0]); #endif //infinite loop, receive packet and update servos int cptError = 0; while (1) { //wait packet (1.46ms) or timeout(1.5ms) since last call to this function switch( rx.waitPacket() ) { case NO_ERROR: //update servo position for (i=0; i<6; i++) servoPwmOut[i].pulsewidth_us( rx.servoPulseDur[i] ); break; case TIME_OUT_ERROR: printf("\r\n%d: time out error", cptError); cptError++; break; case VALIDITY_ERROR: printf("\r\n%d: validity error", cptError); cptError++; break; case TX_ID_ERROR: printf("\r\n%d: tx id error", cptError); cptError++; break; } } } #if DEBUG else { printf("\r\ninit failed !!!"); } #endif } */ ///////////////////////////////////////////////////////////////////////////////////////////////////// #include "mbed.h" #include "A7105_FLYSKY_RX.h" A7105_flysky_RX::A7105_flysky_RX( PinName SDIO, PinName GIO1, PinName SCK, PinName CS, PinName GIO2, PinName LED ): _spi(SDIO, GIO1, SCK), _cs(CS), _waitRx(GIO2), _statusLed(LED) { //config SPI _spi.format(8, 0); //8 bits, mode 0 _spi.frequency(4000000); //4MHz //CS high _cs = 1; //init channel counter (0->15) channelCnt = 0; } ////////////// // init ////////////// bool A7105_flysky_RX::init(void) { unsigned char flysky_id_read[4], i; //wait 10ms to let A7105 wake up wait(0.010); //reset A7105 writeRegVal(0x00, 0x00); //init registers A7105 for (i=0; i<0x33; i++) if (A7105_regs_val[i] != 0xFF) writeRegVal(i, A7105_regs_val[i]); //set flysky protocol id <- 0x2AC57554 _cs = 0; _spi.write(0x06); //set ID command for(i=0; i<4; i++) _spi.write(flysky_id[i]); //set 4 bytes _cs = 1; //read flysky protocol id -> id[0..3] _cs = 0; _spi.write(0x46); //read ID for (i=0; i<4; i++) flysky_id_read[i] = _spi.write(0x00); //read 4 bytes (send dummy bytes 0x00) _cs = 1; //check flysky id read for (i=0; i<4; i++) { if (flysky_id_read[i] != flysky_id[i]) { return false; } } //standby mode writeCmd( 0xA0); //A0 -> standby mode //IF Filter Bank Calibration writeRegVal( 0x02, 0x01 ); while ( readReg(0x02) ); //wait calibration end //VCO Current Calibration writeRegVal(0x24, 0x013); //VCO Bank Calibration writeRegVal(0x26, 0x03B); //VCO Bank Calibrate channel 0x00 writeRegVal( 0x0F, 0x00); //Set Channel 0 writeRegVal( 0x02, 0x02); //VCO Calibration while ( readReg(0x02) ); //wait calibration end //VCO Bank Calibrate channel 0xA0 writeRegVal( 0x0F, 0xA0); //Set Channel 0xA0 writeRegVal( 0x02, 0x02); //VCO Calibration while ( readReg(0x02) ); //wait calibration end //Reset VCO Band calibration writeRegVal( 0x25, 0x08); return true; } ////////////////// // writeCmd ////////////////// void A7105_flysky_RX::writeCmd(unsigned char cmd) { _cs = 0; _spi.write(cmd); _cs = 1; } ///////////////////// // writeRegVal ///////////////////// void A7105_flysky_RX::writeRegVal(unsigned char regAddr, unsigned char regVal) { _cs = 0; regAddr = regAddr & 0xBF; //clear R/W bit _spi.write(regAddr); _spi.write(regVal); _cs = 1; } ///////////////// // readReg ///////////////// unsigned char A7105_flysky_RX::readReg(unsigned char regAddr) { unsigned char regVal; _cs = 0; regAddr = regAddr | 0x40; //set R/W bit _spi.write(regAddr); regVal = _spi.write(0x00); _cs = 1; return regVal; } //////////////////// // readPacket //////////////////// void A7105_flysky_RX::readPacket(void) { unsigned char i, highByte, lowByte; unsigned int pulseDur; _cs = 0; _spi.write(0x45); //cmd read rx packet _spi.write(0x00); //read sync for (i=0; i<4; i++) packetTxId[i] = _spi.write(0x00); //read tx id: 4 bytes for (i=0; i<8; i++) //read pulse duration: 8 x (lowByte, highByte) { lowByte = _spi.write(0x00); highByte = _spi.write(0x00); pulseDur = lowByte + ( highByte << 8 ); if ( pulseDur > 2000 ) pulseDur = 2000; if ( pulseDur < 1000 ) pulseDur = 1000; packetServoPulseDur[i] = pulseDur; } _cs = 1; } ////////////////// // bind ////////////////// //tries to bind TX for approx 3 seconds //if bind succeed then return true and update txId //if bind failed then return false and take default txId bool A7105_flysky_RX::bind(void) { short int cpt, i; unsigned char modeReg; bool bindOk = false; //wait bind packet for 3 seconds (300 loops of 10ms) for (cpt=0; cpt<300; cpt++) { //blink led at 12hz if (cpt & 0x0004) _statusLed = 1; else _statusLed = 0; //listen on channel 0 writeCmd( 0xA0); //A0 -> standby mode writeCmd( 0xF0); //F0 -> reset RX FIFO writeRegVal( 0x0F, 0x00 ); //Set Channel 0 writeCmd( 0xC0 ); //C0 -> get into RX mode //wait WAIT_RX to setup (takes about 1us) while ( !_waitRx ); //wait 10ms to receive packet wait(0.010); //test receive bind packet if ( !_waitRx ) { modeReg = readReg(0x00); //read mode register if ( !(modeReg & 0x60) ) //test valid packet ( CRC(bit5) and CEF(bit6) = 0 ) { bindOk = true; //bind ok readPacket(); //read packet for (i=0; i<4; i++) //txId <- packetTxId txId[i] = packetTxId[i]; break; //exit loop } } } //default id if failed if (!bindOk) for (cpt=0; cpt<4; cpt++) txId[cpt] = default_tx_id[cpt]; //txId <- pdefault_tx_id //compute channel offset channelOffset = txId[0] >> 4; if ( channelOffset > 9 ) channelOffset = 9; //printf("\r\txId[0] = %x, channelOffset = %x", txId[0], channelOffset); //led off _statusLed = 1; return bindOk; } //////////////////////////////////// // waitPacket //////////////////////////////////// //return error code unsigned char A7105_flysky_RX::waitPacket(void) { unsigned char channelNbr; int begin, now; unsigned char modeReg, i; unsigned char errorCode = NO_ERROR; //wait time out(1.5ms) or packet received timer.start(); begin = timer.read_us(); now = begin; while ( ( ( now - begin ) < 1500 ) & _waitRx ) { now = timer.read_us(); } //test packet received or time out if (!_waitRx) //packet received, check validity { modeReg = readReg(0x00); //read mode register if ( !(modeReg & 0x60) ) //test valid packet ( CRC(bit5) and CEF(bit6) = 0 ) { //valid packet -> read packet readPacket(); //check txId if ( ( txId[0] == packetTxId[0] ) &( txId[1] == packetTxId[1] ) &( txId[2] == packetTxId[2] ) &( txId[3] == packetTxId[3] ) ) { //led on _statusLed = 0; //update servo pulse duration for (i=0; i<8; i++) servoPulseDur[i] = packetServoPulseDur[i]; } else { errorCode = TX_ID_ERROR; //led off _statusLed = 1; } } else { errorCode = VALIDITY_ERROR; //led off _statusLed = 1; } } else //time out { errorCode = TIME_OUT_ERROR; //led off _statusLed = 1; //increment channel counter channelCnt++; if ( channelCnt > 15 ) channelCnt = 0; } //increment channel counter channelCnt++; if ( channelCnt > 15 ) channelCnt = 0; //get channel number into "A7105_tx_channels" table channelNbr = A7105_tx_channels[ txId[0] & 0x0F ][ channelCnt ] - channelOffset - 1; //listen to channel number writeCmd( 0xA0); //A0 -> standby mode (does a falling edge on WAIT_RX pin !!!) writeCmd( 0xF0); //F0 -> reset RX FIFO writeRegVal( 0x0F, channelNbr ); //Set Channel number writeCmd( 0xC0 ); //C0 -> get into RX mode //result of previous listening... return errorCode; }