diff --git a/Multiprotocol/CC2500_SPI.ino b/Multiprotocol/CC2500_SPI.ino index ad37973..fc3053a 100644 --- a/Multiprotocol/CC2500_SPI.ino +++ b/Multiprotocol/CC2500_SPI.ino @@ -24,138 +24,138 @@ //---------------------------- void CC2500_WriteReg(uint8_t address, uint8_t data) { - CC25_CSN_off; - SPI_Write(address); - NOP(); - SPI_Write(data); - CC25_CSN_on; -} + CC25_CSN_off; + SPI_Write(address); + NOP(); + SPI_Write(data); + CC25_CSN_on; +} //---------------------- static void CC2500_ReadRegisterMulti(uint8_t address, uint8_t data[], uint8_t length) { - CC25_CSN_off; - SPI_Write(CC2500_READ_BURST | address); - for(uint8_t i = 0; i < length; i++) - data[i] = SPI_Read(); - CC25_CSN_on; + CC25_CSN_off; + SPI_Write(CC2500_READ_BURST | address); + for(uint8_t i = 0; i < length; i++) + data[i] = SPI_Read(); + CC25_CSN_on; } //-------------------------------------------- static uint8_t CC2500_ReadReg(uint8_t address) -{ - uint8_t result; - CC25_CSN_off; - SPI_Write(CC2500_READ_SINGLE | address); - result = SPI_Read(); - CC25_CSN_on; - return(result); -} +{ + uint8_t result; + CC25_CSN_off; + SPI_Write(CC2500_READ_SINGLE | address); + result = SPI_Read(); + CC25_CSN_on; + return(result); +} //------------------------ void CC2500_ReadData(uint8_t *dpbuffer, uint8_t len) { - CC2500_ReadRegisterMulti(CC2500_3F_RXFIFO, dpbuffer, len); + CC2500_ReadRegisterMulti(CC2500_3F_RXFIFO, dpbuffer, len); } //********************************************* void CC2500_Strobe(uint8_t state) { - CC25_CSN_off; - SPI_Write(state); - CC25_CSN_on; + CC25_CSN_off; + SPI_Write(state); + CC25_CSN_on; } static void CC2500_WriteRegisterMulti(uint8_t address, const uint8_t data[], uint8_t length) { - CC25_CSN_off; - SPI_Write(CC2500_WRITE_BURST | address); - for(uint8_t i = 0; i < length; i++) - SPI_Write(data[i]); - CC25_CSN_on; + CC25_CSN_off; + SPI_Write(CC2500_WRITE_BURST | address); + for(uint8_t i = 0; i < length; i++) + SPI_Write(data[i]); + CC25_CSN_on; } void CC2500_WriteData(uint8_t *dpbuffer, uint8_t len) { - CC2500_Strobe(CC2500_SFTX); - CC2500_WriteRegisterMulti(CC2500_3F_TXFIFO, dpbuffer, len); - CC2500_Strobe(CC2500_STX); + CC2500_Strobe(CC2500_SFTX); + CC2500_WriteRegisterMulti(CC2500_3F_TXFIFO, dpbuffer, len); + CC2500_Strobe(CC2500_STX); } void CC2500_SetTxRxMode(uint8_t mode) { - if(mode == TX_EN) - {//from deviation firmware - CC2500_WriteReg(CC2500_00_IOCFG2, 0x2F); - CC2500_WriteReg(CC2500_02_IOCFG0, 0x2F | 0x40); - } - else - if (mode == RX_EN) - { - CC2500_WriteReg(CC2500_02_IOCFG0, 0x2F); - CC2500_WriteReg(CC2500_00_IOCFG2, 0x2F | 0x40); - } - else - { - CC2500_WriteReg(CC2500_02_IOCFG0, 0x2F); - CC2500_WriteReg(CC2500_00_IOCFG2, 0x2F); - } + if(mode == TX_EN) + {//from deviation firmware + CC2500_WriteReg(CC2500_00_IOCFG2, 0x2F); + CC2500_WriteReg(CC2500_02_IOCFG0, 0x2F | 0x40); + } + else + if (mode == RX_EN) + { + CC2500_WriteReg(CC2500_02_IOCFG0, 0x2F); + CC2500_WriteReg(CC2500_00_IOCFG2, 0x2F | 0x40); + } + else + { + CC2500_WriteReg(CC2500_02_IOCFG0, 0x2F); + CC2500_WriteReg(CC2500_00_IOCFG2, 0x2F); + } } //------------------------ /*static void cc2500_resetChip(void) { - // Toggle chip select signal - CC25_CSN_on; - delayMicroseconds(30); - CC25_CSN_off; - delayMicroseconds(30); - CC25_CSN_on; - delayMicroseconds(45); - CC2500_Strobe(CC2500_SRES); - _delay_ms(100); + // Toggle chip select signal + CC25_CSN_on; + delayMicroseconds(30); + CC25_CSN_off; + delayMicroseconds(30); + CC25_CSN_on; + delayMicroseconds(45); + CC2500_Strobe(CC2500_SRES); + _delay_ms(100); } */ uint8_t CC2500_Reset() { - CC2500_Strobe(CC2500_SRES); - delay(1); - CC2500_SetTxRxMode(TXRX_OFF); - return CC2500_ReadReg(CC2500_0E_FREQ1) == 0xC4;//check if reset + CC2500_Strobe(CC2500_SRES); + delay(1); + CC2500_SetTxRxMode(TXRX_OFF); + return CC2500_ReadReg(CC2500_0E_FREQ1) == 0xC4;//check if reset } /* static void CC2500_SetPower_Value(uint8_t power) { - const unsigned char patable[8]= { - 0xC5, // -12dbm - 0x97, // -10dbm - 0x6E, // -8dbm - 0x7F, // -6dbm - 0xA9, // -4dbm - 0xBB, // -2dbm - 0xFE, // 0dbm - 0xFF // 1.5dbm - }; - if (power > 7) - power = 7; - CC2500_WriteReg(CC2500_3E_PATABLE, patable[power]); + const unsigned char patable[8]= { + 0xC5, // -12dbm + 0x97, // -10dbm + 0x6E, // -8dbm + 0x7F, // -6dbm + 0xA9, // -4dbm + 0xBB, // -2dbm + 0xFE, // 0dbm + 0xFF // 1.5dbm + }; + if (power > 7) + power = 7; + CC2500_WriteReg(CC2500_3E_PATABLE, patable[power]); } */ void CC2500_SetPower() { - uint8_t power=CC2500_BIND_POWER; - if(IS_BIND_DONE) - #ifdef CC2500_ENABLE_LOW_POWER - power=IS_POWER_FLAG_on?CC2500_HIGH_POWER:CC2500_LOW_POWER; - #else - power=CC2500_HIGH_POWER; - #endif - if(IS_RANGE_FLAG_on) - power=CC2500_RANGE_POWER; - if(prev_power != power) - { - CC2500_WriteReg(CC2500_3E_PATABLE, power); - prev_power=power; - } + uint8_t power=CC2500_BIND_POWER; + if(IS_BIND_DONE) + #ifdef CC2500_ENABLE_LOW_POWER + power=IS_POWER_FLAG_on?CC2500_HIGH_POWER:CC2500_LOW_POWER; + #else + power=CC2500_HIGH_POWER; + #endif + if(IS_RANGE_FLAG_on) + power=CC2500_RANGE_POWER; + if(prev_power != power) + { + CC2500_WriteReg(CC2500_3E_PATABLE, power); + prev_power=power; + } } #endif diff --git a/Multiprotocol/FrSkyD_cc2500.ino b/Multiprotocol/FrSkyD_cc2500.ino index 1127ec7..32d8d46 100644 --- a/Multiprotocol/FrSkyD_cc2500.ino +++ b/Multiprotocol/FrSkyD_cc2500.ino @@ -19,197 +19,197 @@ static void __attribute__((unused)) frsky2way_init(uint8_t bind) { //debugln("frsky2way_init"); - FRSKY_init_cc2500(FRSKYD_cc2500_conf); + FRSKY_init_cc2500(FRSKYD_cc2500_conf); - CC2500_WriteReg(CC2500_09_ADDR, bind ? 0x03 : rx_tx_addr[3]); - CC2500_WriteReg(CC2500_07_PKTCTRL1, 0x05); - CC2500_Strobe(CC2500_SIDLE); // Go to idle... - // - CC2500_WriteReg(CC2500_0A_CHANNR, 0x00); - CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); - CC2500_Strobe(CC2500_SFRX); - //#######END INIT######## + CC2500_WriteReg(CC2500_09_ADDR, bind ? 0x03 : rx_tx_addr[3]); + CC2500_WriteReg(CC2500_07_PKTCTRL1, 0x05); + CC2500_Strobe(CC2500_SIDLE); // Go to idle... + // + CC2500_WriteReg(CC2500_0A_CHANNR, 0x00); + CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); + CC2500_Strobe(CC2500_SFRX); + //#######END INIT######## } - + static void __attribute__((unused)) frsky2way_build_bind_packet() { //debugln("build bind"); - //11 03 01 d7 2d 00 00 1e 3c 5b 78 00 00 00 00 00 00 01 - //11 03 01 19 3e 00 02 8e 2f bb 5c 00 00 00 00 00 00 01 - packet[0] = 0x11; - packet[1] = 0x03; - packet[2] = 0x01; - packet[3] = rx_tx_addr[3]; - packet[4] = rx_tx_addr[2]; - uint16_t idx = ((state -FRSKY_BIND) % 10) * 5; - packet[5] = idx; - packet[6] = hopping_frequency[idx++]; - packet[7] = hopping_frequency[idx++]; - packet[8] = hopping_frequency[idx++]; - packet[9] = hopping_frequency[idx++]; - packet[10] = hopping_frequency[idx++]; - packet[11] = 0x00; - packet[12] = 0x00; - packet[13] = 0x00; - packet[14] = 0x00; - packet[15] = 0x00; - packet[16] = 0x00; - packet[17] = 0x01; + //11 03 01 d7 2d 00 00 1e 3c 5b 78 00 00 00 00 00 00 01 + //11 03 01 19 3e 00 02 8e 2f bb 5c 00 00 00 00 00 00 01 + packet[0] = 0x11; + packet[1] = 0x03; + packet[2] = 0x01; + packet[3] = rx_tx_addr[3]; + packet[4] = rx_tx_addr[2]; + uint16_t idx = ((state -FRSKY_BIND) % 10) * 5; + packet[5] = idx; + packet[6] = hopping_frequency[idx++]; + packet[7] = hopping_frequency[idx++]; + packet[8] = hopping_frequency[idx++]; + packet[9] = hopping_frequency[idx++]; + packet[10] = hopping_frequency[idx++]; + packet[11] = 0x00; + packet[12] = 0x00; + packet[13] = 0x00; + packet[14] = 0x00; + packet[15] = 0x00; + packet[16] = 0x00; + packet[17] = 0x01; } static void __attribute__((unused)) frsky2way_data_frame() { //pachet[4] is telemetry user frame counter(hub) - //11 d7 2d 22 00 01 c9 c9 ca ca 88 88 ca ca c9 ca 88 88 - //11 57 12 00 00 01 f2 f2 f2 f2 06 06 ca ca ca ca 18 18 - packet[0] = 0x11; //Length - packet[1] = rx_tx_addr[3]; - packet[2] = rx_tx_addr[2]; - packet[3] = counter;// - #if defined TELEMETRY - packet[4] = telemetry_counter; - #else - packet[4] = 0x00; - #endif + //11 d7 2d 22 00 01 c9 c9 ca ca 88 88 ca ca c9 ca 88 88 + //11 57 12 00 00 01 f2 f2 f2 f2 06 06 ca ca ca ca 18 18 + packet[0] = 0x11; //Length + packet[1] = rx_tx_addr[3]; + packet[2] = rx_tx_addr[2]; + packet[3] = counter;// + #if defined TELEMETRY + packet[4] = telemetry_counter; + #else + packet[4] = 0x00; + #endif - packet[5] = 0x01; - // - packet[10] = 0; - packet[11] = 0; - packet[16] = 0; - packet[17] = 0; - for(uint8_t i = 0; i < 8; i++) - { - uint16_t value; - value = convert_channel_frsky(i); - if(i < 4) - { - packet[6+i] = value & 0xff; - packet[10+(i>>1)] |= ((value >> 8) & 0x0f) << (4 *(i & 0x01)); - } - else - { - packet[8+i] = value & 0xff; - packet[16+((i-4)>>1)] |= ((value >> 8) & 0x0f) << (4 * ((i-4) & 0x01)); - } - } -} + packet[5] = 0x01; + // + packet[10] = 0; + packet[11] = 0; + packet[16] = 0; + packet[17] = 0; + for(uint8_t i = 0; i < 8; i++) + { + uint16_t value; + value = convert_channel_frsky(i); + if(i < 4) + { + packet[6+i] = value & 0xff; + packet[10+(i>>1)] |= ((value >> 8) & 0x0f) << (4 *(i & 0x01)); + } + else + { + packet[8+i] = value & 0xff; + packet[16+((i-4)>>1)] |= ((value >> 8) & 0x0f) << (4 * ((i-4) & 0x01)); + } + } +} uint16_t initFrSky_2way() { - Frsky_init_hop(); - packet_count=0; - - if(IS_BIND_IN_PROGRESS) - { - frsky2way_init(1); - state = FRSKY_BIND; + Frsky_init_hop(); + packet_count=0; + + if(IS_BIND_IN_PROGRESS) + { + frsky2way_init(1); + state = FRSKY_BIND; debugln("initFrSky_2way bind"); - } - else - { - state = FRSKY_BIND_DONE; + } + else + { + state = FRSKY_BIND_DONE; debugln("initFrSky_2way bind done"); - } - return 10000; -} - + } + return 10000; +} + uint16_t ReadFrSky_2way() -{ +{ //debugln("%s",__func__); - if (state < FRSKY_BIND_DONE) - { - frsky2way_build_bind_packet(); - CC2500_Strobe(CC2500_SIDLE); - CC2500_WriteReg(CC2500_0A_CHANNR, 0x00); - CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); - CC2500_Strobe(CC2500_SFRX);//0x3A - CC2500_WriteData(packet, packet[0]+1); - if(IS_BIND_DONE) { - state = FRSKY_BIND; + if (state < FRSKY_BIND_DONE) + { + frsky2way_build_bind_packet(); + CC2500_Strobe(CC2500_SIDLE); + CC2500_WriteReg(CC2500_0A_CHANNR, 0x00); + CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); + CC2500_Strobe(CC2500_SFRX);//0x3A + CC2500_WriteData(packet, packet[0]+1); + if(IS_BIND_DONE) { + state = FRSKY_BIND; debugln("%s bind done",__func__); - } else - state++; - + } else + state++; + if(state == FRSKY_BIND_DONE) { debugln("%s bind done fr",__func__); } - return 9000; - } - if (state == FRSKY_BIND_DONE) - { - //debugln("%s bind done",__func__); + return 9000; + } + if (state == FRSKY_BIND_DONE) + { + //debugln("%s bind done",__func__); + + state = FRSKY_DATA2; + frsky2way_init(0); + counter = 0; + BIND_DONE; + } + else + if (state == FRSKY_DATA5) + { + CC2500_Strobe(CC2500_SRX);//0x34 RX enable + state = FRSKY_DATA1; + return 9200; + } + counter = (counter + 1) % 188; + if (state == FRSKY_DATA4) + { //telemetry receive + CC2500_SetTxRxMode(RX_EN); + CC2500_Strobe(CC2500_SIDLE); + CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[counter % 47]); + CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); + state++; + return 1300; + } + else + { + if (state == FRSKY_DATA1) + { + len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - state = FRSKY_DATA2; - frsky2way_init(0); - counter = 0; - BIND_DONE; - } - else - if (state == FRSKY_DATA5) - { - CC2500_Strobe(CC2500_SRX);//0x34 RX enable - state = FRSKY_DATA1; - return 9200; - } - counter = (counter + 1) % 188; - if (state == FRSKY_DATA4) - { //telemetry receive - CC2500_SetTxRxMode(RX_EN); - CC2500_Strobe(CC2500_SIDLE); - CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[counter % 47]); - CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); - state++; - return 1300; - } - else - { - if (state == FRSKY_DATA1) - { - len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - //debugln("%d len",len); - if (len && len<=(0x11+3))// 20bytes - { - CC2500_ReadData(pkt, len); //received telemetry packets - #if defined(TELEMETRY) - if(pkt[len-1] & 0x80) - {//with valid crc - packet_count=0; - frsky_check_telemetry(pkt,len); //check if valid telemetry packets and buffer them. - } - #endif - } - else - { - packet_count++; - // restart sequence on missed packet - might need count or timeout instead of one missed - if(packet_count>100) - {//~1sec - packet_count=0; - #if defined TELEMETRY - telemetry_link=0;//no link frames - pkt[6]=0;//no user frames. - #endif - } - } - CC2500_SetTxRxMode(TX_EN); - CC2500_SetPower(); // Set tx_power - } - CC2500_Strobe(CC2500_SIDLE); - CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[counter % 47]); - if ( prev_option != option ) - { - CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack - prev_option = option ; - } - CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); - CC2500_Strobe(CC2500_SFRX); - frsky2way_data_frame(); - CC2500_WriteData(packet, packet[0]+1); - state++; - } - return state == FRSKY_DATA4 ? 7500 : 9000; + if (len && len<=(0x11+3))// 20bytes + { + CC2500_ReadData(pkt, len); //received telemetry packets + #if defined(TELEMETRY) + if(pkt[len-1] & 0x80) + {//with valid crc + packet_count=0; + frsky_check_telemetry(pkt,len); //check if valid telemetry packets and buffer them. + } + #endif + } + else + { + packet_count++; + // restart sequence on missed packet - might need count or timeout instead of one missed + if(packet_count>100) + {//~1sec + packet_count=0; + #if defined TELEMETRY + telemetry_link=0;//no link frames + pkt[6]=0;//no user frames. + #endif + } + } + CC2500_SetTxRxMode(TX_EN); + CC2500_SetPower(); // Set tx_power + } + CC2500_Strobe(CC2500_SIDLE); + CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[counter % 47]); + if ( prev_option != option ) + { + CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack + prev_option = option ; + } + CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); + CC2500_Strobe(CC2500_SFRX); + frsky2way_data_frame(); + CC2500_WriteData(packet, packet[0]+1); + state++; + } + return state == FRSKY_DATA4 ? 7500 : 9000; } #endif diff --git a/Multiprotocol/FrSkyV_cc2500.ino b/Multiprotocol/FrSkyV_cc2500.ino index f277b2c..a32cf94 100644 --- a/Multiprotocol/FrSkyV_cc2500.ino +++ b/Multiprotocol/FrSkyV_cc2500.ino @@ -18,43 +18,43 @@ #define FRSKYV_BIND_COUNT 200 enum { - FRSKYV_DATA1=0, - FRSKYV_DATA2, - FRSKYV_DATA3, - FRSKYV_DATA4, - FRSKYV_DATA5 + FRSKYV_DATA1=0, + FRSKYV_DATA2, + FRSKYV_DATA3, + FRSKYV_DATA4, + FRSKYV_DATA5 }; #include "iface_cc2500.h" static uint8_t __attribute__((unused)) FRSKYV_crc8(uint8_t result, uint8_t *data, uint8_t len) { - for(uint8_t i = 0; i < len; i++) - { - result = result ^ data[i]; - for(uint8_t j = 0; j < 8; j++) - if(result & 0x80) - result = (result << 1) ^ 0x07; - else - result = result << 1; - } - return result; + for(uint8_t i = 0; i < len; i++) + { + result = result ^ data[i]; + for(uint8_t j = 0; j < 8; j++) + if(result & 0x80) + result = (result << 1) ^ 0x07; + else + result = result << 1; + } + return result; } static uint8_t __attribute__((unused)) FRSKYV_crc8_le(uint8_t *data, uint8_t len) { - uint8_t result = 0xD6; - - for(uint8_t i = 0; i < len; i++) - { - result = result ^ data[i]; - for(uint8_t j = 0; j < 8; j++) - if(result & 0x01) - result = (result >> 1) ^ 0x83; - else - result = result >> 1; - } - return result; + uint8_t result = 0xD6; + + for(uint8_t i = 0; i < len; i++) + { + result = result ^ data[i]; + for(uint8_t j = 0; j < 8; j++) + if(result & 0x01) + result = (result >> 1) ^ 0x83; + else + result = result >> 1; + } + return result; } static void __attribute__((unused)) FRSKYV_build_bind_packet() @@ -79,87 +79,87 @@ static void __attribute__((unused)) FRSKYV_build_bind_packet() static uint8_t __attribute__((unused)) FRSKYV_calc_channel() { - uint32_t temp=seed; - temp = (temp * 0xaa) % 0x7673; - seed = temp; - return (seed & 0xff) % 0x32; + uint32_t temp=seed; + temp = (temp * 0xaa) % 0x7673; + seed = temp; + return (seed & 0xff) % 0x32; } static void __attribute__((unused)) FRSKYV_build_data_packet() { - uint8_t idx = 0; // transmit lower channels - - packet[0] = 0x0e; - packet[1] = rx_tx_addr[3]; - packet[2] = rx_tx_addr[2]; - packet[3] = seed & 0xff; - packet[4] = seed >> 8; - if (phase == FRSKYV_DATA1 || phase == FRSKYV_DATA3) - packet[5] = 0x0f; - else - if(phase == FRSKYV_DATA2 || phase == FRSKYV_DATA4) - { - packet[5] = 0xf0; - idx=4; // transmit upper channels - } - else - packet[5] = 0x00; - for(uint8_t i = 0; i < 4; i++) - { - uint16_t value = convert_channel_frsky(i+idx); - packet[2*i + 6] = value & 0xff; - packet[2*i + 7] = value >> 8; - } - packet[14] = FRSKYV_crc8(crc8, packet, 14); + uint8_t idx = 0; // transmit lower channels + + packet[0] = 0x0e; + packet[1] = rx_tx_addr[3]; + packet[2] = rx_tx_addr[2]; + packet[3] = seed & 0xff; + packet[4] = seed >> 8; + if (phase == FRSKYV_DATA1 || phase == FRSKYV_DATA3) + packet[5] = 0x0f; + else + if(phase == FRSKYV_DATA2 || phase == FRSKYV_DATA4) + { + packet[5] = 0xf0; + idx=4; // transmit upper channels + } + else + packet[5] = 0x00; + for(uint8_t i = 0; i < 4; i++) + { + uint16_t value = convert_channel_frsky(i+idx); + packet[2*i + 6] = value & 0xff; + packet[2*i + 7] = value >> 8; + } + packet[14] = FRSKYV_crc8(crc8, packet, 14); } uint16_t ReadFRSKYV() { - if(IS_BIND_DONE) - { // Normal operation - uint8_t chan = FRSKYV_calc_channel(); - CC2500_Strobe(CC2500_SIDLE); - if (option != prev_option) - { - CC2500_WriteReg(CC2500_0C_FSCTRL0, option); - prev_option=option; - } - CC2500_WriteReg(CC2500_0A_CHANNR, chan * 5 + 6); - FRSKYV_build_data_packet(); - - if (phase == FRSKYV_DATA5) - { - CC2500_SetPower(); - phase = FRSKYV_DATA1; - } - else - phase++; - - CC2500_WriteData(packet, packet[0]+1); - return 9006; - } - // Bind mode - FRSKYV_build_bind_packet(); - CC2500_Strobe(CC2500_SIDLE); - CC2500_WriteReg(CC2500_0A_CHANNR, 0x00); - CC2500_WriteData(packet, packet[0]+1); - binding_idx++; - if(binding_idx>=FRSKYV_BIND_COUNT) - BIND_DONE; - return 53460; + if(IS_BIND_DONE) + { // Normal operation + uint8_t chan = FRSKYV_calc_channel(); + CC2500_Strobe(CC2500_SIDLE); + if (option != prev_option) + { + CC2500_WriteReg(CC2500_0C_FSCTRL0, option); + prev_option=option; + } + CC2500_WriteReg(CC2500_0A_CHANNR, chan * 5 + 6); + FRSKYV_build_data_packet(); + + if (phase == FRSKYV_DATA5) + { + CC2500_SetPower(); + phase = FRSKYV_DATA1; + } + else + phase++; + + CC2500_WriteData(packet, packet[0]+1); + return 9006; + } + // Bind mode + FRSKYV_build_bind_packet(); + CC2500_Strobe(CC2500_SIDLE); + CC2500_WriteReg(CC2500_0A_CHANNR, 0x00); + CC2500_WriteData(packet, packet[0]+1); + binding_idx++; + if(binding_idx>=FRSKYV_BIND_COUNT) + BIND_DONE; + return 53460; } uint16_t initFRSKYV() { - //ID is 15 bits. Using rx_tx_addr[2] and rx_tx_addr[3] since we want to use RX_Num for model match - rx_tx_addr[2]&=0x7F; - crc8 = FRSKYV_crc8_le(rx_tx_addr+2, 2); - - FRSKY_init_cc2500(FRSKYV_cc2500_conf); - seed = 1; - binding_idx=0; - phase = FRSKYV_DATA1; - return 10000; + //ID is 15 bits. Using rx_tx_addr[2] and rx_tx_addr[3] since we want to use RX_Num for model match + rx_tx_addr[2]&=0x7F; + crc8 = FRSKYV_crc8_le(rx_tx_addr+2, 2); + + FRSKY_init_cc2500(FRSKYV_cc2500_conf); + seed = 1; + binding_idx=0; + phase = FRSKYV_DATA1; + return 10000; } #endif diff --git a/Multiprotocol/FrSkyX_cc2500.ino b/Multiprotocol/FrSkyX_cc2500.ino index d9bea04..9522e41 100644 --- a/Multiprotocol/FrSkyX_cc2500.ino +++ b/Multiprotocol/FrSkyX_cc2500.ino @@ -1,18 +1,18 @@ /* ************************** - * By Midelic on RCGroups * - ************************** - 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. - - Multiprotocol 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 Multiprotocol. If not, see . + * By Midelic on RCGroups * + ************************** + 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. + + Multiprotocol 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 Multiprotocol. If not, see . */ #if defined(FRSKYX_CC2500_INO) @@ -27,335 +27,335 @@ uint8_t FrX_receive_seq ; static void __attribute__((unused)) frskyX_set_start(uint8_t ch ) { - CC2500_Strobe(CC2500_SIDLE); - CC2500_WriteReg(CC2500_25_FSCAL1, calData[ch]); - CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[ch]); -} + CC2500_Strobe(CC2500_SIDLE); + CC2500_WriteReg(CC2500_25_FSCAL1, calData[ch]); + CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[ch]); +} static void __attribute__((unused)) frskyX_init() { - FRSKY_init_cc2500((sub_protocol&2)?FRSKYXEU_cc2500_conf:FRSKYX_cc2500_conf); // LBT or FCC - // - for(uint8_t c=0;c < 48;c++) - {//calibrate hop channels - CC2500_Strobe(CC2500_SIDLE); - CC2500_WriteReg(CC2500_0A_CHANNR,hopping_frequency[c]); - CC2500_Strobe(CC2500_SCAL); - delayMicroseconds(900);// - calData[c] = CC2500_ReadReg(CC2500_25_FSCAL1); - } - //#######END INIT######## + FRSKY_init_cc2500((sub_protocol&2)?FRSKYXEU_cc2500_conf:FRSKYX_cc2500_conf); // LBT or FCC + // + for(uint8_t c=0;c < 48;c++) + {//calibrate hop channels + CC2500_Strobe(CC2500_SIDLE); + CC2500_WriteReg(CC2500_0A_CHANNR,hopping_frequency[c]); + CC2500_Strobe(CC2500_SCAL); + delayMicroseconds(900);// + calData[c] = CC2500_ReadReg(CC2500_25_FSCAL1); + } + //#######END INIT######## } static void __attribute__((unused)) frskyX_initialize_data(uint8_t adr) { - CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack - CC2500_WriteReg(CC2500_18_MCSM0, 0x8); - CC2500_WriteReg(CC2500_09_ADDR, adr ? 0x03 : rx_tx_addr[3]); - CC2500_WriteReg(CC2500_07_PKTCTRL1,0x05); + CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack + CC2500_WriteReg(CC2500_18_MCSM0, 0x8); + CC2500_WriteReg(CC2500_09_ADDR, adr ? 0x03 : rx_tx_addr[3]); + CC2500_WriteReg(CC2500_07_PKTCTRL1,0x05); } //**CRC** const uint16_t PROGMEM frskyX_CRC_Short[]={ - 0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF, - 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7 }; + 0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF, + 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7 }; static uint16_t __attribute__((unused)) frskyX_CRCTable(uint8_t val) { - uint16_t word ; - word = pgm_read_word(&frskyX_CRC_Short[val&0x0F]) ; - val /= 16 ; - return word ^ (0x1081 * val) ; + uint16_t word ; + word = pgm_read_word(&frskyX_CRC_Short[val&0x0F]) ; + val /= 16 ; + return word ^ (0x1081 * val) ; } static uint16_t __attribute__((unused)) frskyX_crc_x(uint8_t *data, uint8_t len) { - uint16_t crc = 0; - for(uint8_t i=0; i < len; i++) - crc = (crc<<8) ^ frskyX_CRCTable((uint8_t)(crc>>8) ^ *data++); - return crc; + uint16_t crc = 0; + for(uint8_t i=0; i < len; i++) + crc = (crc<<8) ^ frskyX_CRCTable((uint8_t)(crc>>8) ^ *data++); + return crc; } static void __attribute__((unused)) frskyX_build_bind_packet() { // debugln("%s:%d build bind", __func__, __LINE__); - packet[0] = (sub_protocol & 2 ) ? 0x20 : 0x1D ; // LBT or FCC - packet[1] = 0x03; - packet[2] = 0x01; - // - packet[3] = rx_tx_addr[3]; - packet[4] = rx_tx_addr[2]; - int idx = ((state -FRSKY_BIND) % 10) * 5; - packet[5] = idx; - packet[6] = hopping_frequency[idx++]; - packet[7] = hopping_frequency[idx++]; - packet[8] = hopping_frequency[idx++]; - packet[9] = hopping_frequency[idx++]; - packet[10] = hopping_frequency[idx++]; - packet[11] = 0x02; - packet[12] = RX_num; - // - uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ; - memset(&packet[13], 0, limit - 13); - uint16_t lcrc = frskyX_crc_x(&packet[3], limit-3); - // - packet[limit++] = lcrc >> 8; - packet[limit] = lcrc; - // + packet[0] = (sub_protocol & 2 ) ? 0x20 : 0x1D ; // LBT or FCC + packet[1] = 0x03; + packet[2] = 0x01; + // + packet[3] = rx_tx_addr[3]; + packet[4] = rx_tx_addr[2]; + int idx = ((state -FRSKY_BIND) % 10) * 5; + packet[5] = idx; + packet[6] = hopping_frequency[idx++]; + packet[7] = hopping_frequency[idx++]; + packet[8] = hopping_frequency[idx++]; + packet[9] = hopping_frequency[idx++]; + packet[10] = hopping_frequency[idx++]; + packet[11] = 0x02; + packet[12] = RX_num; + // + uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ; + memset(&packet[13], 0, limit - 13); + uint16_t lcrc = frskyX_crc_x(&packet[3], limit-3); + // + packet[limit++] = lcrc >> 8; + packet[limit] = lcrc; + // } // 0-2047, 0 = 817, 1024 = 1500, 2047 = 2182 //64=860,1024=1500,1984=2140//Taranis 125% static uint16_t __attribute__((unused)) frskyX_scaleForPXX( uint8_t i ) -{ //mapped 860,2140(125%) range to 64,1984(PXX values); - uint16_t chan_val=convert_channel_frsky(i)-1226; - if(i>7) chan_val|=2048; // upper channels offset - return chan_val; +{ //mapped 860,2140(125%) range to 64,1984(PXX values); + uint16_t chan_val=convert_channel_frsky(i)-1226; + if(i>7) chan_val|=2048; // upper channels offset + return chan_val; } #ifdef FAILSAFE_ENABLE static uint16_t __attribute__((unused)) frskyX_scaleForPXX_FS( uint8_t i ) -{ //mapped 1,2046(125%) range to 64,1984(PXX values); - uint16_t chan_val=((Failsafe_data[i]*15)>>4)+64; - if(Failsafe_data[i]==FAILSAFE_CHANNEL_NOPULSES) - chan_val=FAILSAFE_CHANNEL_NOPULSES; - else if(Failsafe_data[i]==FAILSAFE_CHANNEL_HOLD) - chan_val=FAILSAFE_CHANNEL_HOLD; - if(i>7) chan_val|=2048; // upper channels offset - return chan_val; +{ //mapped 1,2046(125%) range to 64,1984(PXX values); + uint16_t chan_val=((Failsafe_data[i]*15)>>4)+64; + if(Failsafe_data[i]==FAILSAFE_CHANNEL_NOPULSES) + chan_val=FAILSAFE_CHANNEL_NOPULSES; + else if(Failsafe_data[i]==FAILSAFE_CHANNEL_HOLD) + chan_val=FAILSAFE_CHANNEL_HOLD; + if(i>7) chan_val|=2048; // upper channels offset + return chan_val; } #endif #define FRX_FAILSAFE_TIME 1032 static void __attribute__((unused)) frskyX_data_frame() { - //0x1D 0xB3 0xFD 0x02 0x56 0x07 0x15 0x00 0x00 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x08 0x00 0x00 0x00 0x00 0x00 0x00 0x96 0x12 - // - static uint8_t chan_offset=0; - uint16_t chan_0 ; - uint16_t chan_1 ; - // + //0x1D 0xB3 0xFD 0x02 0x56 0x07 0x15 0x00 0x00 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x08 0x00 0x00 0x00 0x00 0x00 0x00 0x96 0x12 + // + static uint8_t chan_offset=0; + uint16_t chan_0 ; + uint16_t chan_1 ; + // // data frames sent every 9ms; failsafe every 9 seconds - #ifdef FAILSAFE_ENABLE - static uint16_t failsafe_count=0; - static uint8_t FS_flag=0,failsafe_chan=0; - if (FS_flag == 0 && failsafe_count > FRX_FAILSAFE_TIME && chan_offset == 0 && IS_FAILSAFE_VALUES_on) - { - FS_flag = 0x10; - failsafe_chan = 0; - } else if (FS_flag & 0x10 && failsafe_chan < (sub_protocol & 0x01 ? 8-1:16-1)) - { - FS_flag = 0x10 | ((FS_flag + 2) & 0x0F); //10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet - failsafe_chan ++; - } else if (FS_flag & 0x10) - { - FS_flag = 0; - failsafe_count = 0; - } - failsafe_count++; - #endif - - packet[0] = (sub_protocol & 0x02 ) ? 0x20 : 0x1D ; // LBT or FCC - packet[1] = rx_tx_addr[3]; - packet[2] = rx_tx_addr[2]; - packet[3] = 0x02; - // - packet[4] = (FrX_chanskip<<6)|hopping_frequency_no; - packet[5] = FrX_chanskip>>2; - packet[6] = RX_num; - //packet[7] = FLAGS 00 - standard packet - //10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet - //20 - range check packet - #ifdef FAILSAFE_ENABLE - packet[7] = FS_flag; - #else - packet[7] = 0; - #endif - packet[8] = 0; - // - uint8_t startChan = chan_offset; for(uint8_t i = 0; i <12 ; i+=3) - {//12 bytes of channel data - #ifdef FAILSAFE_ENABLE - if( (FS_flag & 0x10) && ((failsafe_chan & 0x07) == (startChan & 0x07)) ) - chan_0 = frskyX_scaleForPXX_FS(failsafe_chan); - else - #endif - chan_0 = frskyX_scaleForPXX(startChan); - startChan++; - // - #ifdef FAILSAFE_ENABLE - if( (FS_flag & 0x10) && ((failsafe_chan & 0x07) == (startChan & 0x07)) ) - chan_1 = frskyX_scaleForPXX_FS(failsafe_chan); - else - #endif - chan_1 = frskyX_scaleForPXX(startChan); - startChan++; - // - packet[9+i] = lowByte(chan_0); //3 bytes*4 - packet[9+i+1]=(((chan_0>>8) & 0x0F)|(chan_1 << 4)); - packet[9+i+2]=chan_1>>4; - } - packet[21] = (FrX_receive_seq << 4) | FrX_send_seq ;//8 at start - - if(sub_protocol & 0x01 ) // in X8 mode send only 8ch every 9ms - chan_offset = 0 ; - else - chan_offset^=0x08; - - uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ; - for (uint8_t i=22;i FRX_FAILSAFE_TIME && chan_offset == 0 && IS_FAILSAFE_VALUES_on) + { + FS_flag = 0x10; + failsafe_chan = 0; + } else if (FS_flag & 0x10 && failsafe_chan < (sub_protocol & 0x01 ? 8-1:16-1)) + { + FS_flag = 0x10 | ((FS_flag + 2) & 0x0F); //10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet + failsafe_chan ++; + } else if (FS_flag & 0x10) + { + FS_flag = 0; + failsafe_count = 0; + } + failsafe_count++; + #endif + + packet[0] = (sub_protocol & 0x02 ) ? 0x20 : 0x1D ; // LBT or FCC + packet[1] = rx_tx_addr[3]; + packet[2] = rx_tx_addr[2]; + packet[3] = 0x02; + // + packet[4] = (FrX_chanskip<<6)|hopping_frequency_no; + packet[5] = FrX_chanskip>>2; + packet[6] = RX_num; + //packet[7] = FLAGS 00 - standard packet + //10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet + //20 - range check packet + #ifdef FAILSAFE_ENABLE + packet[7] = FS_flag; + #else + packet[7] = 0; + #endif + packet[8] = 0; + // + uint8_t startChan = chan_offset; for(uint8_t i = 0; i <12 ; i+=3) + {//12 bytes of channel data + #ifdef FAILSAFE_ENABLE + if( (FS_flag & 0x10) && ((failsafe_chan & 0x07) == (startChan & 0x07)) ) + chan_0 = frskyX_scaleForPXX_FS(failsafe_chan); + else + #endif + chan_0 = frskyX_scaleForPXX(startChan); + startChan++; + // + #ifdef FAILSAFE_ENABLE + if( (FS_flag & 0x10) && ((failsafe_chan & 0x07) == (startChan & 0x07)) ) + chan_1 = frskyX_scaleForPXX_FS(failsafe_chan); + else + #endif + chan_1 = frskyX_scaleForPXX(startChan); + startChan++; + // + packet[9+i] = lowByte(chan_0); //3 bytes*4 + packet[9+i+1]=(((chan_0>>8) & 0x0F)|(chan_1 << 4)); + packet[9+i+2]=chan_1>>4; + } + packet[21] = (FrX_receive_seq << 4) | FrX_send_seq ;//8 at start - uint16_t lcrc = frskyX_crc_x(&packet[3], limit-3); - packet[limit++]=lcrc>>8;//high byte - packet[limit]=lcrc;//low byte + if(sub_protocol & 0x01 ) // in X8 mode send only 8ch every 9ms + chan_offset = 0 ; + else + chan_offset^=0x08; + + uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ; + for (uint8_t i=22;i>8;//high byte + packet[limit]=lcrc;//low byte } uint16_t ReadFrSkyX() { - switch(state) - { - default: - frskyX_set_start(47); - CC2500_SetPower(); - CC2500_Strobe(CC2500_SFRX); - // - frskyX_build_bind_packet(); - CC2500_Strobe(CC2500_SIDLE); - CC2500_WriteData(packet, packet[0]+1); - if(IS_BIND_DONE) { - state = FRSKY_BIND_DONE; + switch(state) + { + default: + frskyX_set_start(47); + CC2500_SetPower(); + CC2500_Strobe(CC2500_SFRX); + // + frskyX_build_bind_packet(); + CC2500_Strobe(CC2500_SIDLE); + CC2500_WriteData(packet, packet[0]+1); + if(IS_BIND_DONE) { + state = FRSKY_BIND_DONE; //debugln("%s:%d bind done", __func__, __LINE__); - } - else - state++; - return 9000; - case FRSKY_BIND_DONE: - frskyX_initialize_data(0); - hopping_frequency_no=0; - BIND_DONE; - state++; - break; - case FRSKY_DATA1: - if ( prev_option != option ) - { - CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack - prev_option = option ; - } - CC2500_SetTxRxMode(TX_EN); - frskyX_set_start(hopping_frequency_no); - CC2500_SetPower(); - CC2500_Strobe(CC2500_SFRX); - hopping_frequency_no = (hopping_frequency_no+FrX_chanskip)%47; - CC2500_Strobe(CC2500_SIDLE); - CC2500_WriteData(packet, packet[0]+1); - // -// frskyX_data_frame(); - state++; - return 5200; - case FRSKY_DATA2: - CC2500_SetTxRxMode(RX_EN); - CC2500_Strobe(CC2500_SIDLE); - state++; - return 200; - case FRSKY_DATA3: - CC2500_Strobe(CC2500_SRX); - state++; - return 3100; - case FRSKY_DATA4: - len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - if (len && (len<=(0x0E + 3))) //Telemetry frame is 17 - { - packet_count=0; - CC2500_ReadData(pkt, len); - #if defined TELEMETRY - frsky_check_telemetry(pkt,len); //check if valid telemetry packets - //parse telemetry packets here - //The same telemetry function used by FrSky(D8). - #endif - } - else - { - packet_count++; - // restart sequence on missed packet - might need count or timeout instead of one missed - if(packet_count>100) - {//~1sec -// seq_last_sent = 0; -// seq_last_rcvd = 8; - FrX_send_seq = 0x08 ; -// FrX_receive_seq = 0 ; - packet_count=0; - #if defined TELEMETRY - telemetry_lost=1; - #endif - } - CC2500_Strobe(CC2500_SFRX); //flush the RXFIFO - } - frskyX_data_frame(); - if ( FrX_send_seq != 0x08 ) - { - FrX_send_seq = ( FrX_send_seq + 1 ) & 0x03 ; - } - state = FRSKY_DATA1; - return 500; - } - return 1; + } + else + state++; + return 9000; + case FRSKY_BIND_DONE: + frskyX_initialize_data(0); + hopping_frequency_no=0; + BIND_DONE; + state++; + break; + case FRSKY_DATA1: + if ( prev_option != option ) + { + CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack + prev_option = option ; + } + CC2500_SetTxRxMode(TX_EN); + frskyX_set_start(hopping_frequency_no); + CC2500_SetPower(); + CC2500_Strobe(CC2500_SFRX); + hopping_frequency_no = (hopping_frequency_no+FrX_chanskip)%47; + CC2500_Strobe(CC2500_SIDLE); + CC2500_WriteData(packet, packet[0]+1); + // +// frskyX_data_frame(); + state++; + return 5200; + case FRSKY_DATA2: + CC2500_SetTxRxMode(RX_EN); + CC2500_Strobe(CC2500_SIDLE); + state++; + return 200; + case FRSKY_DATA3: + CC2500_Strobe(CC2500_SRX); + state++; + return 3100; + case FRSKY_DATA4: + len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (len && (len<=(0x0E + 3))) //Telemetry frame is 17 + { + packet_count=0; + CC2500_ReadData(pkt, len); + #if defined TELEMETRY + frsky_check_telemetry(pkt,len); //check if valid telemetry packets + //parse telemetry packets here + //The same telemetry function used by FrSky(D8). + #endif + } + else + { + packet_count++; + // restart sequence on missed packet - might need count or timeout instead of one missed + if(packet_count>100) + {//~1sec +// seq_last_sent = 0; +// seq_last_rcvd = 8; + FrX_send_seq = 0x08 ; +// FrX_receive_seq = 0 ; + packet_count=0; + #if defined TELEMETRY + telemetry_lost=1; + #endif + } + CC2500_Strobe(CC2500_SFRX); //flush the RXFIFO + } + frskyX_data_frame(); + if ( FrX_send_seq != 0x08 ) + { + FrX_send_seq = ( FrX_send_seq + 1 ) & 0x03 ; + } + state = FRSKY_DATA1; + return 500; + } + return 1; } uint16_t initFrSkyX() { - set_rx_tx_addr(MProtocol_id_master); - Frsky_init_hop(); - packet_count=0; - while(!FrX_chanskip) - FrX_chanskip=random(0xfefefefe)%47; + set_rx_tx_addr(MProtocol_id_master); + Frsky_init_hop(); + packet_count=0; + while(!FrX_chanskip) + FrX_chanskip=random(0xfefefefe)%47; - //for test*************** - rx_tx_addr[3]=0xB3; - rx_tx_addr[2]=0xFD; - //************************ - frskyX_init(); + //for test*************** + rx_tx_addr[3]=0xB3; + rx_tx_addr[2]=0xFD; + //************************ + frskyX_init(); #if defined SPORT_POLLING #ifdef INVERT_SERIAL - start_timer4() ; + start_timer4() ; #endif #endif - // - if(IS_BIND_IN_PROGRESS) - { - state = FRSKY_BIND; - frskyX_initialize_data(1); - } - else - { - state = FRSKY_DATA1; - frskyX_initialize_data(0); - } -// seq_last_sent = 0; -// seq_last_rcvd = 8; - FrX_send_seq = 0x08 ; - FrX_receive_seq = 0 ; - return 10000; -} + // + if(IS_BIND_IN_PROGRESS) + { + state = FRSKY_BIND; + frskyX_initialize_data(1); + } + else + { + state = FRSKY_DATA1; + frskyX_initialize_data(0); + } +// seq_last_sent = 0; +// seq_last_rcvd = 8; + FrX_send_seq = 0x08 ; + FrX_receive_seq = 0 ; + return 10000; +} #endif diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index e43326a..043e5c0 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -354,14 +354,7 @@ enum MultiPacketTypes #define FAILSAFE_CHANNEL_HOLD 2047 #define FAILSAFE_CHANNEL_NOPULSES 0 -//******************** -//** Debug messages ** -//******************** - uint16_t debug_time=0; - char buf[128]; - #define debug(msg, ...) { sprintf(buf, msg, ##__VA_ARGS__); Serial.println(buf);} - #define debugln(msg, ...) { sprintf(buf, msg "\r\n", ##__VA_ARGS__); Serial.println(buf);} - #define debug_time(msg) { uint16_t debug_time_TCNT1=TCNT1; debug_time=debug_time_TCNT1-debug_time; debugln(msg "%u", debug_time); debug_time=debug_time_TCNT1; } +#include "debug.h" //******************** //*** Blink timing *** //******************** diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 5986c03..cc9c36b 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -1,11 +1,11 @@ /********************************************************* - Multiprotocol Tx code + Multiprotocol Tx code by Midelic and Pascal Langer(hpnuts) - http://www.rcgroups.com/forums/showthread.php?t=2165676 + http://www.rcgroups.com/forums/showthread.php?t=2165676 https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/edit/master/README.md - Thanks to PhracturedBlue, Hexfet, Goebish, Victzh and all protocol developers - Ported from deviation firmware + Thanks to PhracturedBlue, Hexfet, Goebish, Victzh and all protocol developers + Ported from deviation firmware 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 @@ -25,11 +25,11 @@ #include #endif -//#define DEBUG_PIN // Use pin TX for AVR and SPI_CS for STM32 => DEBUG_PIN_on, DEBUG_PIN_off, DEBUG_PIN_toggle -#define DEBUG_SERIAL // Only for STM32_BOARD compiled with Upload method "Serial"->usart1, "STM32duino bootloader"->USB serial +//#define DEBUG_PIN // Use pin TX for AVR and SPI_CS for STM32 => DEBUG_PIN_on, DEBUG_PIN_off, DEBUG_PIN_toggle +#define DEBUG_SERIAL // Only for STM32_BOARD compiled with Upload method "Serial"->usart1, "STM32duino bootloader"->USB serial #if defined (ARDUINO_AVR_XMEGA32D4) || defined (ARDUINO_MULTI_ORANGERX) - #include "MultiOrange.h" + #include "MultiOrange.h" #endif #include "Multiprotocol.h" @@ -40,6 +40,7 @@ #include "Pins.h" #include "TX_Def.h" #include "Validate.h" +#include "state.h" //Global constants/variables uint32_t MProtocol_id;//tx id, @@ -55,7 +56,7 @@ uint8_t packet[40]; uint16_t Channel_data[NUM_CHN]; uint8_t Channel_AUX; #ifdef FAILSAFE_ENABLE - uint16_t Failsafe_data[NUM_CHN]; + uint16_t Failsafe_data[NUM_CHN]; #endif // Protocol variables @@ -84,7 +85,7 @@ uint16_t state; uint8_t len; #if defined(FRSKYX_CC2500_INO) || defined(SFHSS_CC2500_INO) - uint8_t calData[48]; + uint8_t calData[48]; #endif //Channel mapping for protocols @@ -135,144 +136,144 @@ void_function_t remote_callback = 0; // Init void setup() { - // Setup diagnostic uart before anything else - #ifdef DEBUG_SERIAL - Serial.begin(115200,SERIAL_8N1); - while (!Serial); // Wait for ever for the serial port to connect... - debugln("Multiprotocol version: %d.%d.%d.%d", VERSION_MAJOR, VERSION_MINOR, VERSION_REVISION, VERSION_PATCH_LEVEL); + // Setup diagnostic uart before anything else + #ifdef DEBUG_SERIAL + Serial.begin(115200,SERIAL_8N1); + while (!Serial); // Wait for ever for the serial port to connect... + debugln("Multiprotocol version: %d.%d.%d.%d", VERSION_MAJOR, VERSION_MINOR, VERSION_REVISION, VERSION_PATCH_LEVEL); debugln("time %s ", __TIME__); - #endif - - // General pinout - //ATMEGA328p - // all inputs - // outputs - SDI_output; - SCLK_output; - #ifdef A7105_CSN_pin - A7105_CSN_output; - #endif - #ifdef CC25_CSN_pin - CC25_CSN_output; - #endif - #ifdef CYRF_CSN_pin - CYRF_RST_output; - CYRF_CSN_output; - #endif - #ifdef NRF_CSN_pin - NRF_CSN_output; - #endif - - // Timer1 config + #endif + + // General pinout + //ATMEGA328p + // all inputs + // outputs + SDI_output; + SCLK_output; + #ifdef A7105_CSN_pin + A7105_CSN_output; + #endif + #ifdef CC25_CSN_pin + CC25_CSN_output; + #endif + #ifdef CYRF_CSN_pin + CYRF_RST_output; + CYRF_CSN_output; + #endif + #ifdef NRF_CSN_pin + NRF_CSN_output; + #endif + + // Timer1 config #ifdef TCCR1A - TCCR1A = 0; - TCCR1B = (1 << CS11); //prescaler8, set timer1 to increment every 0.5us(16Mhz) and start timer + TCCR1A = 0; + TCCR1B = (1 << CS11); //prescaler8, set timer1 to increment every 0.5us(16Mhz) and start timer + #endif + // Random + //random_init(); + + // Set Chip selects + #ifdef A7105_CSN_pin + A7105_CSN_on; + #endif + #ifdef CC25_CSN_pin + CC25_CSN_on; + #endif + #ifdef CYRF_CSN_pin + CYRF_CSN_on; + #endif + #ifdef NRF_CSN_pin + NRF_CSN_on; + #endif + // Set SPI lines + #ifdef STM32_BOARD + initSPI2(); + #else + SDI_on; + SCLK_off; #endif - // Random - //random_init(); - - // Set Chip selects - #ifdef A7105_CSN_pin - A7105_CSN_on; - #endif - #ifdef CC25_CSN_pin - CC25_CSN_on; - #endif - #ifdef CYRF_CSN_pin - CYRF_CSN_on; - #endif - #ifdef NRF_CSN_pin - NRF_CSN_on; - #endif - // Set SPI lines - #ifdef STM32_BOARD - initSPI2(); - #else - SDI_on; - SCLK_off; - #endif - - //Wait for every component to start - delay(100); - - // Read status of bind button - if( /*IS_BIND_BUTTON_on */ true) - { - BIND_BUTTON_FLAG_on; // If bind button pressed save the status - BIND_IN_PROGRESS; // Request bind - } - else - BIND_DONE; - - // Read status of mode select binary switch - // after this mode_select will be one of {0000, 0001, ..., 1111} - - + + //Wait for every component to start + delay(100); + + // Read status of bind button + if( /*IS_BIND_BUTTON_on */ true) + { + BIND_BUTTON_FLAG_on; // If bind button pressed save the status + BIND_IN_PROGRESS; // Request bind + } + else + BIND_DONE; + + // Read status of mode select binary switch + // after this mode_select will be one of {0000, 0001, ..., 1111} + + String str; debugln("select bank:"); str=""; while(true) { if (Serial.available() > 0) { char recieved = Serial.read(); - if (recieved == '\n') { + if (recieved == '\n') { int new_bank = atoi(str.c_str()); curr_bank = new_bank; debugln("Bank selection %d", new_bank); break; }else { - str += recieved; + str += recieved; } } } - + debugln("select mode:"); str=""; while(true) { if (Serial.available() > 0) { char recieved = Serial.read(); - if (recieved == '\n') { + if (recieved == '\n') { int new_mode = atoi(str.c_str()); debugln("Protocol selection switch reads as %d with \'%s\'", new_mode,str.c_str()); mode_select = new_mode; break; }else { - str += recieved; + str += recieved; } } } debugln("Protocol selection switch reads as %d", mode_select); - #ifdef ENABLE_PPM - uint8_t bank=bank_switch(); - #endif + #ifdef ENABLE_PPM + uint8_t bank=bank_switch(); + #endif + + // Set default channels' value + InitChannel(); + InitPPM(); + // Update LED + LED_off; + LED_output; - // Set default channels' value - InitChannel(); - InitPPM(); - // Update LED - LED_off; - LED_output; + //Init RF modules + modules_reset(); - //Init RF modules - modules_reset(); + randomSeed(42); - randomSeed(42); + // Read or create protocol id + MProtocol_id_master=random_id(10,false); - // Read or create protocol id - MProtocol_id_master=random_id(10,false); + debugln("Module Id: %lx", MProtocol_id_master); - debugln("Module Id: %lx", MProtocol_id_master); - #ifdef ENABLE_PPM - //Protocol and interrupts initialization - if(mode_select != MODE_SERIAL) - { // PPM - uint8_t line=bank*14+mode_select-1; - protocol = PPM_prot[line].protocol; - cur_protocol[1] = protocol; - sub_protocol = PPM_prot[line].sub_proto; - RX_num = PPM_prot[line].rx_num; - + //Protocol and interrupts initialization + if(mode_select != MODE_SERIAL) + { // PPM + uint8_t line=bank*14+mode_select-1; + protocol = PPM_prot[line].protocol; + cur_protocol[1] = protocol; + sub_protocol = PPM_prot[line].sub_proto; + RX_num = PPM_prot[line].rx_num; + debugln("protocol: %d", protocol); switch(protocol) { case PROTO_FRSKYD: @@ -286,58 +287,36 @@ void setup() break; } debugln("sub_protocol: %d", sub_protocol); - option = PPM_prot[line].option; // Use radio-defined option value + option = PPM_prot[line].option; // Use radio-defined option value debugln("freq offset: %d", option); - if(PPM_prot[line].power) - POWER_FLAG_on; - if(PPM_prot[line].autobind) - { - AUTOBIND_FLAG_on; - BIND_IN_PROGRESS; // Force a bind at protocol startup - } - line++; - - protocol_init(); - - #if defined(TELEMETRY) - PPM_Telemetry_serial_init();// Configure serial for telemetry - #endif - } - else + if(PPM_prot[line].power) + POWER_FLAG_on; + if(PPM_prot[line].autobind) + { + AUTOBIND_FLAG_on; + BIND_IN_PROGRESS; // Force a bind at protocol startup + } + line++; + + protocol_init(); + + #if defined(TELEMETRY) + PPM_Telemetry_serial_init();// Configure serial for telemetry + #endif + } + else #endif //ENABLE_PPM - debugln("Init complete"); + debugln("Init complete"); update_inputs(); - - debugln("time now %lu ", micros()); - delay(1000); - debugln("time after 1 sec %lu", micros()); - - - #ifdef TCCR1A - while((TIFR1 & OCF1A_bm) == 0); // wait - unsigned long s = micros(); - cli(); // Disable global int due to RW of 16 bits registers - OCR1A += 10000 * TIMER_PRESCALE; // set compare A for callback - TIFR1=OCF1A_bm; // clear compare A=callback flag - sei(); // enable global int - while((TIFR1 & OCF1A_bm) == 0); // wait 2ms... - unsigned long e = micros(); - double a = e; - a -= s; - a = 10000/a; - debugln("time now %lu", s); - debugln("time after 1 sec %lu", e); - debugln("scalor %d", a); - TIMER_PRESCALE *= a; - #endif + init_state(); } // Main // Protocol scheduler void loop() -{ +{ uint32_t next_callback; - uint16_t diff=0xFFFF; + uint16_t diff=0xFFFF; uint32_t end__ = micros(); uint32_t start = micros(); @@ -348,57 +327,59 @@ void loop() Update_All(); } while(remote_callback==0 || IS_WAIT_BIND_on); - } - while(1) - { + } + while(1) + { #if 0 - if( (TIFR1 & OCF1A_bm) != 0) - { - cli(); // Disable global int due to RW of 16 bits registers - OCR1A=TCNT1; // Callback should already have been called... Use "now" as new sync point. - sei(); // Enable global int - } - else - while((TIFR1 & OCF1A_bm) == 0); // Wait before callback - + if( (TIFR1 & OCF1A_bm) != 0) + { + cli(); // Disable global int due to RW of 16 bits registers + OCR1A=TCNT1; // Callback should already have been called... Use "now" as new sync point. + sei(); // Enable global int + } + else + while((TIFR1 & OCF1A_bm) == 0); // Wait before callback + #endif - + end__ = micros(); - do - { - //TX_MAIN_PAUSE_on; - //tx_pause(); + do + { + //TX_MAIN_PAUSE_on; + //tx_pause(); start = end__; - next_callback=remote_callback(); + next_callback=remote_callback(); #if 0 - TX_MAIN_PAUSE_off; - tx_resume(); - while(next_callback>4000) - { // start to wait here as much as we can... - next_callback-=2000; // We will wait below for 2ms - cli(); // Disable global int due to RW of 16 bits registers - OCR1A += 2000 * TIMER_PRESCALE; // set compare A for callback - TIFR1=OCF1A_bm; // clear compare A=callback flag - sei(); // enable global int - if(Update_All()) // Protocol changed? - { - next_callback=0; // Launch new protocol ASAP - break; - } - while((TIFR1 & OCF1A_bm) == 0); // wait 2ms... - } - // at this point we have a maximum of 4ms in next_callback - cli(); // Disable global int due to RW of 16 bits registers - OCR1A+= next_callback*TIMER_PRESCALE ; // set compare A for callback - TIFR1=OCF1A_bm; // clear compare A=callback flag - diff=OCR1A-TCNT1; // compare timer and comparator - sei(); // enable global int -#else - - if (next_callback > 4000) + TX_MAIN_PAUSE_off; + tx_resume(); + while(next_callback>4000) + { // start to wait here as much as we can... + next_callback-=2000; // We will wait below for 2ms + cli(); // Disable global int due to RW of 16 bits registers + OCR1A += 2000 * TIMER_PRESCALE; // set compare A for callback + TIFR1=OCF1A_bm; // clear compare A=callback flag + sei(); // enable global int + if(Update_All()) // Protocol changed? + { + next_callback=0; // Launch new protocol ASAP + break; + } + while((TIFR1 & OCF1A_bm) == 0); // wait 2ms... + } + // at this point we have a maximum of 4ms in next_callback + cli(); // Disable global int due to RW of 16 bits registers + OCR1A+= next_callback*TIMER_PRESCALE ; // set compare A for callback + TIFR1=OCF1A_bm; // clear compare A=callback flag + diff=OCR1A-TCNT1; // compare timer and comparator + sei(); // enable global int +#else + + if (next_callback > 4000) { update_inputs(); + update_state(); + } uint32_t wait_until = start + next_callback; end__ = micros(); if (end__-start < next_callback) { @@ -420,295 +401,295 @@ void loop() end__ = micros(); //debugln("%lu vs %lu",end__-start,next_callback); //debugln("next %d diff %lu (%lu to %lu) diff %x ",org_next_call, end__ - start_, end__, start_, diff); - - } - while(diff&0x8000); // Callback did not took more than requested time for next callback - // so we can launch Update_All before next callback - } + + } + while(diff&0x8000); // Callback did not took more than requested time for next callback + // so we can launch Update_All before next callback + } } uint8_t Update_All() { - #ifdef ENABLE_SERIAL - #ifdef CHECK_FOR_BOOTLOADER - if ( (mode_select==MODE_SERIAL) && (NotBootChecking == 0) ) - pollBoot() ; - else - #endif - if(mode_select==MODE_SERIAL && IS_RX_FLAG_on) // Serial mode and something has been received - { - update_serial_data(); // Update protocol and data - update_channels_aux(); - INPUT_SIGNAL_on; //valid signal received - last_signal=millis(); - } - #endif //ENABLE_SERIAL - #ifdef ENABLE_PPM - if(mode_select!=MODE_SERIAL && IS_PPM_FLAG_on) // PPM mode and a full frame has been received - { + #ifdef ENABLE_SERIAL + #ifdef CHECK_FOR_BOOTLOADER + if ( (mode_select==MODE_SERIAL) && (NotBootChecking == 0) ) + pollBoot() ; + else + #endif + if(mode_select==MODE_SERIAL && IS_RX_FLAG_on) // Serial mode and something has been received + { + update_serial_data(); // Update protocol and data + update_channels_aux(); + INPUT_SIGNAL_on; //valid signal received + last_signal=millis(); + } + #endif //ENABLE_SERIAL + #ifdef ENABLE_PPM + if(mode_select!=MODE_SERIAL && IS_PPM_FLAG_on) // PPM mode and a full frame has been received + { debugln("%s:%d",__func__, __LINE__); - for(uint8_t i=0;iCHANNEL_MAX_125) val=CHANNEL_MAX_125; - //Channel_data[i]=val; - } - PPM_FLAG_off; // wait for next frame before update - update_channels_aux(); - INPUT_SIGNAL_on; // valid signal received - last_signal=millis(); - } - #endif //ENABLE_PPM - - - #ifdef ENABLE_BIND_CH - if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && Channel_data[BIND_CH-1]>CHANNEL_MAX_COMMAND && Channel_data[THROTTLE]<(CHANNEL_MIN_100+50)) - { // Autobind is on and BIND_CH went up and Throttle is low - CHANGE_PROTOCOL_FLAG_on; //reload protocol - BIND_IN_PROGRESS; //enable bind - + for(uint8_t i=0;iCHANNEL_MAX_125) val=CHANNEL_MAX_125; + //Channel_data[i]=val; + } + PPM_FLAG_off; // wait for next frame before update + update_channels_aux(); + INPUT_SIGNAL_on; // valid signal received + last_signal=millis(); + } + #endif //ENABLE_PPM + + + #ifdef ENABLE_BIND_CH + if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && Channel_data[BIND_CH-1]>CHANNEL_MAX_COMMAND && Channel_data[THROTTLE]<(CHANNEL_MIN_100+50)) + { // Autobind is on and BIND_CH went up and Throttle is low + CHANGE_PROTOCOL_FLAG_on; //reload protocol + BIND_IN_PROGRESS; //enable bind + debugln("%s:%d set bind prog",__func__, __LINE__); - BIND_CH_PREV_on; - } - if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_on && Channel_data[BIND_CH-1]2) - bind_counter=2; - } - #endif //ENABLE_BIND_CH - - + BIND_CH_PREV_on; + } + if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_on && Channel_data[BIND_CH-1]2) + bind_counter=2; + } + #endif //ENABLE_BIND_CH + + update_inputs(); - - if(IS_CHANGE_PROTOCOL_FLAG_on) - { + + if(IS_CHANGE_PROTOCOL_FLAG_on) + { debugln("%s:%d set bind prog",__func__, __LINE__); - // Protocol needs to be changed or relaunched for bind - protocol_init(); //init new protocol - return 1; - } - return 0; + // Protocol needs to be changed or relaunched for bind + protocol_init(); //init new protocol + return 1; + } + return 0; } // Update channels direction and Channel_AUX flags based on servo AUX positions static void update_channels_aux(void) { - //Reverse channels direction - #ifdef REVERSE_AILERON - reverse_channel(AILREON); - #endif - #ifdef REVERSE_ELEVATOR - reverse_channel(ELEVATOR); - #endif - #ifdef REVERSE_THROTTLE - reverse_channel(THROTTLE); - #endif - #ifdef REVERSE_RUDDER - reverse_channel(RUDDER); - #endif - - //Calc AUX flags - Channel_AUX=0; - for(uint8_t i=0;i<8;i++) - if(Channel_data[CH5+i]>CHANNEL_SWITCH) - Channel_AUX|=1<CHANNEL_SWITCH) + Channel_AUX|=1<=NBR_BANKS) - { // Wrong number of bank - curr_bank = 0; // set bank to 0 - bank=0; - } - debugln("Using bank %d", bank); - - phase=3; - uint32_t check=millis(); - blink=millis(); - while(mode_select==15) - { //loop here if the dial is on position 15 for user to select the bank - if(blink>2) >= bank) - { - phase=0; - blink+=BLINK_BANK_REPEAT; - } - else - phase+=2; - break; - case 3: - blink+=BLINK_BANK_TIME_LOW; - phase=0; - break; - } - } - if(check=NBR_BANKS) - bank=0; - curr_bank = bank; - debugln("Using bank %d", bank); - phase=3; - blink+=BLINK_BANK_REPEAT; - check+=2*BLINK_BANK_REPEAT; - } - check+=1; - } - } - return bank; + uint8_t bank= curr_bank; + if(bank>=NBR_BANKS) + { // Wrong number of bank + curr_bank = 0; // set bank to 0 + bank=0; + } + debugln("Using bank %d", bank); + + phase=3; + uint32_t check=millis(); + blink=millis(); + while(mode_select==15) + { //loop here if the dial is on position 15 for user to select the bank + if(blink>2) >= bank) + { + phase=0; + blink+=BLINK_BANK_REPEAT; + } + else + phase+=2; + break; + case 3: + blink+=BLINK_BANK_TIME_LOW; + phase=0; + break; + } + } + if(check=NBR_BANKS) + bank=0; + curr_bank = bank; + debugln("Using bank %d", bank); + phase=3; + blink+=BLINK_BANK_REPEAT; + check+=2*BLINK_BANK_REPEAT; + } + check+=1; + } + } + return bank; } #endif inline void tx_pause() { - #ifdef TELEMETRY - // Pause telemetry by disabling transmitter interrupt - #ifdef ORANGE_TX - USARTC0.CTRLA &= ~0x03 ; - #else - #ifndef BASH_SERIAL - #ifdef STM32_BOARD - USART3_BASE->CR1 &= ~ USART_CR1_TXEIE; - #else - UCSR0B &= ~_BV(UDRIE0); - #endif - #endif - #endif - #endif + #ifdef TELEMETRY + // Pause telemetry by disabling transmitter interrupt + #ifdef ORANGE_TX + USARTC0.CTRLA &= ~0x03 ; + #else + #ifndef BASH_SERIAL + #ifdef STM32_BOARD + USART3_BASE->CR1 &= ~ USART_CR1_TXEIE; + #else + UCSR0B &= ~_BV(UDRIE0); + #endif + #endif + #endif + #endif } inline void tx_resume() { - #ifdef TELEMETRY - // Resume telemetry by enabling transmitter interrupt - #ifndef SPORT_POLLING - if(!IS_TX_PAUSE_on) - #endif - { - #ifdef ORANGE_TX - cli() ; - USARTC0.CTRLA = (USARTC0.CTRLA & 0xFC) | 0x01 ; - sei() ; - #else - #ifndef BASH_SERIAL - #ifdef STM32_BOARD - USART3_BASE->CR1 |= USART_CR1_TXEIE; - #else - UCSR0B |= _BV(UDRIE0); - #endif - #else - resumeBashSerial(); - #endif - #endif - } - #endif + #ifdef TELEMETRY + // Resume telemetry by enabling transmitter interrupt + #ifndef SPORT_POLLING + if(!IS_TX_PAUSE_on) + #endif + { + #ifdef ORANGE_TX + cli() ; + USARTC0.CTRLA = (USARTC0.CTRLA & 0xFC) | 0x01 ; + sei() ; + #else + #ifndef BASH_SERIAL + #ifdef STM32_BOARD + USART3_BASE->CR1 |= USART_CR1_TXEIE; + #else + UCSR0B |= _BV(UDRIE0); + #endif + #else + resumeBashSerial(); + #endif + #endif + } + #endif } // Protocol start static void protocol_init() { - static uint16_t next_callback; - if(IS_WAIT_BIND_off) - { - remote_callback = 0; // No protocol - next_callback=0; // Default is immediate call back - modules_reset(); // Reset all modules - - //Set global ID and rx_tx_addr - MProtocol_id = RX_num + MProtocol_id_master; - set_rx_tx_addr(MProtocol_id); - - #ifdef FAILSAFE_ENABLE - InitFailsafe(); - #endif - - blink=millis(); - - debugln("Protocol selected: %d, sub proto %d, rxnum %d, option %d", protocol, sub_protocol, RX_num, option); - - switch(protocol) // Init the requested protocol - { - #ifdef CC2500_INSTALLED - #if defined(FRSKYD_CC2500_INO) - case PROTO_FRSKYD: - next_callback = initFrSky_2way(); - remote_callback = ReadFrSky_2way; - break; - #endif - #if defined(FRSKYV_CC2500_INO) - case PROTO_FRSKYV: - next_callback = initFRSKYV(); - remote_callback = ReadFRSKYV; - break; - #endif - #if defined(FRSKYX_CC2500_INO) - case PROTO_FRSKYX: - next_callback = initFrSkyX(); - remote_callback = ReadFrSkyX; - break; - #endif - #endif - } - } - - #if defined(WAIT_FOR_BIND) && defined(ENABLE_BIND_CH) - if( IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && (cur_protocol[1]&0x80)==0 && mode_select == MODE_SERIAL) - { // Autobind is active but no bind requested by either BIND_CH or BIND. But do not wait if in PPM mode... - WAIT_BIND_on; - return; - } - #endif - WAIT_BIND_off; - CHANGE_PROTOCOL_FLAG_off; + static uint16_t next_callback; + if(IS_WAIT_BIND_off) + { + remote_callback = 0; // No protocol + next_callback=0; // Default is immediate call back + modules_reset(); // Reset all modules + + //Set global ID and rx_tx_addr + MProtocol_id = RX_num + MProtocol_id_master; + set_rx_tx_addr(MProtocol_id); + + #ifdef FAILSAFE_ENABLE + InitFailsafe(); + #endif + + blink=millis(); + + debugln("Protocol selected: %d, sub proto %d, rxnum %d, option %d", protocol, sub_protocol, RX_num, option); + + switch(protocol) // Init the requested protocol + { + #ifdef CC2500_INSTALLED + #if defined(FRSKYD_CC2500_INO) + case PROTO_FRSKYD: + next_callback = initFrSky_2way(); + remote_callback = ReadFrSky_2way; + break; + #endif + #if defined(FRSKYV_CC2500_INO) + case PROTO_FRSKYV: + next_callback = initFRSKYV(); + remote_callback = ReadFRSKYV; + break; + #endif + #if defined(FRSKYX_CC2500_INO) + case PROTO_FRSKYX: + next_callback = initFrSkyX(); + remote_callback = ReadFrSkyX; + break; + #endif + #endif + } + } + + #if defined(WAIT_FOR_BIND) && defined(ENABLE_BIND_CH) + if( IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && (cur_protocol[1]&0x80)==0 && mode_select == MODE_SERIAL) + { // Autobind is active but no bind requested by either BIND_CH or BIND. But do not wait if in PPM mode... + WAIT_BIND_on; + return; + } + #endif + WAIT_BIND_off; + CHANGE_PROTOCOL_FLAG_off; #if 0 - if(next_callback>32000) - { // next_callback should not be more than 32767 so we will wait here... - uint16_t temp=(next_callback>>10)-2; - delayMilliseconds(temp); - next_callback-=temp<<10; // between 2-3ms left at this stage - } - cli(); // disable global int - OCR1A = TCNT1 + next_callback*2; // set compare A for callback - #ifndef STM32_BOARD - TIFR1 = OCF1A_bm ; // clear compare A flag - #else - TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC1IF; // Clear Timer2/Comp1 interrupt flag - #endif - sei(); // enable global int - BIND_BUTTON_FLAG_off; // do not bind/reset id anymore even if protocol change + if(next_callback>32000) + { // next_callback should not be more than 32767 so we will wait here... + uint16_t temp=(next_callback>>10)-2; + delayMilliseconds(temp); + next_callback-=temp<<10; // between 2-3ms left at this stage + } + cli(); // disable global int + OCR1A = TCNT1 + next_callback*2; // set compare A for callback + #ifndef STM32_BOARD + TIFR1 = OCF1A_bm ; // clear compare A flag + #else + TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC1IF; // Clear Timer2/Comp1 interrupt flag + #endif + sei(); // enable global int + BIND_BUTTON_FLAG_off; // do not bind/reset id anymore even if protocol change #else delayMicroseconds(next_callback); #endif @@ -717,336 +698,336 @@ static void protocol_init() void update_serial_data() { - RX_DONOTUPDATE_on; - RX_FLAG_off; //data is being processed - #ifdef SAMSON // Extremely dangerous, do not enable this unless you know what you are doing... - if( rx_ok_buff[0]==0x55 && (rx_ok_buff[1]&0x1F)==PROTO_FRSKYD && rx_ok_buff[2]==0x7F && rx_ok_buff[24]==217 && rx_ok_buff[25]==202 ) - {//proto==FRSKYD+sub==7+rx_num==7+CH15==73%+CH16==73% - rx_ok_buff[1]=(rx_ok_buff[1]&0xE0) | PROTO_FLYSKY; // change the protocol to Flysky - memcpy((void*)(rx_ok_buff+4),(void*)(rx_ok_buff+4+11),11); // reassign channels 9-16 to 1-8 - } - #endif - if(rx_ok_buff[1]&0x20) //check range - RANGE_FLAG_on; - else - RANGE_FLAG_off; - if(rx_ok_buff[1]&0x40) //check autobind - AUTOBIND_FLAG_on; - else - AUTOBIND_FLAG_off; - if(rx_ok_buff[2]&0x80) //if rx_ok_buff[2] ==1,power is low ,0-power high - POWER_FLAG_off; //power low - else - POWER_FLAG_on; //power high - - //Forced frequency tuning values for CC2500 protocols - #if defined(FORCE_FRSKYD_TUNING) && defined(FRSKYD_CC2500_INO) - if(protocol==PROTO_FRSKYD) - option=FORCE_FRSKYD_TUNING; // Use config-defined tuning value for FrSkyD - else - #endif - #if defined(FORCE_FRSKYV_TUNING) && defined(FRSKYV_CC2500_INO) - if(protocol==PROTO_FRSKYV) - option=FORCE_FRSKYV_TUNING; // Use config-defined tuning value for FrSkyV - else - #endif - #if defined(FORCE_FRSKYX_TUNING) && defined(FRSKYX_CC2500_INO) - if(protocol==PROTO_FRSKYX) - option=FORCE_FRSKYX_TUNING; // Use config-defined tuning value for FrSkyX - else - #endif - #if defined(FORCE_SFHSS_TUNING) && defined(SFHSS_CC2500_INO) - if (protocol==PROTO_SFHSS) - option=FORCE_SFHSS_TUNING; // Use config-defined tuning value for SFHSS - else - #endif - #if defined(FORCE_CORONA_TUNING) && defined(CORONA_CC2500_INO) - if (protocol==PROTO_CORONA) - option=FORCE_CORONA_TUNING; // Use config-defined tuning value for CORONA - else - #endif - #if defined(FORCE_HITEC_TUNING) && defined(HITEC_CC2500_INO) - if (protocol==PROTO_HITEC) - option=FORCE_HITEC_TUNING; // Use config-defined tuning value for HITEC - else - #endif - option=rx_ok_buff[3]; // Use radio-defined option value - - #ifdef FAILSAFE_ENABLE - bool failsafe=false; - if(rx_ok_buff[0]&0x02) - { // Packet contains failsafe instead of channels - failsafe=true; - rx_ok_buff[0]&=0xFD; //remove the failsafe flag - FAILSAFE_VALUES_on; //failsafe data has been received - } - #endif - #ifdef BONI - if(CH14_SW) - rx_ok_buff[2]=(rx_ok_buff[2]&0xF0)|((rx_ok_buff[2]+1)&0x0F); // Extremely dangerous, do not enable this!!! This is really for a special case... - #endif - if( (rx_ok_buff[0] != cur_protocol[0]) || ((rx_ok_buff[1]&0x5F) != (cur_protocol[1]&0x5F)) || ( (rx_ok_buff[2]&0x7F) != (cur_protocol[2]&0x7F) ) ) - { // New model has been selected - CHANGE_PROTOCOL_FLAG_on; //change protocol - WAIT_BIND_off; - if((rx_ok_buff[1]&0x80)!=0 || IS_AUTOBIND_FLAG_on) - BIND_IN_PROGRESS; //launch bind right away if in autobind mode or bind is set - else - BIND_DONE; - protocol=(rx_ok_buff[0]==0x55?0:32) + (rx_ok_buff[1]&0x1F); //protocol no (0-63) bits 4-6 of buff[1] and bit 0 of buf[0] - sub_protocol=(rx_ok_buff[2]>>4)& 0x07; //subprotocol no (0-7) bits 4-6 - RX_num=rx_ok_buff[2]& 0x0F; // rx_num bits 0---3 - } - else - if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) ) // Bind flag has been set - { // Restart protocol with bind - CHANGE_PROTOCOL_FLAG_on; - BIND_IN_PROGRESS; - } - else - if( ((rx_ok_buff[1]&0x80)==0) && ((cur_protocol[1]&0x80)!=0) ) // Bind flag has been reset - { // Request protocol to end bind - #if defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) || defined(FRSKYV_CC2500_INO) - if(protocol==PROTO_FRSKYD || protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYV) - BIND_DONE; - else - #endif - if(bind_counter>2) - bind_counter=2; - } - - //store current protocol values - for(uint8_t i=0;i<3;i++) - cur_protocol[i] = rx_ok_buff[i]; - - // decode channel/failsafe values - volatile uint8_t *p=rx_ok_buff+3; - uint8_t dec=-3; - for(uint8_t i=0;i=8) - { - dec-=8; - p++; - } - p++; - uint16_t temp=((*((uint32_t *)p))>>dec)&0x7FF; - #ifdef FAILSAFE_ENABLE - if(failsafe) - Failsafe_data[i]=temp; //value range 0..2047, 0=no pulses, 2047=hold - else - #endif - Channel_data[i]=temp; //value range 0..2047, 0=-125%, 2047=+125% - } - RX_DONOTUPDATE_off; - cli(); - if(IS_RX_MISSED_BUFF_on) // If the buffer is still valid - { memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer - RX_FLAG_on; // data to be processed next time... - RX_MISSED_BUFF_off; - } - sei(); - #ifdef FAILSAFE_ENABLE - if(failsafe) - debugln("RX_FS:%d,%d,%d,%d",Failsafe_data[0],Failsafe_data[1],Failsafe_data[2],Failsafe_data[3]); - #endif + RX_DONOTUPDATE_on; + RX_FLAG_off; //data is being processed + #ifdef SAMSON // Extremely dangerous, do not enable this unless you know what you are doing... + if( rx_ok_buff[0]==0x55 && (rx_ok_buff[1]&0x1F)==PROTO_FRSKYD && rx_ok_buff[2]==0x7F && rx_ok_buff[24]==217 && rx_ok_buff[25]==202 ) + {//proto==FRSKYD+sub==7+rx_num==7+CH15==73%+CH16==73% + rx_ok_buff[1]=(rx_ok_buff[1]&0xE0) | PROTO_FLYSKY; // change the protocol to Flysky + memcpy((void*)(rx_ok_buff+4),(void*)(rx_ok_buff+4+11),11); // reassign channels 9-16 to 1-8 + } + #endif + if(rx_ok_buff[1]&0x20) //check range + RANGE_FLAG_on; + else + RANGE_FLAG_off; + if(rx_ok_buff[1]&0x40) //check autobind + AUTOBIND_FLAG_on; + else + AUTOBIND_FLAG_off; + if(rx_ok_buff[2]&0x80) //if rx_ok_buff[2] ==1,power is low ,0-power high + POWER_FLAG_off; //power low + else + POWER_FLAG_on; //power high + + //Forced frequency tuning values for CC2500 protocols + #if defined(FORCE_FRSKYD_TUNING) && defined(FRSKYD_CC2500_INO) + if(protocol==PROTO_FRSKYD) + option=FORCE_FRSKYD_TUNING; // Use config-defined tuning value for FrSkyD + else + #endif + #if defined(FORCE_FRSKYV_TUNING) && defined(FRSKYV_CC2500_INO) + if(protocol==PROTO_FRSKYV) + option=FORCE_FRSKYV_TUNING; // Use config-defined tuning value for FrSkyV + else + #endif + #if defined(FORCE_FRSKYX_TUNING) && defined(FRSKYX_CC2500_INO) + if(protocol==PROTO_FRSKYX) + option=FORCE_FRSKYX_TUNING; // Use config-defined tuning value for FrSkyX + else + #endif + #if defined(FORCE_SFHSS_TUNING) && defined(SFHSS_CC2500_INO) + if (protocol==PROTO_SFHSS) + option=FORCE_SFHSS_TUNING; // Use config-defined tuning value for SFHSS + else + #endif + #if defined(FORCE_CORONA_TUNING) && defined(CORONA_CC2500_INO) + if (protocol==PROTO_CORONA) + option=FORCE_CORONA_TUNING; // Use config-defined tuning value for CORONA + else + #endif + #if defined(FORCE_HITEC_TUNING) && defined(HITEC_CC2500_INO) + if (protocol==PROTO_HITEC) + option=FORCE_HITEC_TUNING; // Use config-defined tuning value for HITEC + else + #endif + option=rx_ok_buff[3]; // Use radio-defined option value + + #ifdef FAILSAFE_ENABLE + bool failsafe=false; + if(rx_ok_buff[0]&0x02) + { // Packet contains failsafe instead of channels + failsafe=true; + rx_ok_buff[0]&=0xFD; //remove the failsafe flag + FAILSAFE_VALUES_on; //failsafe data has been received + } + #endif + #ifdef BONI + if(CH14_SW) + rx_ok_buff[2]=(rx_ok_buff[2]&0xF0)|((rx_ok_buff[2]+1)&0x0F); // Extremely dangerous, do not enable this!!! This is really for a special case... + #endif + if( (rx_ok_buff[0] != cur_protocol[0]) || ((rx_ok_buff[1]&0x5F) != (cur_protocol[1]&0x5F)) || ( (rx_ok_buff[2]&0x7F) != (cur_protocol[2]&0x7F) ) ) + { // New model has been selected + CHANGE_PROTOCOL_FLAG_on; //change protocol + WAIT_BIND_off; + if((rx_ok_buff[1]&0x80)!=0 || IS_AUTOBIND_FLAG_on) + BIND_IN_PROGRESS; //launch bind right away if in autobind mode or bind is set + else + BIND_DONE; + protocol=(rx_ok_buff[0]==0x55?0:32) + (rx_ok_buff[1]&0x1F); //protocol no (0-63) bits 4-6 of buff[1] and bit 0 of buf[0] + sub_protocol=(rx_ok_buff[2]>>4)& 0x07; //subprotocol no (0-7) bits 4-6 + RX_num=rx_ok_buff[2]& 0x0F; // rx_num bits 0---3 + } + else + if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) ) // Bind flag has been set + { // Restart protocol with bind + CHANGE_PROTOCOL_FLAG_on; + BIND_IN_PROGRESS; + } + else + if( ((rx_ok_buff[1]&0x80)==0) && ((cur_protocol[1]&0x80)!=0) ) // Bind flag has been reset + { // Request protocol to end bind + #if defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) || defined(FRSKYV_CC2500_INO) + if(protocol==PROTO_FRSKYD || protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYV) + BIND_DONE; + else + #endif + if(bind_counter>2) + bind_counter=2; + } + + //store current protocol values + for(uint8_t i=0;i<3;i++) + cur_protocol[i] = rx_ok_buff[i]; + + // decode channel/failsafe values + volatile uint8_t *p=rx_ok_buff+3; + uint8_t dec=-3; + for(uint8_t i=0;i=8) + { + dec-=8; + p++; + } + p++; + uint16_t temp=((*((uint32_t *)p))>>dec)&0x7FF; + #ifdef FAILSAFE_ENABLE + if(failsafe) + Failsafe_data[i]=temp; //value range 0..2047, 0=no pulses, 2047=hold + else + #endif + Channel_data[i]=temp; //value range 0..2047, 0=-125%, 2047=+125% + } + RX_DONOTUPDATE_off; + cli(); + if(IS_RX_MISSED_BUFF_on) // If the buffer is still valid + { memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer + RX_FLAG_on; // data to be processed next time... + RX_MISSED_BUFF_off; + } + sei(); + #ifdef FAILSAFE_ENABLE + if(failsafe) + debugln("RX_FS:%d,%d,%d,%d",Failsafe_data[0],Failsafe_data[1],Failsafe_data[2],Failsafe_data[3]); + #endif } void modules_reset() { - #ifdef CC2500_INSTALLED - CC2500_Reset(); - #endif - #ifdef A7105_INSTALLED - A7105_Reset(); - #endif - #ifdef CYRF6936_INSTALLED - CYRF_Reset(); - #endif - #ifdef NRF24L01_INSTALLED - NRF24L01_Reset(); - #endif - - //Wait for every component to reset - delay(100); - prev_power=0xFD; // unused power value + #ifdef CC2500_INSTALLED + CC2500_Reset(); + #endif + #ifdef A7105_INSTALLED + A7105_Reset(); + #endif + #ifdef CYRF6936_INSTALLED + CYRF_Reset(); + #endif + #ifdef NRF24L01_INSTALLED + NRF24L01_Reset(); + #endif + + //Wait for every component to reset + delay(100); + prev_power=0xFD; // unused power value } #ifdef STM32_BOARD - void usart2_begin(uint32_t baud,uint32_t config ) - { - usart_init(USART2); - usart_config_gpios_async(USART2,GPIOA,PIN_MAP[PA3].gpio_bit,GPIOA,PIN_MAP[PA2].gpio_bit,config); - LED2_output; - usart_set_baud_rate(USART2, STM32_PCLK1, baud); - usart_enable(USART2); - } - void usart3_begin(uint32_t baud,uint32_t config ) - { - usart_init(USART3); - usart_config_gpios_async(USART3,GPIOB,PIN_MAP[PB11].gpio_bit,GPIOB,PIN_MAP[PB10].gpio_bit,config); - usart_set_baud_rate(USART3, STM32_PCLK1, baud); - usart_enable(USART3); - } - void init_HWTimer() - { - HWTimer2.pause(); // Pause the timer2 while we're configuring it - - TIMER2_BASE->PSC = 35; // 36-1;for 72 MHZ /0.5sec/(35+1) - TIMER2_BASE->ARR = 0xFFFF; // Count until 0xFFFF - - HWTimer2.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE); // Main scheduler - HWTimer2.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE); // Serial check - - TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer2/Comp2 interrupt flag - HWTimer2.attachInterrupt(TIMER_CH2,ISR_COMPB); // Assign function to Timer2/Comp2 interrupt - TIMER2_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer2/Comp2 interrupt - - HWTimer2.refresh(); // Refresh the timer's count, prescale, and overflow - HWTimer2.resume(); - } + void usart2_begin(uint32_t baud,uint32_t config ) + { + usart_init(USART2); + usart_config_gpios_async(USART2,GPIOA,PIN_MAP[PA3].gpio_bit,GPIOA,PIN_MAP[PA2].gpio_bit,config); + LED2_output; + usart_set_baud_rate(USART2, STM32_PCLK1, baud); + usart_enable(USART2); + } + void usart3_begin(uint32_t baud,uint32_t config ) + { + usart_init(USART3); + usart_config_gpios_async(USART3,GPIOB,PIN_MAP[PB11].gpio_bit,GPIOB,PIN_MAP[PB10].gpio_bit,config); + usart_set_baud_rate(USART3, STM32_PCLK1, baud); + usart_enable(USART3); + } + void init_HWTimer() + { + HWTimer2.pause(); // Pause the timer2 while we're configuring it + + TIMER2_BASE->PSC = 35; // 36-1;for 72 MHZ /0.5sec/(35+1) + TIMER2_BASE->ARR = 0xFFFF; // Count until 0xFFFF + + HWTimer2.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE); // Main scheduler + HWTimer2.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE); // Serial check + + TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer2/Comp2 interrupt flag + HWTimer2.attachInterrupt(TIMER_CH2,ISR_COMPB); // Assign function to Timer2/Comp2 interrupt + TIMER2_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer2/Comp2 interrupt + + HWTimer2.refresh(); // Refresh the timer's count, prescale, and overflow + HWTimer2.resume(); + } #endif #ifdef CHECK_FOR_BOOTLOADER void pollBoot() { - uint8_t rxchar ; - uint8_t lState = BootState ; - uint8_t millisTime = millis(); // Call this once only - - #ifdef ORANGE_TX - if ( USARTC0.STATUS & USART_RXCIF_bm ) - #elif defined STM32_BOARD - if ( USART2_BASE->SR & USART_SR_RXNE ) - #else - if ( UCSR0A & ( 1 << RXC0 ) ) - #endif - { - rxchar = UDR0 ; - BootCount += 1 ; - if ( ( lState == BOOT_WAIT_30_IDLE ) || ( lState == BOOT_WAIT_30_DATA ) ) - { - if ( lState == BOOT_WAIT_30_IDLE ) // Waiting for 0x30 - BootTimer = millisTime ; // Start timeout - if ( rxchar == 0x30 ) - lState = BOOT_WAIT_20 ; - else - lState = BOOT_WAIT_30_DATA ; - } - else - if ( lState == BOOT_WAIT_20 && rxchar == 0x20 ) // Waiting for 0x20 - lState = BOOT_READY ; - } - else // No byte received - { - if ( lState != BOOT_WAIT_30_IDLE ) // Something received - { - uint8_t time = millisTime - BootTimer ; - if ( time > 5 ) - { - #ifdef STM32_BOARD - if ( BootCount > 4 ) - #else - if ( BootCount > 2 ) - #endif - { // Run normally - NotBootChecking = 0xFF ; - Mprotocol_serial_init( 0 ) ; - } - else if ( lState == BOOT_READY ) - { - #ifdef STM32_BOARD - nvic_sys_reset(); - while(1); /* wait until reset */ - #else - cli(); // Disable global int due to RW of 16 bits registers - void (*p)(); - #ifndef ORANGE_TX - p = (void (*)())0x3F00 ; // Word address (0x7E00 byte) - #else - p = (void (*)())0x4000 ; // Word address (0x8000 byte) - #endif - (*p)() ; // go to boot - #endif - } - else - { - lState = BOOT_WAIT_30_IDLE ; - BootCount = 0 ; - } - } - } - } - BootState = lState ; + uint8_t rxchar ; + uint8_t lState = BootState ; + uint8_t millisTime = millis(); // Call this once only + + #ifdef ORANGE_TX + if ( USARTC0.STATUS & USART_RXCIF_bm ) + #elif defined STM32_BOARD + if ( USART2_BASE->SR & USART_SR_RXNE ) + #else + if ( UCSR0A & ( 1 << RXC0 ) ) + #endif + { + rxchar = UDR0 ; + BootCount += 1 ; + if ( ( lState == BOOT_WAIT_30_IDLE ) || ( lState == BOOT_WAIT_30_DATA ) ) + { + if ( lState == BOOT_WAIT_30_IDLE ) // Waiting for 0x30 + BootTimer = millisTime ; // Start timeout + if ( rxchar == 0x30 ) + lState = BOOT_WAIT_20 ; + else + lState = BOOT_WAIT_30_DATA ; + } + else + if ( lState == BOOT_WAIT_20 && rxchar == 0x20 ) // Waiting for 0x20 + lState = BOOT_READY ; + } + else // No byte received + { + if ( lState != BOOT_WAIT_30_IDLE ) // Something received + { + uint8_t time = millisTime - BootTimer ; + if ( time > 5 ) + { + #ifdef STM32_BOARD + if ( BootCount > 4 ) + #else + if ( BootCount > 2 ) + #endif + { // Run normally + NotBootChecking = 0xFF ; + Mprotocol_serial_init( 0 ) ; + } + else if ( lState == BOOT_READY ) + { + #ifdef STM32_BOARD + nvic_sys_reset(); + while(1); /* wait until reset */ + #else + cli(); // Disable global int due to RW of 16 bits registers + void (*p)(); + #ifndef ORANGE_TX + p = (void (*)())0x3F00 ; // Word address (0x7E00 byte) + #else + p = (void (*)())0x4000 ; // Word address (0x8000 byte) + #endif + (*p)() ; // go to boot + #endif + } + else + { + lState = BOOT_WAIT_30_IDLE ; + BootCount = 0 ; + } + } + } + } + BootState = lState ; } #endif //CHECK_FOR_BOOTLOADER #if defined(TELEMETRY) void PPM_Telemetry_serial_init() { - if( (protocol==PROTO_FRSKYD) || (protocol==PROTO_HUBSAN) || (protocol==PROTO_AFHDS2A) || (protocol==PROTO_BAYANG) || (protocol==PROTO_CABELL) || (protocol==PROTO_HITEC) || (protocol==PROTO_BUGS)) - initTXSerial( SPEED_9600 ) ; - if(protocol==PROTO_FRSKYX) - initTXSerial( SPEED_57600 ) ; - if(protocol==PROTO_DSM) - initTXSerial( SPEED_125K ) ; + if( (protocol==PROTO_FRSKYD) || (protocol==PROTO_HUBSAN) || (protocol==PROTO_AFHDS2A) || (protocol==PROTO_BAYANG) || (protocol==PROTO_CABELL) || (protocol==PROTO_HITEC) || (protocol==PROTO_BUGS)) + initTXSerial( SPEED_9600 ) ; + if(protocol==PROTO_FRSKYX) + initTXSerial( SPEED_57600 ) ; + if(protocol==PROTO_DSM) + initTXSerial( SPEED_125K ) ; } #endif // Convert 32b id to rx_tx_addr static void set_rx_tx_addr(uint32_t id) { // Used by almost all protocols - rx_tx_addr[0] = (id >> 24) & 0xFF; - rx_tx_addr[1] = (id >> 16) & 0xFF; - rx_tx_addr[2] = (id >> 8) & 0xFF; - rx_tx_addr[3] = (id >> 0) & 0xFF; - rx_tx_addr[4] = (rx_tx_addr[2]&0xF0)|(rx_tx_addr[3]&0x0F); + rx_tx_addr[0] = (id >> 24) & 0xFF; + rx_tx_addr[1] = (id >> 16) & 0xFF; + rx_tx_addr[2] = (id >> 8) & 0xFF; + rx_tx_addr[3] = (id >> 0) & 0xFF; + rx_tx_addr[4] = (rx_tx_addr[2]&0xF0)|(rx_tx_addr[3]&0x0F); } static uint32_t random_id(uint16_t address, uint8_t create_new) { - #ifndef FORCE_GLOBAL_ID - uint32_t id=0; - - #if 0 - //(eeprom_read_byte((EE_ADDR)(address+10))==0xf0 && !create_new) - { // TXID exists in EEPROM - for(uint8_t i=4;i>0;i--) - { - id<<=8; - id|=eeprom_read_byte((EE_ADDR)address+i-1); - } - if(id!=0x2AD141A7) //ID with seed=0 - { - debugln("Read ID from EEPROM"); - return id; - } - } - // Generate a random ID - #if defined STM32_BOARD - #define STM32_UUID ((uint32_t *)0x1FFFF7E8) - if (!create_new) - { - id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2]; - debugln("Generated ID from STM32 UUID"); - } - else - #endif + #ifndef FORCE_GLOBAL_ID + uint32_t id=0; + + #if 0 + //(eeprom_read_byte((EE_ADDR)(address+10))==0xf0 && !create_new) + { // TXID exists in EEPROM + for(uint8_t i=4;i>0;i--) + { + id<<=8; + id|=eeprom_read_byte((EE_ADDR)address+i-1); + } + if(id!=0x2AD141A7) //ID with seed=0 + { + debugln("Read ID from EEPROM"); + return id; + } + } + // Generate a random ID + #if defined STM32_BOARD + #define STM32_UUID ((uint32_t *)0x1FFFF7E8) + if (!create_new) + { + id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2]; + debugln("Generated ID from STM32 UUID"); + } + else + #endif #endif - id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16); + id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16); #if 0 - for(uint8_t i=0;i<4;i++) - eeprom_write_byte((EE_ADDR)address+i,id >> (i*8)); - eeprom_write_byte((EE_ADDR)(address+10),0xf0);//write bind flag in eeprom. + for(uint8_t i=0;i<4;i++) + eeprom_write_byte((EE_ADDR)address+i,id >> (i*8)); + eeprom_write_byte((EE_ADDR)(address+10),0xf0);//write bind flag in eeprom. + #endif + return id; + #else + (void)address; + (void)create_new; + return FORCE_GLOBAL_ID; #endif - return id; - #else - (void)address; - (void)create_new; - return FORCE_GLOBAL_ID; - #endif } /**************************/ @@ -1057,153 +1038,151 @@ static uint32_t random_id(uint16_t address, uint8_t create_new) //PPM #ifdef blubber // ENABLE_PPM - #ifdef ORANGE_TX - #if PPM_pin == 2 - ISR(PORTD_INT0_vect) - #else - ISR(PORTD_INT1_vect) - #endif - #elif defined STM32_BOARD - void PPM_decode() - #else - #if PPM_pin == 2 - ISR(INT0_vect, ISR_NOBLOCK) - #else - ISR(INT1_vect, ISR_NOBLOCK) - #endif - #endif - { // Interrupt on PPM pin - static int8_t chan=0,bad_frame=1; - static uint16_t Prev_TCNT1=0; - uint16_t Cur_TCNT1; - - Cur_TCNT1 = TCNT1 - Prev_TCNT1 ; // Capture current Timer1 value - if(Cur_TCNT1<1600) - bad_frame=1; // bad frame - else - if(Cur_TCNT1>4400) - { //start of frame - if(chan>=MIN_PPM_CHANNELS) - { - PPM_FLAG_on; // good frame received if at least 4 channels have been seen - if(chan>PPM_chan_max) PPM_chan_max=chan; // Saving the number of channels received - } - chan=0; // reset channel counter - bad_frame=0; - } - else - if(bad_frame==0) // need to wait for start of frame - { //servo values between 800us and 2200us will end up here - PPM_data[chan]=Cur_TCNT1; - if(chan++>=MAX_PPM_CHANNELS) - bad_frame=1; // don't accept any new channels - } - Prev_TCNT1+=Cur_TCNT1; - } + #ifdef ORANGE_TX + #if PPM_pin == 2 + ISR(PORTD_INT0_vect) + #else + ISR(PORTD_INT1_vect) + #endif + #elif defined STM32_BOARD + void PPM_decode() + #else + #if PPM_pin == 2 + ISR(INT0_vect, ISR_NOBLOCK) + #else + ISR(INT1_vect, ISR_NOBLOCK) + #endif + #endif + { // Interrupt on PPM pin + static int8_t chan=0,bad_frame=1; + static uint16_t Prev_TCNT1=0; + uint16_t Cur_TCNT1; + + Cur_TCNT1 = TCNT1 - Prev_TCNT1 ; // Capture current Timer1 value + if(Cur_TCNT1<1600) + bad_frame=1; // bad frame + else + if(Cur_TCNT1>4400) + { //start of frame + if(chan>=MIN_PPM_CHANNELS) + { + PPM_FLAG_on; // good frame received if at least 4 channels have been seen + if(chan>PPM_chan_max) PPM_chan_max=chan; // Saving the number of channels received + } + chan=0; // reset channel counter + bad_frame=0; + } + else + if(bad_frame==0) // need to wait for start of frame + { //servo values between 800us and 2200us will end up here + PPM_data[chan]=Cur_TCNT1; + if(chan++>=MAX_PPM_CHANNELS) + bad_frame=1; // don't accept any new channels + } + Prev_TCNT1+=Cur_TCNT1; + } #endif //ENABLE_PPM //Serial RX #ifdef ENABLE_SERIAL - #ifdef ORANGE_TX - ISR(USARTC0_RXC_vect) - #elif defined STM32_BOARD - void __irq_usart2() - #else - ISR(USART_RX_vect) - #endif - { // RX interrupt - static uint8_t idx=0; - #ifdef ORANGE_TX - if((USARTC0.STATUS & 0x1C)==0) // Check frame error, data overrun and parity error - #elif defined STM32_BOARD - if((USART2_BASE->SR & USART_SR_RXNE) && (USART2_BASE->SR &0x0F)==0) - #else - UCSR0B &= ~_BV(RXCIE0) ; // RX interrupt disable - sei() ; - if((UCSR0A&0x1C)==0) // Check frame error, data overrun and parity error - #endif - { // received byte is ok to process - if(idx==0||discard_frame==1) - { // Let's try to sync at this point - idx=0;discard_frame=0; - RX_MISSED_BUFF_off; // If rx_buff was good it's not anymore... - rx_buff[0]=UDR0; - #ifdef FAILSAFE_ENABLE - if((rx_buff[0]&0xFC)==0x54) // If 1st byte is 0x54, 0x55, 0x56 or 0x57 it looks ok - #else - if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok - #endif - { - TX_RX_PAUSE_on; - tx_pause(); - #if defined STM32_BOARD - TIMER2_BASE->CCR2=TIMER2_BASE->CNT+(6500L); // Full message should be received within timer of 3250us - TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer2/Comp2 interrupt flag - TIMER2_BASE->DIER |= TIMER_DIER_CC2IE; // Enable Timer2/Comp2 interrupt - #else - OCR1B = TCNT1+(6500L) ; // Full message should be received within timer of 3250us - TIFR1 = OCF1B_bm ; // clear OCR1B match flag - SET_TIMSK1_OCIE1B ; // enable interrupt on compare B match - #endif - idx++; - } - } - else - { - rx_buff[idx++]=UDR0; // Store received byte - if(idx>=RXBUFFER_SIZE) - { // A full frame has been received - if(!IS_RX_DONOTUPDATE_on) - { //Good frame received and main is not working on the buffer - memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer - RX_FLAG_on; // flag for main to process servo data - } - else - RX_MISSED_BUFF_on; // notify that rx_buff is good - discard_frame=1; // start again - } - } - } - else - { - idx=UDR0; // Dummy read - discard_frame=1; // Error encountered discard full frame... - debugln("Bad frame RX"); - } - if(discard_frame==1) - { - #ifdef STM32_BOARD - TIMER2_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer2/Comp2 interrupt - #else - CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match - #endif - TX_RX_PAUSE_off; - tx_resume(); - } - #if not defined (ORANGE_TX) && not defined (STM32_BOARD) - cli() ; - UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable - #endif - } - - //Serial timer - #ifdef ORANGE_TX - ISR(TCC1_CCB_vect) - #elif defined STM32_BOARD - void ISR_COMPB() - #else - ISR(TIMER1_COMPB_vect, ISR_NOBLOCK ) - #endif - { // Timer1 compare B interrupt - discard_frame=1; - #ifdef STM32_BOARD - TIMER2_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer2/Comp2 interrupt - debugln("Bad frame timer"); - #else - CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match - #endif - tx_resume(); - } -#endif //ENABLE_SERIAL - + #ifdef ORANGE_TX + ISR(USARTC0_RXC_vect) + #elif defined STM32_BOARD + void __irq_usart2() + #else + ISR(USART_RX_vect) + #endif + { // RX interrupt + static uint8_t idx=0; + #ifdef ORANGE_TX + if((USARTC0.STATUS & 0x1C)==0) // Check frame error, data overrun and parity error + #elif defined STM32_BOARD + if((USART2_BASE->SR & USART_SR_RXNE) && (USART2_BASE->SR &0x0F)==0) + #else + UCSR0B &= ~_BV(RXCIE0) ; // RX interrupt disable + sei() ; + if((UCSR0A&0x1C)==0) // Check frame error, data overrun and parity error + #endif + { // received byte is ok to process + if(idx==0||discard_frame==1) + { // Let's try to sync at this point + idx=0;discard_frame=0; + RX_MISSED_BUFF_off; // If rx_buff was good it's not anymore... + rx_buff[0]=UDR0; + #ifdef FAILSAFE_ENABLE + if((rx_buff[0]&0xFC)==0x54) // If 1st byte is 0x54, 0x55, 0x56 or 0x57 it looks ok + #else + if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok + #endif + { + TX_RX_PAUSE_on; + tx_pause(); + #if defined STM32_BOARD + TIMER2_BASE->CCR2=TIMER2_BASE->CNT+(6500L); // Full message should be received within timer of 3250us + TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer2/Comp2 interrupt flag + TIMER2_BASE->DIER |= TIMER_DIER_CC2IE; // Enable Timer2/Comp2 interrupt + #else + OCR1B = TCNT1+(6500L) ; // Full message should be received within timer of 3250us + TIFR1 = OCF1B_bm ; // clear OCR1B match flag + SET_TIMSK1_OCIE1B ; // enable interrupt on compare B match + #endif + idx++; + } + } + else + { + rx_buff[idx++]=UDR0; // Store received byte + if(idx>=RXBUFFER_SIZE) + { // A full frame has been received + if(!IS_RX_DONOTUPDATE_on) + { //Good frame received and main is not working on the buffer + memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer + RX_FLAG_on; // flag for main to process servo data + } + else + RX_MISSED_BUFF_on; // notify that rx_buff is good + discard_frame=1; // start again + } + } + } + else + { + idx=UDR0; // Dummy read + discard_frame=1; // Error encountered discard full frame... + debugln("Bad frame RX"); + } + if(discard_frame==1) + { + #ifdef STM32_BOARD + TIMER2_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer2/Comp2 interrupt + #else + CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match + #endif + TX_RX_PAUSE_off; + tx_resume(); + } + #if not defined (ORANGE_TX) && not defined (STM32_BOARD) + cli() ; + UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable + #endif + } + //Serial timer + #ifdef ORANGE_TX + ISR(TCC1_CCB_vect) + #elif defined STM32_BOARD + void ISR_COMPB() + #else + ISR(TIMER1_COMPB_vect, ISR_NOBLOCK ) + #endif + { // Timer1 compare B interrupt + discard_frame=1; + #ifdef STM32_BOARD + TIMER2_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer2/Comp2 interrupt + debugln("Bad frame timer"); + #else + CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match + #endif + tx_resume(); + } +#endif //ENABLE_SERIAL diff --git a/Multiprotocol/debug.h b/Multiprotocol/debug.h new file mode 100644 index 0000000..cf84734 --- /dev/null +++ b/Multiprotocol/debug.h @@ -0,0 +1,10 @@ +#ifndef DEBUG_H +#define DEBUG_H + +//******************** +//** Debug messages ** +//******************** + #define debug(msg, ...) { char buf[128]; sprintf(buf, msg, ##__VA_ARGS__); Serial.println(buf);} + #define debugln(msg, ...) { char buf[128]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); Serial.println(buf);} + #define debug_time(msg) { uint16_t debug_time_TCNT1=TCNT1; debug_time=debug_time_TCNT1-debug_time; debugln(msg "%u", debug_time); debug_time=debug_time_TCNT1; } +#endif diff --git a/Multiprotocol/inputs.ino b/Multiprotocol/inputs.ino index 223b703..8a76de6 100644 --- a/Multiprotocol/inputs.ino +++ b/Multiprotocol/inputs.ino @@ -1,22 +1,21 @@ void update_inputs(void) { - - INPUT_SIGNAL_on; - - if (state < FRSKY_BIND_DONE) - return; - #ifdef Throttle_pin + INPUT_SIGNAL_on; + + if (state < FRSKY_BIND_DONE) + return; + uint16_t throttle = analogRead(Throttle_pin); uint16_t yaw = analogRead(Yaw_pin); uint16_t roll = analogRead(Roll_pin); uint16_t pitch = analogRead(Pitch_pin); uint16_t aux1 = 0;//analogRead(Aux1_pin); uint16_t aux2 = analogRead(Aux2_pin); - + uint16_t ch_min=CHANNEL_MIN_100; uint16_t ch_max=CHANNEL_MAX_100; - + Channel_data[THROTTLE] += map(throttle, 0, 3500, ch_min, ch_max); Channel_data[RUDDER] += map(yaw, 0, 3500, ch_min, ch_max); Channel_data[AILERON] += map(roll, 0, 3500, ch_min, ch_max); @@ -24,23 +23,16 @@ void update_inputs(void) { //Channel_data[CH5] += map(aux1, 0, 3500, ch_min, ch_max); Channel_data[CH6] += map(aux2, 0, 3500, ch_min, ch_max); - + Channel_data[THROTTLE] /= 2; Channel_data[RUDDER] /= 2; Channel_data[AILERON] /= 2; Channel_data[ELEVATOR] /= 2; Channel_data[CH5] /= 2; Channel_data[CH6] /= 2; - + //debugln("T %d y %d r %d p %d a1 %d a2 %d",throttle,yaw,roll,pitch,aux1,aux2); //debugln("T %d y %d r %d p %d a1 %d a2 %d",Channel_data[THROTTLE],Channel_data[RUDDER],Channel_data[AILERON],Channel_data[ELEVATOR],Channel_data[CH5],Channel_data[CH6]); - #else - //analogRead() - if (Channel_data[THROTTLE] < CHANNEL_MAX_100) { - Channel_data[THROTTLE] += 1; - } else { - Channel_data[THROTTLE] = CHANNEL_MIN_100; - } - #endif } + diff --git a/Multiprotocol/state.cpp b/Multiprotocol/state.cpp new file mode 100644 index 0000000..b16befa --- /dev/null +++ b/Multiprotocol/state.cpp @@ -0,0 +1,139 @@ +#include +#include "state.h" +#include "Arduino.h" +#include "debug.h" + +LiquidCrystal_I2C lcd(0x27,16,2); + +State *curr_state = NULL; +State *new_state = NULL; + +State *s_init = new LCD_state_init(); +State *s_bind = new LCD_state_bind(); + + +enum lcd_special_chars { + battery_66 = 0, + battery_33 = 1, + battery_0 = 2, + battery_100 = 3, + rssiantenna = 4, + rssi_bars_1 = 5, + rssi_bars_2 = 6, + rssi_bars_3 = 7, + MAX_SPECIAL_CHARS =8, +}; +// 6 Byte-Arrays für 6 verschiedene Batteriesymbole +__extension__ struct lcd_special_chars_data { byte data[MAX_SPECIAL_CHARS]; } + lcd_special_chars_data[MAX_SPECIAL_CHARS] = +{ + //[battery_0] = + { 0b01110, 0b11011, 0b10001, 0b10001, 0b10001, 0b10001, 0b10001, 0b11111 }, + //[battery_33] = + { 0b01110, 0b11011, 0b10001, 0b10001, 0b10001, 0b11111, 0b11111, 0b11111 }, + //[battery_66] = + { 0b01110, 0b11011, 0b10001, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111 }, + //[battery_100] = + { 0b01110, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111 }, + //[rssiantenna] = + { 0b10101, 0b10101, 0b01110, 0b00100, 0b00100, 0b00101, 0b00101, 0b00101 }, + //[rssi_bars_1] = + { 0b00001, 0b00001, 0b00001, 0b00001, 0b00101, 0b00101, 0b10101, 0b10101 }, + //[rssi_bars_2] = + { 0b00001, 0b00001, 0b00001, 0b00001, 0b10101, 0b10101, 0b10101, 0b10101 }, + //[rssi_bars_3] = + { 0b00001, 0b00001, 0b00101, 0b00101, 0b10101, 0b10101, 0b10101, 0b10101 }, +}; + + +void install_special_caracters(void) +{ + for(int i = 0; i < MAX_SPECIAL_CHARS; i ++) { + lcd.createChar(i, lcd_special_chars_data[i].data); + } +} + +void init_state(void) { + + Wire.setSDA(PB9); + Wire.setSCL(PB8); + Wire.begin(); + lcd.init(); + + lcd.backlight(); + curr_state = NULL; + new_state = s_init; + + install_special_caracters(); + update_state(); +} + +void update_state(void) { + if(curr_state == new_state) { + if(curr_state) + curr_state->update(); + } else { + if(curr_state) + curr_state->leave(); + + curr_state = new_state; + + if(curr_state) + curr_state->enter(); + } +} + +//LCD_state_init + +LCD_state_init::LCD_state_init(void) { + snprintf(this->line1,sizeof(this->line2)," wellcome "); + snprintf(this->line2,sizeof(this->line2)," phschoen "); +} +void LCD_state_init::enter(void) { + lcd.setCursor(0,0); + lcd.print(this->line1); + lcd.setCursor(0,1); + lcd.print(this->line2); + time_enter = millis(); +} +void LCD_state_init::update(void) +{ + uint32_t diff; + diff = millis()-time_enter; + if (diff > 5 * 1000) { + new_state = s_bind; + } +} +void LCD_state_init::leave(void) +{ + +} +//LCD_state_bind +LCD_state_bind::LCD_state_bind(void) { + snprintf(this->line1,sizeof(this->line2),"bind mode "); + snprintf(this->line2,sizeof(this->line2)," "); + this->bind_time = 20; +} +void LCD_state_bind::enter(void) { + lcd.setCursor(0,0); + lcd.print(this->line1); + lcd.setCursor(0,1); + lcd.print(this->line2); + this->time_enter = millis(); +} + +void LCD_state_bind::update(void) +{ + debugln("blubber\n"); + unsigned long remain = millis() - this->time_enter; + remain = remain/1000; // to sec + remain = this->bind_time - remain; + snprintf(this->line2,sizeof(this->line2),"remaining sec %02d",remain); + lcd.setCursor(0,1); + lcd.print(this->line2); + time_enter = millis(); +} +void LCD_state_bind::leave(void) +{ + +} diff --git a/Multiprotocol/state.h b/Multiprotocol/state.h new file mode 100644 index 0000000..309cfe5 --- /dev/null +++ b/Multiprotocol/state.h @@ -0,0 +1,62 @@ + +#ifndef _STATE_H_ +#define _STATE_H_ + +#include + + +void init_state(void); +void update_state(void); + +class State { + protected: + + char line1[17]; + char line2[17]; + public: + virtual void enter(void) { + + } + virtual void update(void) { + + } + virtual void leave(void) { + + } + +}; + +extern State *new_state; + +class LCD_state_init: public State { +private: + unsigned long time_enter; +public: + LCD_state_init(void); + void enter(void); + void update(void); + void leave(void); +}; + +class LCD_state_bind: public State { +private: + unsigned long time_enter; + unsigned long bind_time; + +public: + LCD_state_bind(void); + void enter(void); + void update(void); + void leave(void); +}; + +class LCD_state_flight: public State { +private: + unsigned long time_enter; + +public: + void enter(void); + void update(void); + void leave(void); +}; +#endif /*_STATE_H_*/