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@3:19d3601a7744, 2015-11-13 (annotated)

Committer:
pebayle
Date:
Fri Nov 13 08:49:01 2015 +0000
Revision:
3:19d3601a7744
Parent:
1:b08e4695ffb7 
just few changes in comments...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pebayle 1:b08e4695ffb7 1 /////////////////////////////////////////////////////////////////////////////////////////////////////
pebayle 1:b08e4695ffb7 2 //FLYSKY RC receiver library with A7105 2.4Ghz module
pebayle 1:b08e4695ffb7 3 //compatible with TH9X
pebayle 1:b08e4695ffb7 4 /////////////////////////////////////////////////////////////////////////////////////////////////////
pebayle 1:b08e4695ffb7 5
pebayle 1:b08e4695ffb7 6 /*
pebayle 3:19d3601a7744 7 // example program: 8 channels receiver with a FRDM-KL05Z platform
pebayle 3:19d3601a7744 8 //
pebayle 3:19d3601a7744 9 // If you want to make a small RC receiver, you can do a custom board with a cheap KL05Z32VLC4 (LQPF32 0.8mm pitch package ),
pebayle 3:19d3601a7744 10 // an A7105CL or A7105SY module and a small 3.3V regulator. It works with target FRDM-KL05Z and no modification of mbed library!
pebayle 3:19d3601a7744 11 //
pebayle 3:19d3601a7744 12 // In order to program an external KL05Z32VLC4, I take a FRDM-KL25Z board (don't take FRDM-KL05Z board, there's a bug on design !!!),
pebayle 3:19d3601a7744 13 // cut wire under JP11 connector to disconnect onboard KL25Z chip, do a special cable (see FRDM-KL25Z schematic, J6 SWD CONNECTOR,
pebayle 3:19d3601a7744 14 // 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
pebayle 3:19d3601a7744 15 // board with an FRDM-KL05Z firmware and it works fine as a cheap programmer interface!
pebayle 3:19d3601a7744 16 //
pebayle 1:b08e4695ffb7 17
pebayle 1:b08e4695ffb7 18 #include "mbed.h"
pebayle 1:b08e4695ffb7 19 #include "A7105_FLYSKY_RX.h"
pebayle 1:b08e4695ffb7 20
pebayle 1:b08e4695ffb7 21 //compiler option
pebayle 1:b08e4695ffb7 22 #define DEBUG 1 //display status through serial
pebayle 1:b08e4695ffb7 23
pebayle 1:b08e4695ffb7 24 //A7105 <-> KL05Z pin assignment
pebayle 1:b08e4695ffb7 25 #define PIN_SDIO PTA7 //SPI mosi
pebayle 1:b08e4695ffb7 26 #define PIN_GIO1 PTA6 //SPI miso
pebayle 1:b08e4695ffb7 27 #define PIN_SCK PTB0 //SPI sck
pebayle 1:b08e4695ffb7 28 #define PIN_CS PTA10 //CS
pebayle 1:b08e4695ffb7 29 #define PIN_GIO2 PTA11 //wait rx
pebayle 1:b08e4695ffb7 30
pebayle 1:b08e4695ffb7 31 //servo <-> KL05Z pin assignment
pebayle 1:b08e4695ffb7 32 #define SERVO1 PTB5
pebayle 1:b08e4695ffb7 33 #define SERVO2 PTA12
pebayle 1:b08e4695ffb7 34 #define SERVO3 PTB6
pebayle 1:b08e4695ffb7 35 #define SERVO4 PTB7
pebayle 1:b08e4695ffb7 36 #define SERVO5 PTB11
pebayle 1:b08e4695ffb7 37 #define SERVO6 PTA5
pebayle 1:b08e4695ffb7 38 #define SERVO7 PTA9
pebayle 1:b08e4695ffb7 39 #define SERVO8 PTA8
pebayle 1:b08e4695ffb7 40
pebayle 1:b08e4695ffb7 41 //serial debug
pebayle 1:b08e4695ffb7 42 #if DEBUG
pebayle 1:b08e4695ffb7 43 Serial pc(USBTX, USBRX);
pebayle 1:b08e4695ffb7 44 #endif
pebayle 1:b08e4695ffb7 45
pebayle 1:b08e4695ffb7 46 //servo pwm output
pebayle 1:b08e4695ffb7 47 PwmOut servoPwmOut[6] = { PwmOut(PTB5), PwmOut(PTA12), PwmOut(PTB6), PwmOut(PTB7), PwmOut(PTB11), PwmOut(PTA5) };
pebayle 1:b08e4695ffb7 48
pebayle 1:b08e4695ffb7 49 //A7105 flysky RX
pebayle 1:b08e4695ffb7 50 A7105_flysky_RX rx(PIN_SDIO, PIN_GIO1, PIN_SCK, PIN_CS, PIN_GIO2, LED_GREEN); //mosi, miso, sck, cs, wait_rx, status_led)
pebayle 1:b08e4695ffb7 51
pebayle 1:b08e4695ffb7 52 /////////////
pebayle 1:b08e4695ffb7 53 // M A I N
pebayle 1:b08e4695ffb7 54 /////////////
pebayle 1:b08e4695ffb7 55
pebayle 1:b08e4695ffb7 56 int main()
pebayle 1:b08e4695ffb7 57 {
pebayle 1:b08e4695ffb7 58 int i;
pebayle 1:b08e4695ffb7 59 bool ok;
pebayle 1:b08e4695ffb7 60
pebayle 1:b08e4695ffb7 61 #if DEBUG
pebayle 1:b08e4695ffb7 62 //say hello!
pebayle 1:b08e4695ffb7 63 pc.printf("\n\rHello!");
pebayle 1:b08e4695ffb7 64 #endif
pebayle 1:b08e4695ffb7 65
pebayle 1:b08e4695ffb7 66 //init servo PWM output ( 1.5ms pulse width = center position )
pebayle 1:b08e4695ffb7 67 for (i=0; i<6; i++)
pebayle 1:b08e4695ffb7 68 {
pebayle 1:b08e4695ffb7 69 servoPwmOut[i].period_us(20000); //50hz
pebayle 1:b08e4695ffb7 70 servoPwmOut[i].pulsewidth_us(1500); //1.5ms
pebayle 1:b08e4695ffb7 71 }
pebayle 1:b08e4695ffb7 72
pebayle 1:b08e4695ffb7 73 //init A7105
pebayle 1:b08e4695ffb7 74 ok = rx.init();
pebayle 1:b08e4695ffb7 75 if (ok)
pebayle 1:b08e4695ffb7 76 {
pebayle 1:b08e4695ffb7 77 //bind TX
pebayle 1:b08e4695ffb7 78 ok = rx.bind(); //if bind failed, take default tx id
pebayle 1:b08e4695ffb7 79
pebayle 1:b08e4695ffb7 80 #if DEBUG
pebayle 1:b08e4695ffb7 81 //display bind status
pebayle 1:b08e4695ffb7 82 if (!ok) printf("\r\nbind failed !!!");
pebayle 1:b08e4695ffb7 83 //display tx id (default tx id if bind failed)
pebayle 1:b08e4695ffb7 84 printf("\r\ntx id = %x %x %x %x", rx.txId[3], rx.txId[2], rx.txId[1], rx.txId[0]);
pebayle 1:b08e4695ffb7 85 #endif
pebayle 1:b08e4695ffb7 86
pebayle 1:b08e4695ffb7 87 //infinite loop, receive packet and update servos
pebayle 1:b08e4695ffb7 88 int cptError = 0;
pebayle 1:b08e4695ffb7 89 while (1)
pebayle 1:b08e4695ffb7 90 {
pebayle 1:b08e4695ffb7 91 //wait packet (1.46ms) or timeout(1.5ms) since last call to this function
pebayle 1:b08e4695ffb7 92 switch( rx.waitPacket() )
pebayle 1:b08e4695ffb7 93 {
pebayle 1:b08e4695ffb7 94 case NO_ERROR: //update servo position
pebayle 1:b08e4695ffb7 95 for (i=0; i<6; i++) servoPwmOut[i].pulsewidth_us( rx.servoPulseDur[i] ); break;
pebayle 1:b08e4695ffb7 96 case TIME_OUT_ERROR:
pebayle 1:b08e4695ffb7 97 printf("\r\n%d: time out error", cptError); cptError++; break;
pebayle 1:b08e4695ffb7 98 case VALIDITY_ERROR:
pebayle 1:b08e4695ffb7 99 printf("\r\n%d: validity error", cptError); cptError++; break;
pebayle 1:b08e4695ffb7 100 case TX_ID_ERROR:
pebayle 1:b08e4695ffb7 101 printf("\r\n%d: tx id error", cptError); cptError++; break;
pebayle 1:b08e4695ffb7 102 }
pebayle 1:b08e4695ffb7 103 }
pebayle 1:b08e4695ffb7 104 }
pebayle 1:b08e4695ffb7 105 #if DEBUG
pebayle 1:b08e4695ffb7 106 else
pebayle 1:b08e4695ffb7 107 {
pebayle 1:b08e4695ffb7 108 printf("\r\ninit failed !!!");
pebayle 1:b08e4695ffb7 109 }
pebayle 1:b08e4695ffb7 110 #endif
pebayle 1:b08e4695ffb7 111 }
pebayle 1:b08e4695ffb7 112 */
pebayle 1:b08e4695ffb7 113
pebayle 1:b08e4695ffb7 114 /////////////////////////////////////////////////////////////////////////////////////////////////////
pebayle 1:b08e4695ffb7 115
pebayle 0:01963ae1d0c4 116 #include "mbed.h"
pebayle 0:01963ae1d0c4 117 #include "A7105_FLYSKY_RX.h"
pebayle 0:01963ae1d0c4 118
pebayle 0:01963ae1d0c4 119 A7105_flysky_RX::A7105_flysky_RX( PinName SDIO, PinName GIO1, PinName SCK, PinName CS, PinName GIO2, PinName LED ):
pebayle 0:01963ae1d0c4 120 _spi(SDIO, GIO1, SCK), _cs(CS), _waitRx(GIO2), _statusLed(LED)
pebayle 0:01963ae1d0c4 121 {
pebayle 0:01963ae1d0c4 122 //config SPI
pebayle 0:01963ae1d0c4 123 _spi.format(8, 0); //8 bits, mode 0
pebayle 0:01963ae1d0c4 124 _spi.frequency(4000000); //4MHz
pebayle 0:01963ae1d0c4 125 //CS high
pebayle 0:01963ae1d0c4 126 _cs = 1;
pebayle 0:01963ae1d0c4 127 //init channel counter (0->15)
pebayle 0:01963ae1d0c4 128 channelCnt = 0;
pebayle 0:01963ae1d0c4 129 }
pebayle 0:01963ae1d0c4 130
pebayle 0:01963ae1d0c4 131 //////////////
pebayle 0:01963ae1d0c4 132 // init
pebayle 0:01963ae1d0c4 133 //////////////
pebayle 0:01963ae1d0c4 134
pebayle 0:01963ae1d0c4 135 bool A7105_flysky_RX::init(void)
pebayle 0:01963ae1d0c4 136 {
pebayle 0:01963ae1d0c4 137 unsigned char flysky_id_read[4], i;
pebayle 0:01963ae1d0c4 138
pebayle 0:01963ae1d0c4 139 //wait 10ms to let A7105 wake up
pebayle 0:01963ae1d0c4 140 wait(0.010);
pebayle 0:01963ae1d0c4 141
pebayle 0:01963ae1d0c4 142 //reset A7105
pebayle 0:01963ae1d0c4 143 writeRegVal(0x00, 0x00);
pebayle 0:01963ae1d0c4 144
pebayle 0:01963ae1d0c4 145 //init registers A7105
pebayle 0:01963ae1d0c4 146 for (i=0; i<0x33; i++)
pebayle 0:01963ae1d0c4 147 if (A7105_regs_val[i] != 0xFF)
pebayle 0:01963ae1d0c4 148 writeRegVal(i, A7105_regs_val[i]);
pebayle 0:01963ae1d0c4 149
pebayle 0:01963ae1d0c4 150 //set flysky protocol id <- 0x2AC57554
pebayle 0:01963ae1d0c4 151 _cs = 0;
pebayle 0:01963ae1d0c4 152 _spi.write(0x06); //set ID command
pebayle 0:01963ae1d0c4 153 for(i=0; i<4; i++) _spi.write(flysky_id[i]); //set 4 bytes
pebayle 0:01963ae1d0c4 154 _cs = 1;
pebayle 0:01963ae1d0c4 155
pebayle 0:01963ae1d0c4 156 //read flysky protocol id -> id[0..3]
pebayle 0:01963ae1d0c4 157 _cs = 0;
pebayle 0:01963ae1d0c4 158 _spi.write(0x46); //read ID
pebayle 0:01963ae1d0c4 159 for (i=0; i<4; i++) flysky_id_read[i] = _spi.write(0x00); //read 4 bytes (send dummy bytes 0x00)
pebayle 0:01963ae1d0c4 160 _cs = 1;
pebayle 0:01963ae1d0c4 161
pebayle 0:01963ae1d0c4 162 //check flysky id read
pebayle 0:01963ae1d0c4 163 for (i=0; i<4; i++)
pebayle 0:01963ae1d0c4 164 {
pebayle 0:01963ae1d0c4 165 if (flysky_id_read[i] != flysky_id[i])
pebayle 0:01963ae1d0c4 166 {
pebayle 0:01963ae1d0c4 167 return false;
pebayle 0:01963ae1d0c4 168 }
pebayle 0:01963ae1d0c4 169 }
pebayle 0:01963ae1d0c4 170
pebayle 0:01963ae1d0c4 171 //standby mode
pebayle 0:01963ae1d0c4 172 writeCmd( 0xA0); //A0 -> standby mode
pebayle 0:01963ae1d0c4 173
pebayle 0:01963ae1d0c4 174 //IF Filter Bank Calibration
pebayle 0:01963ae1d0c4 175 writeRegVal( 0x02, 0x01 );
pebayle 0:01963ae1d0c4 176 while ( readReg(0x02) ); //wait calibration end
pebayle 0:01963ae1d0c4 177
pebayle 0:01963ae1d0c4 178 //VCO Current Calibration
pebayle 0:01963ae1d0c4 179 writeRegVal(0x24, 0x013);
pebayle 0:01963ae1d0c4 180
pebayle 0:01963ae1d0c4 181 //VCO Bank Calibration
pebayle 0:01963ae1d0c4 182 writeRegVal(0x26, 0x03B);
pebayle 0:01963ae1d0c4 183
pebayle 0:01963ae1d0c4 184 //VCO Bank Calibrate channel 0x00
pebayle 0:01963ae1d0c4 185 writeRegVal( 0x0F, 0x00); //Set Channel 0
pebayle 0:01963ae1d0c4 186 writeRegVal( 0x02, 0x02); //VCO Calibration
pebayle 0:01963ae1d0c4 187 while ( readReg(0x02) ); //wait calibration end
pebayle 0:01963ae1d0c4 188
pebayle 0:01963ae1d0c4 189 //VCO Bank Calibrate channel 0xA0
pebayle 0:01963ae1d0c4 190 writeRegVal( 0x0F, 0xA0); //Set Channel 0xA0
pebayle 0:01963ae1d0c4 191 writeRegVal( 0x02, 0x02); //VCO Calibration
pebayle 0:01963ae1d0c4 192 while ( readReg(0x02) ); //wait calibration end
pebayle 0:01963ae1d0c4 193
pebayle 0:01963ae1d0c4 194 //Reset VCO Band calibration
pebayle 0:01963ae1d0c4 195 writeRegVal( 0x25, 0x08);
pebayle 0:01963ae1d0c4 196
pebayle 0:01963ae1d0c4 197 return true;
pebayle 0:01963ae1d0c4 198 }
pebayle 0:01963ae1d0c4 199
pebayle 0:01963ae1d0c4 200 //////////////////
pebayle 0:01963ae1d0c4 201 // writeCmd
pebayle 0:01963ae1d0c4 202 //////////////////
pebayle 0:01963ae1d0c4 203
pebayle 0:01963ae1d0c4 204 void A7105_flysky_RX::writeCmd(unsigned char cmd)
pebayle 0:01963ae1d0c4 205 {
pebayle 0:01963ae1d0c4 206 _cs = 0;
pebayle 0:01963ae1d0c4 207 _spi.write(cmd);
pebayle 0:01963ae1d0c4 208 _cs = 1;
pebayle 0:01963ae1d0c4 209 }
pebayle 0:01963ae1d0c4 210
pebayle 0:01963ae1d0c4 211 /////////////////////
pebayle 0:01963ae1d0c4 212 // writeRegVal
pebayle 0:01963ae1d0c4 213 /////////////////////
pebayle 0:01963ae1d0c4 214
pebayle 0:01963ae1d0c4 215 void A7105_flysky_RX::writeRegVal(unsigned char regAddr, unsigned char regVal)
pebayle 0:01963ae1d0c4 216 {
pebayle 0:01963ae1d0c4 217 _cs = 0;
pebayle 0:01963ae1d0c4 218 regAddr = regAddr & 0xBF; //clear R/W bit
pebayle 0:01963ae1d0c4 219 _spi.write(regAddr);
pebayle 0:01963ae1d0c4 220 _spi.write(regVal);
pebayle 0:01963ae1d0c4 221 _cs = 1;
pebayle 0:01963ae1d0c4 222 }
pebayle 0:01963ae1d0c4 223
pebayle 0:01963ae1d0c4 224 /////////////////
pebayle 0:01963ae1d0c4 225 // readReg
pebayle 0:01963ae1d0c4 226 /////////////////
pebayle 0:01963ae1d0c4 227
pebayle 0:01963ae1d0c4 228 unsigned char A7105_flysky_RX::readReg(unsigned char regAddr)
pebayle 0:01963ae1d0c4 229 {
pebayle 0:01963ae1d0c4 230 unsigned char regVal;
pebayle 0:01963ae1d0c4 231 _cs = 0;
pebayle 0:01963ae1d0c4 232 regAddr = regAddr | 0x40; //set R/W bit
pebayle 0:01963ae1d0c4 233 _spi.write(regAddr);
pebayle 0:01963ae1d0c4 234 regVal = _spi.write(0x00);
pebayle 0:01963ae1d0c4 235 _cs = 1;
pebayle 0:01963ae1d0c4 236 return regVal;
pebayle 0:01963ae1d0c4 237 }
pebayle 0:01963ae1d0c4 238
pebayle 0:01963ae1d0c4 239 ////////////////////
pebayle 0:01963ae1d0c4 240 // readPacket
pebayle 0:01963ae1d0c4 241 ////////////////////
pebayle 0:01963ae1d0c4 242
pebayle 0:01963ae1d0c4 243 void A7105_flysky_RX::readPacket(void)
pebayle 0:01963ae1d0c4 244 {
pebayle 0:01963ae1d0c4 245 unsigned char i, highByte, lowByte;
pebayle 0:01963ae1d0c4 246 unsigned int pulseDur;
pebayle 0:01963ae1d0c4 247
pebayle 0:01963ae1d0c4 248 _cs = 0;
pebayle 0:01963ae1d0c4 249 _spi.write(0x45); //cmd read rx packet
pebayle 0:01963ae1d0c4 250 _spi.write(0x00); //read sync
pebayle 0:01963ae1d0c4 251 for (i=0; i<4; i++) packetTxId[i] = _spi.write(0x00); //read tx id: 4 bytes
pebayle 0:01963ae1d0c4 252 for (i=0; i<8; i++) //read pulse duration: 8 x (lowByte, highByte)
pebayle 0:01963ae1d0c4 253 {
pebayle 0:01963ae1d0c4 254 lowByte = _spi.write(0x00);
pebayle 0:01963ae1d0c4 255 highByte = _spi.write(0x00);
pebayle 0:01963ae1d0c4 256 pulseDur = lowByte + ( highByte << 8 );
pebayle 0:01963ae1d0c4 257 if ( pulseDur > 2000 ) pulseDur = 2000;
pebayle 0:01963ae1d0c4 258 if ( pulseDur < 1000 ) pulseDur = 1000;
pebayle 0:01963ae1d0c4 259 packetServoPulseDur[i] = pulseDur;
pebayle 0:01963ae1d0c4 260 }
pebayle 0:01963ae1d0c4 261 _cs = 1;
pebayle 0:01963ae1d0c4 262 }
pebayle 0:01963ae1d0c4 263
pebayle 0:01963ae1d0c4 264 //////////////////
pebayle 0:01963ae1d0c4 265 // bind
pebayle 0:01963ae1d0c4 266 //////////////////
pebayle 0:01963ae1d0c4 267
pebayle 0:01963ae1d0c4 268 //tries to bind TX for approx 3 seconds
pebayle 0:01963ae1d0c4 269 //if bind succeed then return true and update txId
pebayle 0:01963ae1d0c4 270 //if bind failed then return false and take default txId
pebayle 0:01963ae1d0c4 271
pebayle 0:01963ae1d0c4 272 bool A7105_flysky_RX::bind(void)
pebayle 0:01963ae1d0c4 273 {
pebayle 0:01963ae1d0c4 274 short int cpt, i;
pebayle 0:01963ae1d0c4 275 unsigned char modeReg;
pebayle 0:01963ae1d0c4 276 bool bindOk = false;
pebayle 0:01963ae1d0c4 277
pebayle 0:01963ae1d0c4 278 //wait bind packet for 3 seconds (300 loops of 10ms)
pebayle 0:01963ae1d0c4 279 for (cpt=0; cpt<300; cpt++)
pebayle 0:01963ae1d0c4 280 {
pebayle 0:01963ae1d0c4 281 //blink led at 12hz
pebayle 0:01963ae1d0c4 282 if (cpt & 0x0004) _statusLed = 1; else _statusLed = 0;
pebayle 0:01963ae1d0c4 283
pebayle 0:01963ae1d0c4 284 //listen on channel 0
pebayle 0:01963ae1d0c4 285 writeCmd( 0xA0); //A0 -> standby mode
pebayle 0:01963ae1d0c4 286 writeCmd( 0xF0); //F0 -> reset RX FIFO
pebayle 0:01963ae1d0c4 287 writeRegVal( 0x0F, 0x00 ); //Set Channel 0
pebayle 0:01963ae1d0c4 288 writeCmd( 0xC0 ); //C0 -> get into RX mode
pebayle 0:01963ae1d0c4 289
pebayle 0:01963ae1d0c4 290 //wait WAIT_RX to setup (takes about 1us)
pebayle 0:01963ae1d0c4 291 while ( !_waitRx );
pebayle 0:01963ae1d0c4 292
pebayle 0:01963ae1d0c4 293 //wait 10ms to receive packet
pebayle 0:01963ae1d0c4 294 wait(0.010);
pebayle 0:01963ae1d0c4 295
pebayle 0:01963ae1d0c4 296 //test receive bind packet
pebayle 0:01963ae1d0c4 297 if ( !_waitRx )
pebayle 0:01963ae1d0c4 298 {
pebayle 0:01963ae1d0c4 299 modeReg = readReg(0x00); //read mode register
pebayle 0:01963ae1d0c4 300 if ( !(modeReg & 0x60) ) //test valid packet ( CRC(bit5) and CEF(bit6) = 0 )
pebayle 0:01963ae1d0c4 301 {
pebayle 0:01963ae1d0c4 302 bindOk = true; //bind ok
pebayle 0:01963ae1d0c4 303 readPacket(); //read packet
pebayle 0:01963ae1d0c4 304 for (i=0; i<4; i++) //txId <- packetTxId
pebayle 0:01963ae1d0c4 305 txId[i] = packetTxId[i];
pebayle 0:01963ae1d0c4 306 break; //exit loop
pebayle 0:01963ae1d0c4 307 }
pebayle 0:01963ae1d0c4 308 }
pebayle 0:01963ae1d0c4 309 }
pebayle 0:01963ae1d0c4 310
pebayle 0:01963ae1d0c4 311 //default id if failed
pebayle 0:01963ae1d0c4 312 if (!bindOk) for (cpt=0; cpt<4; cpt++)
pebayle 0:01963ae1d0c4 313 txId[cpt] = default_tx_id[cpt]; //txId <- pdefault_tx_id
pebayle 3:19d3601a7744 314
pebayle 0:01963ae1d0c4 315 //led off
pebayle 0:01963ae1d0c4 316 _statusLed = 1;
pebayle 0:01963ae1d0c4 317
pebayle 0:01963ae1d0c4 318 return bindOk;
pebayle 0:01963ae1d0c4 319 }
pebayle 0:01963ae1d0c4 320
pebayle 0:01963ae1d0c4 321 ////////////////////////////////////
pebayle 0:01963ae1d0c4 322 // waitPacket
pebayle 0:01963ae1d0c4 323 ////////////////////////////////////
pebayle 0:01963ae1d0c4 324
pebayle 0:01963ae1d0c4 325 //return error code
pebayle 0:01963ae1d0c4 326
pebayle 0:01963ae1d0c4 327 unsigned char A7105_flysky_RX::waitPacket(void)
pebayle 0:01963ae1d0c4 328 {
pebayle 0:01963ae1d0c4 329 unsigned char channelNbr;
pebayle 0:01963ae1d0c4 330 int begin, now;
pebayle 0:01963ae1d0c4 331 unsigned char modeReg, i;
pebayle 0:01963ae1d0c4 332 unsigned char errorCode = NO_ERROR;
pebayle 0:01963ae1d0c4 333
pebayle 0:01963ae1d0c4 334 //wait time out(1.5ms) or packet received
pebayle 0:01963ae1d0c4 335 timer.start();
pebayle 0:01963ae1d0c4 336 begin = timer.read_us();
pebayle 0:01963ae1d0c4 337 now = begin;
pebayle 0:01963ae1d0c4 338 while ( ( ( now - begin ) < 1500 ) & _waitRx )
pebayle 0:01963ae1d0c4 339 {
pebayle 0:01963ae1d0c4 340 now = timer.read_us();
pebayle 0:01963ae1d0c4 341 }
pebayle 0:01963ae1d0c4 342
pebayle 0:01963ae1d0c4 343 //test packet received or time out
pebayle 0:01963ae1d0c4 344 if (!_waitRx)
pebayle 0:01963ae1d0c4 345 //packet received, check validity
pebayle 0:01963ae1d0c4 346 {
pebayle 0:01963ae1d0c4 347 modeReg = readReg(0x00); //read mode register
pebayle 0:01963ae1d0c4 348 if ( !(modeReg & 0x60) ) //test valid packet ( CRC(bit5) and CEF(bit6) = 0 )
pebayle 0:01963ae1d0c4 349 {
pebayle 0:01963ae1d0c4 350 //valid packet -> read packet
pebayle 0:01963ae1d0c4 351 readPacket();
pebayle 0:01963ae1d0c4 352
pebayle 0:01963ae1d0c4 353 //check txId
pebayle 0:01963ae1d0c4 354 if ( ( txId[0] == packetTxId[0] )
pebayle 0:01963ae1d0c4 355 &( txId[1] == packetTxId[1] )
pebayle 0:01963ae1d0c4 356 &( txId[2] == packetTxId[2] )
pebayle 0:01963ae1d0c4 357 &( txId[3] == packetTxId[3] ) )
pebayle 0:01963ae1d0c4 358 {
pebayle 0:01963ae1d0c4 359 //led on
pebayle 0:01963ae1d0c4 360 _statusLed = 0;
pebayle 0:01963ae1d0c4 361
pebayle 0:01963ae1d0c4 362 //update servo pulse duration
pebayle 0:01963ae1d0c4 363 for (i=0; i<8; i++) servoPulseDur[i] = packetServoPulseDur[i];
pebayle 0:01963ae1d0c4 364 }
pebayle 0:01963ae1d0c4 365 else
pebayle 0:01963ae1d0c4 366 {
pebayle 0:01963ae1d0c4 367 errorCode = TX_ID_ERROR;
pebayle 0:01963ae1d0c4 368 //led off
pebayle 0:01963ae1d0c4 369 _statusLed = 1;
pebayle 0:01963ae1d0c4 370 }
pebayle 0:01963ae1d0c4 371 }
pebayle 0:01963ae1d0c4 372 else
pebayle 0:01963ae1d0c4 373 {
pebayle 0:01963ae1d0c4 374 errorCode = VALIDITY_ERROR;
pebayle 0:01963ae1d0c4 375 //led off
pebayle 0:01963ae1d0c4 376 _statusLed = 1;
pebayle 0:01963ae1d0c4 377 }
pebayle 0:01963ae1d0c4 378 }
pebayle 0:01963ae1d0c4 379 else
pebayle 0:01963ae1d0c4 380 //time out
pebayle 0:01963ae1d0c4 381 {
pebayle 0:01963ae1d0c4 382 errorCode = TIME_OUT_ERROR;
pebayle 0:01963ae1d0c4 383
pebayle 0:01963ae1d0c4 384 //led off
pebayle 0:01963ae1d0c4 385 _statusLed = 1;
pebayle 0:01963ae1d0c4 386
pebayle 0:01963ae1d0c4 387 //increment channel counter
pebayle 0:01963ae1d0c4 388 channelCnt++; if ( channelCnt > 15 ) channelCnt = 0;
pebayle 0:01963ae1d0c4 389 }
pebayle 0:01963ae1d0c4 390
pebayle 0:01963ae1d0c4 391 //increment channel counter
pebayle 0:01963ae1d0c4 392 channelCnt++; if ( channelCnt > 15 ) channelCnt = 0;
pebayle 0:01963ae1d0c4 393
pebayle 0:01963ae1d0c4 394 //get channel number into "A7105_tx_channels" table
pebayle 3:19d3601a7744 395 channelOffset = txId[0] >> 4; if ( channelOffset > 9 ) channelOffset = 9;
pebayle 0:01963ae1d0c4 396 channelNbr = A7105_tx_channels[ txId[0] & 0x0F ][ channelCnt ] - channelOffset - 1;
pebayle 0:01963ae1d0c4 397
pebayle 0:01963ae1d0c4 398 //listen to channel number
pebayle 0:01963ae1d0c4 399 writeCmd( 0xA0); //A0 -> standby mode (does a falling edge on WAIT_RX pin !!!)
pebayle 0:01963ae1d0c4 400 writeCmd( 0xF0); //F0 -> reset RX FIFO
pebayle 0:01963ae1d0c4 401 writeRegVal( 0x0F, channelNbr ); //Set Channel number
pebayle 0:01963ae1d0c4 402 writeCmd( 0xC0 ); //C0 -> get into RX mode
pebayle 0:01963ae1d0c4 403
pebayle 0:01963ae1d0c4 404 //result of previous listening...
pebayle 0:01963ae1d0c4 405 return errorCode;
pebayle 0:01963ae1d0c4 406 }