From f28cb609f3cca613c79d955581efdddad9a87adc Mon Sep 17 00:00:00 2001 From: Dennis Date: Mon, 19 Dec 2016 12:34:19 -0500 Subject: [PATCH] Added CABELL Protocol --- Multiprotocol/CABELL_nrf224l01.ino | 321 +++++++++++++++++++++++++++++ Multiprotocol/Multiprotocol.h | 10 + Multiprotocol/Multiprotocol.ino | 8 +- Multiprotocol/Validate.h | 1 + 4 files changed, 339 insertions(+), 1 deletion(-) create mode 100644 Multiprotocol/CABELL_nrf224l01.ino diff --git a/Multiprotocol/CABELL_nrf224l01.ino b/Multiprotocol/CABELL_nrf224l01.ino new file mode 100644 index 0000000..3faac6f --- /dev/null +++ b/Multiprotocol/CABELL_nrf224l01.ino @@ -0,0 +1,321 @@ +/* + 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 . + */ +// +// My own personal protocol + +#if defined(CABELL_NRF24L01_INO) + +#include "iface_nrf24l01.h" + +#define CABELL_BIND_COUNT 2000 // At least 2000 so that if TX toggles the serial bind flag then bind mode is never exited +#define CABELL_PACKET_PERIOD 3000 // Do not set too low or else next packet may not be finished transmitting before the channel is changed next time around +#define CABELL_NUM_CHANNELS 16 +#define CABELL_MIN_CHANNELS 4 +#define CABELL_PAYLOAD_BYTES 24 // 12 bits per value +#define CABELL_BIND_RADIO_ADDR 0xA4B7C123F7LL + +#define CABELL_OPTION_MASK_CHANNEL_REDUCTION 0x0F +#define CABELL_OPTION_MASK_RECIEVER_OUTPUT_MODE 0x70 +#define CABELL_OPTION_SHIFT_RECIEVER_OUTPUT_MODE 4 +#define CABELL_OPTION_MASK_MAX_POWER_OVERRIDE 0x80 + +typedef struct { + enum RxMode_t : uint8_t { // Note bit 8 is used to indicate if the packet is the first of 2 on the channel. Mask out this bit before using the enum + normal = 0, + bind = 1, + unBind = 127 + } RxMode; + uint8_t reserved = 0; + uint8_t option; + /* mask 0x0F : Channel reduction. The number of channels to not send (subtracted frim the 16 max channels) at least 4 are always sent + * mask 0x70>>4 : Reciever outout mode + * 1 = Single PPM on individual pins for each channel + * 2 = SUM PPM on channel 1 pin + * mask 0x80>>7 Unused by RX. Contains max power override flag + */ + uint8_t modelNum; + uint16_t checkSum; + uint8_t payloadValue [CABELL_PAYLOAD_BYTES] = {0}; //12 bits per channel value, unsigned +} CABELL_RxTxPacket_t; + +//----------------------------------------------------------------------------------------- +static uint8_t __attribute__((unused)) CABELL_getNextChannel (uint8_t seqArray[], uint8_t seqArraySize, uint8_t prevChannel) { + /* Possible channels are in 5 bands, each band comprised of seqArraySize channels + * seqArray contains seqArraySize elements in the relative order in which we should progress through the band + * + * Each time the channel is changes, bands change in a way so that the next channel will be in a + * different non-adjacent band. Both the band changes and the index in seqArray is incremented. + */ + prevChannel = constrain(prevChannel,1,seqArraySize * 5); // Constain the values just in case something bogus was sent in. + prevChannel--; // Subtract one becasue 1 was added to the return value + + uint8_t currBand = prevChannel / seqArraySize; + uint8_t nextBand = (currBand + 3) % 5; + + uint8_t prevChannalSeqArrayValue = prevChannel % seqArraySize; + uint8_t prevChannalSeqArrayPosition = 0; + for (int x = 0; x < seqArraySize; x++) { // Find the position of the previous channel in the array + if (seqArray[x] == prevChannalSeqArrayValue) { + prevChannalSeqArrayPosition = x; + } + } + uint8_t nextChannalSeqArrayPosition = prevChannalSeqArrayPosition + 1; + if (nextChannalSeqArrayPosition >= seqArraySize) nextChannalSeqArrayPosition = 0; + + return (seqArraySize * nextBand) + seqArray[nextChannalSeqArrayPosition] + 1; // Add one so we dont use channel 0 as it may bleed below 2.400 GHz +} + +//----------------------------------------------------------------------------------------- +static void __attribute__((unused)) CABELL_send_packet(uint8_t bindMode) +{ + CABELL_RxTxPacket_t TxPacket; + static uint8_t currentChannel = 0; + uint8_t maskFirstPacketOnChannelBit = 0x80; + + uint8_t channelReduction = constrain((option & CABELL_OPTION_MASK_CHANNEL_REDUCTION),0,CABELL_NUM_CHANNELS-CABELL_MIN_CHANNELS); // Max 12 - cannot reduce below 4 channels + if (bindMode) { + channelReduction = 0; // Send full packet to bind as higher channels will contain bind info + } + uint8_t packetSize = sizeof(TxPacket) - ((((channelReduction - (channelReduction%2))/ 2)) * 3); // reduce 3 bytes per 2 channels, but not last channel if it is odd + uint8_t maxPayloadValueIndex = sizeof(TxPacket.payloadValue) - (sizeof(TxPacket) - packetSize); + + if (sub_protocol == CABELL_UNBIND) { + TxPacket.RxMode = CABELL_RxTxPacket_t::RxMode_t::unBind; + TxPacket.option = option & (~CABELL_OPTION_MASK_CHANNEL_REDUCTION); //remove channel reduction if in unBind mode + } else { + TxPacket.RxMode = (bindMode) ? CABELL_RxTxPacket_t::RxMode_t::bind : CABELL_RxTxPacket_t::RxMode_t::normal; + TxPacket.option = (bindMode) ? (option & (~CABELL_OPTION_MASK_CHANNEL_REDUCTION)) : option; //remove channel reduction if in bind mode + } + TxPacket.reserved = 0; + TxPacket.modelNum = RX_num; + TxPacket.checkSum = TxPacket.modelNum + TxPacket.option + TxPacket.RxMode + TxPacket.reserved; // Start Calculate checksum + + int adjusted_x; + int payloadIndex = 0; + uint16_t holdValue; + + for (int x = 0;(x < CABELL_NUM_CHANNELS - channelReduction); x++) { + switch (x) { + case 0 : adjusted_x = ELEVATOR; break; + case 1 : adjusted_x = AILERON; break; + case 2 : adjusted_x = RUDDER; break; + case 3 : adjusted_x = THROTTLE; break; + default : adjusted_x = x; break; + } + holdValue = map(limit_channel_100(adjusted_x),servo_min_100,servo_max_100,1000,2000); // valid channel values are 1000 to 2000 + if (bindMode) { + switch (adjusted_x) { + case THROTTLE : holdValue = 1000; break; // always set throttle to off when binding for safety + //tx address sent for bind + case 11 : holdValue = 1000 + rx_tx_addr[0]; break; + case 12 : holdValue = 1000 + rx_tx_addr[1]; break; + case 13 : holdValue = 1000 + rx_tx_addr[2]; break; + case 14 : holdValue = 1000 + rx_tx_addr[3]; break; + case 15 : holdValue = 1000 + rx_tx_addr[4]; break; +// case 11 : holdValue = 1000 + ((((uint64_t)CABELL_normal_addr)>>32) & 0x00000000000000FF); break; +// case 12 : holdValue = 1000 + ((((uint64_t)CABELL_normal_addr)>>24) & 0x00000000000000FF); break; +// case 13 : holdValue = 1000 + ((((uint64_t)CABELL_normal_addr)>>16) & 0x00000000000000FF); break; +// case 14 : holdValue = 1000 + ((((uint64_t)CABELL_normal_addr)>>8) & 0x00000000000000FF); break; +// case 15 : holdValue = 1000 + ((((uint64_t)CABELL_normal_addr)) & 0x00000000000000FF); break; + } + } + + // use 12 bits per value + if (x % 2) { //output channel number is ODD + holdValue = holdValue<<4; + payloadIndex--; + } else { + holdValue &= 0x0FFF; + } + TxPacket.payloadValue[payloadIndex] |= (uint8_t)(holdValue & 0x00FF); + payloadIndex++; + TxPacket.payloadValue[payloadIndex] |= (uint8_t)((holdValue>>8) & 0x00FF); + payloadIndex++; + } + + for(int x = 0; x < maxPayloadValueIndex ; x++) { + TxPacket.checkSum = TxPacket.checkSum + TxPacket.payloadValue[x]; // Finish Calculate checksum + } + + // Set channel for next transmission + rf_ch_num = CABELL_getNextChannel (hopping_frequency,CABELL_NUM_CHANNELS, rf_ch_num); + NRF24L01_WriteReg(NRF24L01_05_RF_CH,rf_ch_num); + + NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); + NRF24L01_FlushTx(); //just in case things got hung up + + uint8_t* p = reinterpret_cast(&TxPacket.RxMode); + *p &= 0x7F; // Make sure 8th bit is clear + *p |= (packet_count++)<<7; // This causes the 8th bit of the first byte to toggle with each xmit so consecrutive payloads are not identical. + // This is a work around for a reported bug in clone NRF24L01 chips that mis-took this case for a re-transmit of the same packet. + + NRF24L01_WritePayload((uint8_t*)&TxPacket, packetSize); +} + +//----------------------------------------------------------------------------------------- +static void __attribute__((unused)) CABELL_getChannelSequence (uint8_t outArray[], uint8_t numChannels, uint64_t permutation) { + /* This procedure initializes an array with the sequence progression of channels. + * This is not the actual channels itself, but the sequence base to be used within bands of + * channels. + * + * There are numChannels! permutations for arranging the channels + * one of these permutations will be calculated based on the permutation input + * permutation should be between 1 and numChannels! but the routine will constrain it + * if these bounds are exceeded. Typically the radio's unique TX ID shouldbe used. + * + * The maximum numChannels is 20. Anything larget than this will cause the uint64_t + * variables to overflow, yielding unknown resutls (possibly infinate loop?). Therefor + * this routine constrains the value. + */ + uint64_t i; //iterator counts numChannels + uint64_t indexOfNextSequenceValue; + uint64_t numChannelsFactorial=1; + uint8_t sequenceValue; + + numChannels = constrain(numChannels,1,20); + + for (i = 1; i <= numChannels;i++) { + numChannelsFactorial *= i; // Calculate n! + outArray[i-1] = i-1; // Initialize array with the sequence + } + + permutation = constrain(permutation,1,numChannelsFactorial); // k must be between 1 and n! or this algorithm will infinate loop + + //Rearrange the array elements based on the permutation selected + for (i=0, permutation--; i i; indexOfNextSequenceValue--) { + outArray[indexOfNextSequenceValue] = outArray[indexOfNextSequenceValue-1]; + } + // Copy the selected value into it's new array slot + outArray[i] = sequenceValue; + } +} + +//----------------------------------------------------------------------------------------- +static void __attribute__((unused)) CABELL_setAddress() +{ + uint64_t CABELL_addr; + +// Serial.print("NORM ID: ");Serial.print((uint32_t)(CABELL_normal_addr>>32)); Serial.print(" ");Serial.println((uint32_t)((CABELL_normal_addr<<32)>>32)); + + if (IS_BIND_DONE_on) { + CABELL_addr = (((uint64_t)rx_tx_addr[0]) << 32) + + (((uint64_t)rx_tx_addr[1]) << 24) + + (((uint64_t)rx_tx_addr[2]) << 16) + + (((uint64_t)rx_tx_addr[3]) << 8) + + (((uint64_t)rx_tx_addr[4])); // Address to use after binding + } + else { + CABELL_addr = CABELL_BIND_RADIO_ADDR; //static addr for binding + } + + CABELL_getChannelSequence(hopping_frequency,CABELL_NUM_CHANNELS,CABELL_addr); // Get the sequence for hopping through channels + rf_ch_num = CABELL_getNextChannel (hopping_frequency,CABELL_NUM_CHANNELS, 1); // initialize the channel sequence + + packet_count=0; + + NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, reinterpret_cast(&CABELL_addr), 5); + NRF24L01_WriteRegisterMulti(NRF24L01_0B_RX_ADDR_P1, reinterpret_cast(&CABELL_addr), 5); + NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, reinterpret_cast(&CABELL_addr), 5); +} + +//----------------------------------------------------------------------------------------- +static void __attribute__((unused)) CABELL_init() +{ + NRF24L01_Initialize(); + CABELL_SetPower(); + NRF24L01_SetBitrate(NRF24L01_BR_250K); + NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknowledgement on all data pipes + NRF24L01_SetTxRxMode(TX_EN); //Power up and 16 bit CRC + + CABELL_setAddress(); + + NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); + NRF24L01_FlushTx(); + NRF24L01_FlushRx(); + NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x02); + NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, 0x20); // 32 byte packet length + NRF24L01_WriteReg(NRF24L01_12_RX_PW_P1, 0x20); // 32 byte packet length + NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); + NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x5F); // no retransmits + NRF24L01_Activate(0x73); // Activate feature register + NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x3F); // Enable dynamic payload length on all pipes + NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x04); // Enable dynamic Payload Length + NRF24L01_Activate(0x73); +} + +//----------------------------------------------------------------------------------------- +static void CABELL_SetPower() // This over-ride the standard Set Power to allow an flag in option to indicate max power setting + // Note that on many modules max power may actually be worse than the normal high power setting + // test and only use max if it helps the range +{ + if(IS_BIND_DONE_on && !IS_RANGE_FLAG_on && (option & CABELL_OPTION_MASK_MAX_POWER_OVERRIDE)) { // If we are not in range or bind mode and power setting override is in effect, then set max power, else standard pawer logic + if(prev_power != NRF_POWER_3) // prev_power is global variable for NRF24L01; NRF_POWER_3 is max power + { + uint8_t rf_setup = NRF24L01_ReadReg(NRF24L01_06_RF_SETUP); + rf_setup = (rf_setup & 0xF9) | (NRF_POWER_3 << 1); + NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, rf_setup); + prev_power=NRF_POWER_3; + } + } + else { + NRF24L01_SetPower(); + } +} + +//----------------------------------------------------------------------------------------- +uint16_t CABELL_callback() +{ + if(IS_BIND_DONE_on) + CABELL_send_packet(0); + else + { + if (bind_counter == 0) + { + BIND_DONE; + CABELL_init(); // non-bind address + } + else + { + CABELL_send_packet(1); + bind_counter--; + } + } + return CABELL_PACKET_PERIOD; +} + +//----------------------------------------------------------------------------------------- +uint16_t initCABELL(void) +{ + if (IS_BIND_DONE_on) { + bind_counter = 0; + } + else + { + bind_counter = CABELL_BIND_COUNT; + } + CABELL_init(); + return CABELL_PACKET_PERIOD; +} + +#endif diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 5354123..e3481a5 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -55,6 +55,7 @@ enum PROTOCOLS MODE_OPENLRS = 27, // =>OpenLRS hardware MODE_AFHDS2A = 28, // =>A7105 MODE_Q2X2 = 29, // =>NRF24L01, extension of CX-10 protocol + MODE_CABELL = 30, // =>NRF24L01 }; enum Flysky @@ -173,6 +174,11 @@ enum FY326 FY326 = 0, FY319 = 1, }; +enum CABELL +{ + CABELL_V3 = 0, + CABELL_UNBIND = 7 +}; #define NONE 0 #define P_HIGH 1 @@ -442,6 +448,7 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- OpenLRS 27 AFHDS2A 28 Q2X2 29 + CABELL 30 BindBit=> 0x80 1=Bind/0=No AutoBindBit=> 0x40 1=Yes /0=No RangeCheck=> 0x20 1=Yes /0=No @@ -529,6 +536,9 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- sub_protocol==FY326 FY326 0 FY319 1 + sub_protocol==CABELL + CABELL_V3 0, + CABELL_UNBIND 7 Power value => 0x80 0=High/1=Low Stream[3] = option_protocol; diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 2a051aa..04e0234 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -852,7 +852,13 @@ static void protocol_init() remote_callback = HONTAI_callback; break; #endif - #endif + #if defined(CABELL_NRF24L01_INO) + case MODE_CABELL: + next_callback=initCABELL(); + remote_callback = CABELL_callback; + break; + #endif + #endif } if(next_callback>32000) diff --git a/Multiprotocol/Validate.h b/Multiprotocol/Validate.h index 6fbd9e3..dcacc1b 100644 --- a/Multiprotocol/Validate.h +++ b/Multiprotocol/Validate.h @@ -64,6 +64,7 @@ #undef FQ777_NRF24L01_INO #undef ASSAN_NRF24L01_INO #undef HONTAI_NRF24L01_INO + #undef CABELL_NRF24L01_INO #endif //Make sure telemetry is selected correctly