gpsEnablePassthroughResult_e gpsEnablePassthrough(void)
{
    if (gpsData.state != GPS_RECEIVING_DATA)
        return GPS_PASSTHROUGH_NO_GPS;

    serialPort_t *gpsPassthroughPort = findOpenSerialPort(FUNCTION_GPS_PASSTHROUGH);
    if (gpsPassthroughPort) {

        waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
        serialSetBaudRate(gpsPassthroughPort, serialConfig->gps_passthrough_baudrate);
    } else {
        gpsPassthroughPort = openSerialPort(FUNCTION_GPS_PASSTHROUGH, NULL, serialConfig->gps_passthrough_baudrate, MODE_RXTX, SERIAL_NOT_INVERTED);
        if (!gpsPassthroughPort) {
            return GPS_PASSTHROUGH_NO_SERIAL_PORT;
        }
    }

    LED0_OFF;
    LED1_OFF;

    while(1) {
        if (serialTotalBytesWaiting(gpsPort)) {
            LED0_ON;
            serialWrite(gpsPassthroughPort, serialRead(gpsPort));
            LED0_OFF;
        }
        if (serialTotalBytesWaiting(gpsPassthroughPort)) {
            LED1_ON;
            serialWrite(gpsPort, serialRead(gpsPassthroughPort));
            LED1_OFF;
        }
    }
    return GPS_PASSTHROUGH_ENABLED;
}

int run_command(int fd, uint8_t *data, uint8_t len)
{
    uint8_t inbuf[2];
    uint8_t sum = 0;
    uint8_t *sump = data;
    uint8_t i = len;
    uint8_t packet_len = len+2;

    while(i--)
        sum += *sump++;

    if (1 != serialWrite(fd, &packet_len, 1))
        return 1;
    if (1 != serialWrite(fd, &sum, 1))
        return 1;
    if (len != serialWrite(fd, data, len))
        return 1;
    if (2 != serialRead(fd, inbuf, 2))
        return 1;
    if (0x00 != inbuf[0] || 0xCC != inbuf[1])
    {
        fprintf(stderr, "NAK\n");
        return 1;
    }
    return 0;
}

static void ltm_initialise_packet(uint8_t ltm_id)
{
    ltm_crc = 0;
    serialWrite(ltmPort, '$');
    serialWrite(ltmPort, 'T');
    serialWrite(ltmPort, ltm_id);
}

void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
{
    waitForSerialPortToFinishTransmitting(gpsState.gpsPort);
    waitForSerialPortToFinishTransmitting(gpsPassthroughPort);

    if(!(gpsState.gpsPort->mode & MODE_TX))
    serialSetMode(gpsState.gpsPort, gpsState.gpsPort->mode | MODE_TX);

    LED0_OFF;
    LED1_OFF;

    char c;
    while(1) {
        if (serialRxBytesWaiting(gpsState.gpsPort)) {
            LED0_ON;
            c = serialRead(gpsState.gpsPort);
            serialWrite(gpsPassthroughPort, c);
            LED0_OFF;
        }
        if (serialRxBytesWaiting(gpsPassthroughPort)) {
            LED1_ON;
            c = serialRead(gpsPassthroughPort);
            serialWrite(gpsState.gpsPort, c);
            LED1_OFF;
        }
    }
}

void stmWriteCommand(serialStruct_t *s, char *msb, char *lsb, char *len, char *data) {
	char startAddress[9];
	char lenPlusData[128];
	char c;

	strncpy(startAddress, msb, sizeof(startAddress));
	strcat(startAddress, lsb);

	sprintf(lenPlusData, "%02x%s", stmHexToChar(len) - 1, data);

	write:
	// send WRITE MEMORY command
	do {
		c = getResults[5];
		serialWrite(s, &c, 1);
		c = 0xff ^ c;
		serialWrite(s, &c, 1);
	} while (!stmWaitAck(s, STM_RETRIES_LONG));

	// send address
	if (!stmWrite(s, startAddress)) {
		putchar('A');
		goto write;
	}

	// send len + data
	if (!stmWrite(s, lenPlusData)) {
		putchar('D');
		goto write;
	}

	putchar('='); fflush(stdout);
}

int DLS::setOutputFilter (int nsamples, int nspikes, int nerrors)
{
    // Request Current Config
    char write_data[] = "s0fi\r\n";
    serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));

    // Read Current Config
    char read_data[] = "g0fi+aa+bb+cc\r\n";
    serialRead(read_data);
    //
    // Check for error indicator (@) and parse error value.
    if (read_data[2]=='@') {
        char errcode_str [] = "000";
        sprintf (errcode_str, "%*s", 3, read_data+4);
        return(-1 * atoi (errcode_str));
    }

    char aa_str [] = "aa";
    aa_str[0] = read_data [5];
    aa_str[1] = read_data [6];
    char bb_str [] = "bb";
    bb_str[0] = read_data [8];
    bb_str[1] = read_data [9];
    char cc_str [] = "cc";
    cc_str[0] = read_data [11];
    cc_str[1] = read_data [12];

    if ((nsamples < 0) || (nsamples > 32))
        nsamples = atoi (aa_str);
    if (nspikes < 0)
        nspikes = atoi (bb_str);
    if (nerrors < 0)
        nerrors = atoi (cc_str);

    debug_print("samples %i\n", nsamples);
    debug_print("spikes %i\n", nspikes);
    debug_print("errors %i\n", nerrors);

    if (2*nspikes+nerrors > 0.4 * nsamples) {
        fprintf(stderr, "ERROR: Make sure that (2*nspikes + nerrors) <= nsamples\n");
        nspikes = atoi (bb_str);
        nerrors = atoi (cc_str);
        nsamples = atoi (aa_str);
    }

    char write_data2[] = "s0fi+aa+bb+cc\r\n";
    sprintf(write_data2, "s0fi+%02i+%02i+%02i\r\n",nsamples,nspikes,nerrors);

    serialWrite(write_data2, sizeof(write_data2)/sizeof(write_data[0]));

    int status = rxData();
    if (status < 0)
        printErrorMsg(status);

    return(status);
}

int main(int argc, char *argv[]) {
	SerialPort SP;
	char buf[1025];
	int nr;

	if (argc<1) {
		printf("Usage: setupbluesmirf <device>\n");
		printf("  <device> must be your serial port, e.g, COM3: on Windows, or /dev/ttyS0 on Linux\n");
		return 0;
	}
		
	if (!serialOpenByName(&SP, argv[1])) {
		fprintf(stderr, "Could not open serial port %s\n", argv[1]);
		return 1;
	}
	
	// last parameter is timeout in 1/10 of a second
	if (!serialSetParameters(&SP, 57600, 8, 0, 1, 1)) {
		fprintf(stderr, "Could not modify serial port parameters\n");
		return 1;
	}
	
	nr = serialRead(&SP, 1024, buf);
	
	serialWrite(&SP, 3, "$$$");
	nr = serialRead(&SP, 1024, buf);
	if (nr > 0) {
		buf[nr]=0;
		printf("Received after $$$: %s", buf);
	} else {
		goto cleanup;
	}
	
	serialWrite(&SP, 2, "D\r");
	nr = serialRead(&SP, 1024, buf);
	if (nr > 0) {
		buf[nr]=0;
		printf("Received after D<cr>: %s", buf);
	} else {
		goto cleanup;
	}
	
	serialWrite(&SP, 9, "U,576K,N\r");
	nr = serialRead(&SP, 1024, buf);
	if (nr > 0) {
		buf[nr]=0;
		printf("Received after U,576K,N<cr>: %s", buf);
	} else {
		goto cleanup;
	}	
	
cleanup:
	serialClose(&SP);
	
	return 0;
}

void serialize16(int16_t a)
{
    static uint8_t t;
    t = a;
    serialWrite(core.mainport, t);
    checksum ^= t;
    t = a >> 8 & 0xff;
    serialWrite(core.mainport, t);
    checksum ^= t;
}

// Get a 16 bit value off the serial port by doing two successive reads
//  Assumes that data is being transmitted high byte first
int16_t get16bitValue(int serialIndex){
	int16_t returnValue = 0;
	serialWrite(serialIndex);
	serialIndex++;
	returnValue = serialRead(25);
	
	serialWrite(serialIndex);
	serialIndex++;
	returnValue += serialRead(25) << 8;	
	return returnValue;
}

void loop() {
    for(i = 0; i < 6; ++i) {
        int old = accum[i];
        v = analogRead(i);
        v = smooth(v, &accum[i], 100);
        if(v != old) {
            serialWrite(0x80 | i);
            serialWrite(v >> 7);
            serialWrite(v & 0x7F);
            x = 5;
        }
    }

static void serializeFrsky(uint8_t data)
{
    // take care of byte stuffing
    if (data == 0x5e) {
        serialWrite(frskyPort, 0x5d);
        serialWrite(frskyPort, 0x3e);
    } else if (data == 0x5d) {
        serialWrite(frskyPort, 0x5d);
        serialWrite(frskyPort, 0x3d);
    } else
        serialWrite(frskyPort, data);
}

int stmWriteLen(serialStruct_t *s, char *data, int len, unsigned char ck) {
	unsigned char c;
	int i;

	for (i = 0; i < len; i++) {
		c = data[i];
		ck ^= c;
		serialWrite(s, (char *)&c, 1);
	}
	serialWrite(s, (char *)&ck, 1);

	return stmWaitAck(s, STM_RETRIES_LONG);
}

/**
*void setServo(int servo, int pos)
*@param servo - int, what servo channel are we talking about?
*@param pos - int, the position or the speed of the servo being requested
*@param hf - int, whether or not the servo is in half or full duplex. 0 = half, 1 is full.
*@return void
*
*Sets a motor as half duplex or full duplex. All default as 
*/
void setServo(int servo, int pos, int hf){
	int startBit;

	//If the servo is in full duplex, set it to such.
	if(hf == FULL) servo += 8;
	
	startBit = STARTBIT;

	serialWrite(4, startBit);
	serialWrite(4, servo);
	serialWrite(4, pos);

}

void writeEscapedByte(
 unsigned char byte,
 GioEndpoint *gioEndpoint){
 unsigned char escapeChar = ESCAPE_CHAR;
 switch(byte){
  case START_FLAG:
  case END_FLAG:
  case ESCAPE_CHAR:
    serialWrite(gioEndpoint, escapeChar);
  default:
   break;
 }
 serialWrite(gioEndpoint, byte);
}

void slaveLoop()
{
  watchdogReset();
  uint32_t now = micros();
  bool     needHop=false;
  switch (state) {
  case 0: // waiting for packet
    if (RF_Mode == Received) {
      Green_LED_ON;
      Red_LED_OFF;
      // got packet
      lastReceived = now;
      lostpkts=0;
      linkQuality = (linkQuality << 1) | 1;

      RF_Mode = Receive;

      spiSendAddress(0x7f); // Send the package read command
      for (int16_t i = 0; i < bind_data.packetSize; i++) {
        rx_buf[i] = spiReadData();
      }

      uint8_t payloadBytes = (rx_buf[0] & 0x3f);
      if (payloadBytes > (bind_data.packetSize - 1)) {
        // INVALID DATA
        payloadBytes = 0;
      }

      // Check if this is a new packet from master and not a resent one
      if ((rx_buf[0] ^ tx_buf[0]) & MASTER_SEQ) {
        tx_buf[0] ^= MASTER_SEQ;
        if (payloadBytes) {
          // DATA FRAME
          if (bind_data.flags & PACKET_MODE) {
            serialWrite(0xf0);
            serialWrite(payloadBytes);
            CRCtx = 0;
          }
          for (uint8_t i=0; i < payloadBytes; i++) {
            serialWrite(rx_buf[1 + i]);
            if ((bind_data.flags & PACKET_MODE) && (bind_data.flags & PACKETCRC_MODE)) {
              CRC16_add(&CRCtx, rx_buf[1 + i]);
            }
          }
          if ((bind_data.flags & PACKET_MODE) && (bind_data.flags & PACKETCRC_MODE)) {
            serialWrite(CRCtx >> 8);
            serialWrite(CRCtx & 0xff);
          }
        }
      }

  __interrupt void ReceiveInterrupt(void)
{
	char rec = UCA0RXBUF;
	if(rec == '1'){
		serialWrite('0');
	}else if(rec == '2'){
		serialWrite('9');
	}else if(rec == '3'){
		serialWrite('0');
	}else if(rec == '4'){
		serialWrite('1');
	}else if(rec == 'a' && alarm_state ==3){
		TA0CCR1 = period>>2;
		alarm_state = 4;
	}

void serialPrint(serialPort_t *instance, const char *str)
{
    uint8_t ch;
    while ((ch = *(str++)) != 0) {
        serialWrite(instance, ch);
    }
}

void sendPacket(TPacket *packet)
{
	char buffer[PACKET_SIZE];
	int len = serialize(buffer, packet, sizeof(TPacket));

	serialWrite(buffer, len);
}

static void _putc(void *p, char c)
{
    (void)p; // avoid compiler warning about unused variable
    serialWrite(Serial1, c);

    while (!isSerialTransmitBufferEmpty(Serial1));
}

void cliUsage(cliCommand_t *cmd) {
    serialPrint("usage: ");
    serialPrint(cmd->name);
    serialWrite(' ');
    serialPrint(cmd->params);
    serialPrint("\r\n");
}