 	delayMicroseconds(100);
 }
 
+// Calculate rx5808 register hex value for given frequency in MHz
+uint16_t freqMhzToRegVal(uint16_t freqInMhz) {
+  uint16_t tf, N, A;
+  tf = (freqInMhz - 479) / 2;
+  N = tf / 32;
+  A = tf % 32;
+  return (N<<7) + A;
+}
+
 // Set the frequency given on the rx5808 module
 void setRxModule(int frequency) {
 	uint8_t i; // Used in the for loops
 
-	uint8_t index; // Find the index in the frequency lookup table
-	for (i = 0; i < sizeof(vtxFreqTable); i++) {
-		if (frequency == vtxFreqTable[i]) {
-			index = i;
-			break;
-		}
-	}
-
-	uint16_t vtxHex; // Get the hex value to send to the rx module
-	vtxHex = vtxHexTable[index];
+	// Get the hex value to send to the rx module
+	uint16_t vtxHex = freqMhzToRegVal(frequency);
 
 	// bit bash out 25 bits of data / Order: A0-3, !R/W, D0-D19 / A0=0, A1=0, A2=0, A3=1, RW=0, D0-19=0
 	SERIAL_ENABLE_HIGH();

// Node Setup -- Set the i2c address here
// Node 1 = 8, Node 2 = 10, Node 3 = 12, Node 4 = 14
// Node 5 = 16, Node 6 = 18, Node 7 = 20, Node 8 = 22
#define i2cSlaveAddress 20

const int slaveSelectPin = 10; // Setup data pins for rx5808 comms
const int spiDataPin = 11;

int ioBufferSize = 0;
int ioBufferIndex = 0;

// Defines for fast ADC reads
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
