Thierry Pébayle / A7105_FLYSKY_RX

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@0:01963ae1d0c4, 2015-06-08 (annotated)

Committer:
pebayle
Date:
Mon Jun 08 09:16:13 2015 +0000
Revision:
0:01963ae1d0c4
Child:
1:b08e4695ffb7
FLYSKY RC receiver library, compatiblel withTH9X transmitter.Uses an A7105-SY or A7105-CL 2.4Ghz module.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pebayle 0:01963ae1d0c4 1 #include "mbed.h"
pebayle 0:01963ae1d0c4 2 #include "A7105_FLYSKY_RX.h"
pebayle 0:01963ae1d0c4 3
pebayle 0:01963ae1d0c4 4 A7105_flysky_RX::A7105_flysky_RX( PinName SDIO, PinName GIO1, PinName SCK, PinName CS, PinName GIO2, PinName LED ):
pebayle 0:01963ae1d0c4 5 _spi(SDIO, GIO1, SCK), _cs(CS), _waitRx(GIO2), _statusLed(LED)
pebayle 0:01963ae1d0c4 6 {
pebayle 0:01963ae1d0c4 7 //config SPI
pebayle 0:01963ae1d0c4 8 _spi.format(8, 0); //8 bits, mode 0
pebayle 0:01963ae1d0c4 9 _spi.frequency(4000000); //4MHz
pebayle 0:01963ae1d0c4 10 //CS high
pebayle 0:01963ae1d0c4 11 _cs = 1;
pebayle 0:01963ae1d0c4 12 //init channel counter (0->15)
pebayle 0:01963ae1d0c4 13 channelCnt = 0;
pebayle 0:01963ae1d0c4 14 }
pebayle 0:01963ae1d0c4 15
pebayle 0:01963ae1d0c4 16 //////////////
pebayle 0:01963ae1d0c4 17 // init
pebayle 0:01963ae1d0c4 18 //////////////
pebayle 0:01963ae1d0c4 19
pebayle 0:01963ae1d0c4 20 bool A7105_flysky_RX::init(void)
pebayle 0:01963ae1d0c4 21 {
pebayle 0:01963ae1d0c4 22 unsigned char flysky_id_read[4], i;
pebayle 0:01963ae1d0c4 23
pebayle 0:01963ae1d0c4 24 //wait 10ms to let A7105 wake up
pebayle 0:01963ae1d0c4 25 wait(0.010);
pebayle 0:01963ae1d0c4 26
pebayle 0:01963ae1d0c4 27 //reset A7105
pebayle 0:01963ae1d0c4 28 writeRegVal(0x00, 0x00);
pebayle 0:01963ae1d0c4 29
pebayle 0:01963ae1d0c4 30 //init registers A7105
pebayle 0:01963ae1d0c4 31 for (i=0; i<0x33; i++)
pebayle 0:01963ae1d0c4 32 if (A7105_regs_val[i] != 0xFF)
pebayle 0:01963ae1d0c4 33 writeRegVal(i, A7105_regs_val[i]);
pebayle 0:01963ae1d0c4 34
pebayle 0:01963ae1d0c4 35 //set flysky protocol id <- 0x2AC57554
pebayle 0:01963ae1d0c4 36 _cs = 0;
pebayle 0:01963ae1d0c4 37 _spi.write(0x06); //set ID command
pebayle 0:01963ae1d0c4 38 for(i=0; i<4; i++) _spi.write(flysky_id[i]); //set 4 bytes
pebayle 0:01963ae1d0c4 39 _cs = 1;
pebayle 0:01963ae1d0c4 40
pebayle 0:01963ae1d0c4 41 //read flysky protocol id -> id[0..3]
pebayle 0:01963ae1d0c4 42 _cs = 0;
pebayle 0:01963ae1d0c4 43 _spi.write(0x46); //read ID
pebayle 0:01963ae1d0c4 44 for (i=0; i<4; i++) flysky_id_read[i] = _spi.write(0x00); //read 4 bytes (send dummy bytes 0x00)
pebayle 0:01963ae1d0c4 45 _cs = 1;
pebayle 0:01963ae1d0c4 46
pebayle 0:01963ae1d0c4 47 //check flysky id read
pebayle 0:01963ae1d0c4 48 for (i=0; i<4; i++)
pebayle 0:01963ae1d0c4 49 {
pebayle 0:01963ae1d0c4 50 if (flysky_id_read[i] != flysky_id[i])
pebayle 0:01963ae1d0c4 51 {
pebayle 0:01963ae1d0c4 52 return false;
pebayle 0:01963ae1d0c4 53 }
pebayle 0:01963ae1d0c4 54 }
pebayle 0:01963ae1d0c4 55
pebayle 0:01963ae1d0c4 56 //standby mode
pebayle 0:01963ae1d0c4 57 writeCmd( 0xA0); //A0 -> standby mode
pebayle 0:01963ae1d0c4 58
pebayle 0:01963ae1d0c4 59 //IF Filter Bank Calibration
pebayle 0:01963ae1d0c4 60 writeRegVal( 0x02, 0x01 );
pebayle 0:01963ae1d0c4 61 while ( readReg(0x02) ); //wait calibration end
pebayle 0:01963ae1d0c4 62
pebayle 0:01963ae1d0c4 63 //VCO Current Calibration
pebayle 0:01963ae1d0c4 64 writeRegVal(0x24, 0x013);
pebayle 0:01963ae1d0c4 65
pebayle 0:01963ae1d0c4 66 //VCO Bank Calibration
pebayle 0:01963ae1d0c4 67 writeRegVal(0x26, 0x03B);
pebayle 0:01963ae1d0c4 68
pebayle 0:01963ae1d0c4 69 //VCO Bank Calibrate channel 0x00
pebayle 0:01963ae1d0c4 70 writeRegVal( 0x0F, 0x00); //Set Channel 0
pebayle 0:01963ae1d0c4 71 writeRegVal( 0x02, 0x02); //VCO Calibration
pebayle 0:01963ae1d0c4 72 while ( readReg(0x02) ); //wait calibration end
pebayle 0:01963ae1d0c4 73
pebayle 0:01963ae1d0c4 74 //VCO Bank Calibrate channel 0xA0
pebayle 0:01963ae1d0c4 75 writeRegVal( 0x0F, 0xA0); //Set Channel 0xA0
pebayle 0:01963ae1d0c4 76 writeRegVal( 0x02, 0x02); //VCO Calibration
pebayle 0:01963ae1d0c4 77 while ( readReg(0x02) ); //wait calibration end
pebayle 0:01963ae1d0c4 78
pebayle 0:01963ae1d0c4 79 //Reset VCO Band calibration
pebayle 0:01963ae1d0c4 80 writeRegVal( 0x25, 0x08);
pebayle 0:01963ae1d0c4 81
pebayle 0:01963ae1d0c4 82 return true;
pebayle 0:01963ae1d0c4 83 }
pebayle 0:01963ae1d0c4 84
pebayle 0:01963ae1d0c4 85 //////////////////
pebayle 0:01963ae1d0c4 86 // writeCmd
pebayle 0:01963ae1d0c4 87 //////////////////
pebayle 0:01963ae1d0c4 88
pebayle 0:01963ae1d0c4 89 void A7105_flysky_RX::writeCmd(unsigned char cmd)
pebayle 0:01963ae1d0c4 90 {
pebayle 0:01963ae1d0c4 91 _cs = 0;
pebayle 0:01963ae1d0c4 92 _spi.write(cmd);
pebayle 0:01963ae1d0c4 93 _cs = 1;
pebayle 0:01963ae1d0c4 94 }
pebayle 0:01963ae1d0c4 95
pebayle 0:01963ae1d0c4 96 /////////////////////
pebayle 0:01963ae1d0c4 97 // writeRegVal
pebayle 0:01963ae1d0c4 98 /////////////////////
pebayle 0:01963ae1d0c4 99
pebayle 0:01963ae1d0c4 100 void A7105_flysky_RX::writeRegVal(unsigned char regAddr, unsigned char regVal)
pebayle 0:01963ae1d0c4 101 {
pebayle 0:01963ae1d0c4 102 _cs = 0;
pebayle 0:01963ae1d0c4 103 regAddr = regAddr & 0xBF; //clear R/W bit
pebayle 0:01963ae1d0c4 104 _spi.write(regAddr);
pebayle 0:01963ae1d0c4 105 _spi.write(regVal);
pebayle 0:01963ae1d0c4 106 _cs = 1;
pebayle 0:01963ae1d0c4 107 }
pebayle 0:01963ae1d0c4 108
pebayle 0:01963ae1d0c4 109 /////////////////
pebayle 0:01963ae1d0c4 110 // readReg
pebayle 0:01963ae1d0c4 111 /////////////////
pebayle 0:01963ae1d0c4 112
pebayle 0:01963ae1d0c4 113 unsigned char A7105_flysky_RX::readReg(unsigned char regAddr)
pebayle 0:01963ae1d0c4 114 {
pebayle 0:01963ae1d0c4 115 unsigned char regVal;
pebayle 0:01963ae1d0c4 116 _cs = 0;
pebayle 0:01963ae1d0c4 117 regAddr = regAddr | 0x40; //set R/W bit
pebayle 0:01963ae1d0c4 118 _spi.write(regAddr);
pebayle 0:01963ae1d0c4 119 regVal = _spi.write(0x00);
pebayle 0:01963ae1d0c4 120 _cs = 1;
pebayle 0:01963ae1d0c4 121 return regVal;
pebayle 0:01963ae1d0c4 122 }
pebayle 0:01963ae1d0c4 123
pebayle 0:01963ae1d0c4 124 ////////////////////
pebayle 0:01963ae1d0c4 125 // readPacket
pebayle 0:01963ae1d0c4 126 ////////////////////
pebayle 0:01963ae1d0c4 127
pebayle 0:01963ae1d0c4 128 void A7105_flysky_RX::readPacket(void)
pebayle 0:01963ae1d0c4 129 {
pebayle 0:01963ae1d0c4 130 unsigned char i, highByte, lowByte;
pebayle 0:01963ae1d0c4 131 unsigned int pulseDur;
pebayle 0:01963ae1d0c4 132
pebayle 0:01963ae1d0c4 133 _cs = 0;
pebayle 0:01963ae1d0c4 134 _spi.write(0x45); //cmd read rx packet
pebayle 0:01963ae1d0c4 135 _spi.write(0x00); //read sync
pebayle 0:01963ae1d0c4 136 for (i=0; i<4; i++) packetTxId[i] = _spi.write(0x00); //read tx id: 4 bytes
pebayle 0:01963ae1d0c4 137 for (i=0; i<8; i++) //read pulse duration: 8 x (lowByte, highByte)
pebayle 0:01963ae1d0c4 138 {
pebayle 0:01963ae1d0c4 139 lowByte = _spi.write(0x00);
pebayle 0:01963ae1d0c4 140 highByte = _spi.write(0x00);
pebayle 0:01963ae1d0c4 141 pulseDur = lowByte + ( highByte << 8 );
pebayle 0:01963ae1d0c4 142 if ( pulseDur > 2000 ) pulseDur = 2000;
pebayle 0:01963ae1d0c4 143 if ( pulseDur < 1000 ) pulseDur = 1000;
pebayle 0:01963ae1d0c4 144 packetServoPulseDur[i] = pulseDur;
pebayle 0:01963ae1d0c4 145 }
pebayle 0:01963ae1d0c4 146 _cs = 1;
pebayle 0:01963ae1d0c4 147 }
pebayle 0:01963ae1d0c4 148
pebayle 0:01963ae1d0c4 149 //////////////////
pebayle 0:01963ae1d0c4 150 // bind
pebayle 0:01963ae1d0c4 151 //////////////////
pebayle 0:01963ae1d0c4 152
pebayle 0:01963ae1d0c4 153 //tries to bind TX for approx 3 seconds
pebayle 0:01963ae1d0c4 154 //if bind succeed then return true and update txId
pebayle 0:01963ae1d0c4 155 //if bind failed then return false and take default txId
pebayle 0:01963ae1d0c4 156
pebayle 0:01963ae1d0c4 157 bool A7105_flysky_RX::bind(void)
pebayle 0:01963ae1d0c4 158 {
pebayle 0:01963ae1d0c4 159 short int cpt, i;
pebayle 0:01963ae1d0c4 160 unsigned char modeReg;
pebayle 0:01963ae1d0c4 161 bool bindOk = false;
pebayle 0:01963ae1d0c4 162
pebayle 0:01963ae1d0c4 163 //wait bind packet for 3 seconds (300 loops of 10ms)
pebayle 0:01963ae1d0c4 164 for (cpt=0; cpt<300; cpt++)
pebayle 0:01963ae1d0c4 165 {
pebayle 0:01963ae1d0c4 166 //blink led at 12hz
pebayle 0:01963ae1d0c4 167 if (cpt & 0x0004) _statusLed = 1; else _statusLed = 0;
pebayle 0:01963ae1d0c4 168
pebayle 0:01963ae1d0c4 169 //listen on channel 0
pebayle 0:01963ae1d0c4 170 writeCmd( 0xA0); //A0 -> standby mode
pebayle 0:01963ae1d0c4 171 writeCmd( 0xF0); //F0 -> reset RX FIFO
pebayle 0:01963ae1d0c4 172 writeRegVal( 0x0F, 0x00 ); //Set Channel 0
pebayle 0:01963ae1d0c4 173 writeCmd( 0xC0 ); //C0 -> get into RX mode
pebayle 0:01963ae1d0c4 174
pebayle 0:01963ae1d0c4 175 //wait WAIT_RX to setup (takes about 1us)
pebayle 0:01963ae1d0c4 176 while ( !_waitRx );
pebayle 0:01963ae1d0c4 177
pebayle 0:01963ae1d0c4 178 //wait 10ms to receive packet
pebayle 0:01963ae1d0c4 179 wait(0.010);
pebayle 0:01963ae1d0c4 180
pebayle 0:01963ae1d0c4 181 //test receive bind packet
pebayle 0:01963ae1d0c4 182 if ( !_waitRx )
pebayle 0:01963ae1d0c4 183 {
pebayle 0:01963ae1d0c4 184 modeReg = readReg(0x00); //read mode register
pebayle 0:01963ae1d0c4 185 if ( !(modeReg & 0x60) ) //test valid packet ( CRC(bit5) and CEF(bit6) = 0 )
pebayle 0:01963ae1d0c4 186 {
pebayle 0:01963ae1d0c4 187 bindOk = true; //bind ok
pebayle 0:01963ae1d0c4 188 readPacket(); //read packet
pebayle 0:01963ae1d0c4 189 for (i=0; i<4; i++) //txId <- packetTxId
pebayle 0:01963ae1d0c4 190 txId[i] = packetTxId[i];
pebayle 0:01963ae1d0c4 191 break; //exit loop
pebayle 0:01963ae1d0c4 192 }
pebayle 0:01963ae1d0c4 193 }
pebayle 0:01963ae1d0c4 194 }
pebayle 0:01963ae1d0c4 195
pebayle 0:01963ae1d0c4 196 //default id if failed
pebayle 0:01963ae1d0c4 197 if (!bindOk) for (cpt=0; cpt<4; cpt++)
pebayle 0:01963ae1d0c4 198 txId[cpt] = default_tx_id[cpt]; //txId <- pdefault_tx_id
pebayle 0:01963ae1d0c4 199
pebayle 0:01963ae1d0c4 200 //compute channel offset
pebayle 0:01963ae1d0c4 201 channelOffset = txId[0] >> 4;
pebayle 0:01963ae1d0c4 202 if ( channelOffset > 9 ) channelOffset = 9;
pebayle 0:01963ae1d0c4 203 //printf("\r\txId[0] = %x, channelOffset = %x", txId[0], channelOffset);
pebayle 0:01963ae1d0c4 204
pebayle 0:01963ae1d0c4 205 //led off
pebayle 0:01963ae1d0c4 206 _statusLed = 1;
pebayle 0:01963ae1d0c4 207
pebayle 0:01963ae1d0c4 208 return bindOk;
pebayle 0:01963ae1d0c4 209 }
pebayle 0:01963ae1d0c4 210
pebayle 0:01963ae1d0c4 211 ////////////////////////////////////
pebayle 0:01963ae1d0c4 212 // waitPacket
pebayle 0:01963ae1d0c4 213 ////////////////////////////////////
pebayle 0:01963ae1d0c4 214
pebayle 0:01963ae1d0c4 215 //return error code
pebayle 0:01963ae1d0c4 216
pebayle 0:01963ae1d0c4 217 unsigned char A7105_flysky_RX::waitPacket(void)
pebayle 0:01963ae1d0c4 218 {
pebayle 0:01963ae1d0c4 219 unsigned char channelNbr;
pebayle 0:01963ae1d0c4 220 int begin, now;
pebayle 0:01963ae1d0c4 221 unsigned char modeReg, i;
pebayle 0:01963ae1d0c4 222 unsigned char errorCode = NO_ERROR;
pebayle 0:01963ae1d0c4 223
pebayle 0:01963ae1d0c4 224 //wait time out(1.5ms) or packet received
pebayle 0:01963ae1d0c4 225 timer.start();
pebayle 0:01963ae1d0c4 226 begin = timer.read_us();
pebayle 0:01963ae1d0c4 227 now = begin;
pebayle 0:01963ae1d0c4 228 while ( ( ( now - begin ) < 1500 ) & _waitRx )
pebayle 0:01963ae1d0c4 229 {
pebayle 0:01963ae1d0c4 230 now = timer.read_us();
pebayle 0:01963ae1d0c4 231 }
pebayle 0:01963ae1d0c4 232
pebayle 0:01963ae1d0c4 233 //test packet received or time out
pebayle 0:01963ae1d0c4 234 if (!_waitRx)
pebayle 0:01963ae1d0c4 235 //packet received, check validity
pebayle 0:01963ae1d0c4 236 {
pebayle 0:01963ae1d0c4 237 modeReg = readReg(0x00); //read mode register
pebayle 0:01963ae1d0c4 238 if ( !(modeReg & 0x60) ) //test valid packet ( CRC(bit5) and CEF(bit6) = 0 )
pebayle 0:01963ae1d0c4 239 {
pebayle 0:01963ae1d0c4 240 //valid packet -> read packet
pebayle 0:01963ae1d0c4 241 readPacket();
pebayle 0:01963ae1d0c4 242
pebayle 0:01963ae1d0c4 243 //check txId
pebayle 0:01963ae1d0c4 244 if ( ( txId[0] == packetTxId[0] )
pebayle 0:01963ae1d0c4 245 &( txId[1] == packetTxId[1] )
pebayle 0:01963ae1d0c4 246 &( txId[2] == packetTxId[2] )
pebayle 0:01963ae1d0c4 247 &( txId[3] == packetTxId[3] ) )
pebayle 0:01963ae1d0c4 248 {
pebayle 0:01963ae1d0c4 249 //led on
pebayle 0:01963ae1d0c4 250 _statusLed = 0;
pebayle 0:01963ae1d0c4 251
pebayle 0:01963ae1d0c4 252 //update servo pulse duration
pebayle 0:01963ae1d0c4 253 for (i=0; i<8; i++) servoPulseDur[i] = packetServoPulseDur[i];
pebayle 0:01963ae1d0c4 254 }
pebayle 0:01963ae1d0c4 255 else
pebayle 0:01963ae1d0c4 256 {
pebayle 0:01963ae1d0c4 257 errorCode = TX_ID_ERROR;
pebayle 0:01963ae1d0c4 258 //led off
pebayle 0:01963ae1d0c4 259 _statusLed = 1;
pebayle 0:01963ae1d0c4 260 }
pebayle 0:01963ae1d0c4 261 }
pebayle 0:01963ae1d0c4 262 else
pebayle 0:01963ae1d0c4 263 {
pebayle 0:01963ae1d0c4 264 errorCode = VALIDITY_ERROR;
pebayle 0:01963ae1d0c4 265 //led off
pebayle 0:01963ae1d0c4 266 _statusLed = 1;
pebayle 0:01963ae1d0c4 267 }
pebayle 0:01963ae1d0c4 268 }
pebayle 0:01963ae1d0c4 269 else
pebayle 0:01963ae1d0c4 270 //time out
pebayle 0:01963ae1d0c4 271 {
pebayle 0:01963ae1d0c4 272 errorCode = TIME_OUT_ERROR;
pebayle 0:01963ae1d0c4 273
pebayle 0:01963ae1d0c4 274 //led off
pebayle 0:01963ae1d0c4 275 _statusLed = 1;
pebayle 0:01963ae1d0c4 276
pebayle 0:01963ae1d0c4 277 //increment channel counter
pebayle 0:01963ae1d0c4 278 channelCnt++; if ( channelCnt > 15 ) channelCnt = 0;
pebayle 0:01963ae1d0c4 279 }
pebayle 0:01963ae1d0c4 280
pebayle 0:01963ae1d0c4 281 //increment channel counter
pebayle 0:01963ae1d0c4 282 channelCnt++; if ( channelCnt > 15 ) channelCnt = 0;
pebayle 0:01963ae1d0c4 283
pebayle 0:01963ae1d0c4 284 //get channel number into "A7105_tx_channels" table
pebayle 0:01963ae1d0c4 285 channelNbr = A7105_tx_channels[ txId[0] & 0x0F ][ channelCnt ] - channelOffset - 1;
pebayle 0:01963ae1d0c4 286
pebayle 0:01963ae1d0c4 287 //listen to channel number
pebayle 0:01963ae1d0c4 288 writeCmd( 0xA0); //A0 -> standby mode (does a falling edge on WAIT_RX pin !!!)
pebayle 0:01963ae1d0c4 289 writeCmd( 0xF0); //F0 -> reset RX FIFO
pebayle 0:01963ae1d0c4 290 writeRegVal( 0x0F, channelNbr ); //Set Channel number
pebayle 0:01963ae1d0c4 291 writeCmd( 0xC0 ); //C0 -> get into RX mode
pebayle 0:01963ae1d0c4 292
pebayle 0:01963ae1d0c4 293 //result of previous listening...
pebayle 0:01963ae1d0c4 294 return errorCode;
pebayle 0:01963ae1d0c4 295 }