int stm32CmdGet()
{
	int ret = STM_ERR;

	if ( sendCommand(0x00)==STM_OK ) {
		int i;
		unsigned char num;

		serialRead(board.serial_fd, &num, 1);	//	number of bytes
		serialRead(board.serial_fd, &board.version, 1);
		serialRead(board.serial_fd, board.cmd, num);

		if ( waitAck()==STM_OK ) {
			printf("Bootloader version: %x.%x\nCommand set-[", board.version >> 4, board.version & 0x0F);
			for ( i=0; i < num; i++ )
				printf(" %02X", board.cmd[i]);
			printf("]\n");
			ret = STM_OK;
		}

void serialCom(void)
{
    uint8_t c;
    int i;

    for (i = 0; i < numTelemetryPorts; i++) {
        currentPortState = &ports[i];

        // in cli mode, all serial stuff goes to here. enter cli mode by sending #
        if (cliMode) {
            cliProcess();
            return;
        }

        if (pendReboot)
            systemReset(false); // noreturn

        while (serialTotalBytesWaiting(currentPortState->port)) {
            c = serialRead(currentPortState->port);

            if (currentPortState->c_state == IDLE) {
                currentPortState->c_state = (c == '$') ? HEADER_START : IDLE;
                if (currentPortState->c_state == IDLE && !f.ARMED)
                    evaluateOtherData(c); // if not armed evaluate all other incoming serial data
            } else if (currentPortState->c_state == HEADER_START) {
                currentPortState->c_state = (c == 'M') ? HEADER_M : IDLE;
            } else if (currentPortState->c_state == HEADER_M) {
                currentPortState->c_state = (c == '<') ? HEADER_ARROW : IDLE;
            } else if (currentPortState->c_state == HEADER_ARROW) {
                if (c > INBUF_SIZE) {       // now we are expecting the payload size
                    currentPortState->c_state = IDLE;
                    continue;
                }
                currentPortState->dataSize = c;
                currentPortState->offset = 0;
                currentPortState->checksum = 0;
                currentPortState->indRX = 0;
                currentPortState->checksum ^= c;
                currentPortState->c_state = HEADER_SIZE;      // the command is to follow
            } else if (currentPortState->c_state == HEADER_SIZE) {
                currentPortState->cmdMSP = c;
                currentPortState->checksum ^= c;
                currentPortState->c_state = HEADER_CMD;
            } else if (currentPortState->c_state == HEADER_CMD && currentPortState->offset < currentPortState->dataSize) {
                currentPortState->checksum ^= c;
                currentPortState->inBuf[currentPortState->offset++] = c;
            } else if (currentPortState->c_state == HEADER_CMD && currentPortState->offset >= currentPortState->dataSize) {
                if (currentPortState->checksum == c) {        // compare calculated and transferred checksum
                    evaluateCommand();      // we got a valid packet, evaluate it
                }
                currentPortState->c_state = IDLE;
            }
        }
    }
}

//Get power of a specific channel
unsigned short Parallax::getPower(unsigned char channel) {
	if(channel > PARALLAX_NUM_MOTORS) {
		Log::logf(Log::WARN, "Invalid PWM channel!\n");
		return 0;
	}

	unsigned char command[8] = {'!', 'S', 'C', 'R', 'S', 'P', channel, '\r'};
	//3 preamble bytes, 3 command bytes, channel byte, command terminator
	if(serialWrite(&command, sizeof(command)) != sizeof(command)) {
		Log::logf(Log::ERR, "Error writing to Parallax servo controller!\n");
	}
	serialRead(&command, sizeof(command));	//Remove remote echo from input buffer, ignore any errors

	unsigned char data[3] = {0};
	if(serialRead(&data, sizeof(data)) == sizeof(data)) {
		return (data[1] << 8) | data[2];
	} else {
		return 0;
	}
}

	//!Wait the response of the module
	void WaspRFID::waitResponse(void)
	{	
		int val = 0xFF;
		long cont = 0;
		
		while((val != PREAMBLE) && (cont < timeOut)) {
			val = serialRead(_uart);
			delay(5);
			cont ++;
		}
	}

int8_t gpsSetPassthrough(void)
{
    if (gpsData.state != GPS_RECEIVINGDATA)
        return -1;

    LED0_OFF;
    LED1_OFF;

    while(1) {
        if (serialTotalBytesWaiting(core.gpsport)) {
            LED0_ON;
            serialWrite(core.mainport, serialRead(core.gpsport));
            LED0_OFF;
        }
        if (serialTotalBytesWaiting(core.mainport)) {
            LED1_ON;
            serialWrite(core.gpsport, serialRead(core.mainport));
            LED1_OFF;
        }
    }
}

void initlbuf(void) {
	lbufptr = lbuf;

#if defined(SERIAL_OVERRIDE) && 0
	// don't do the prompt in serialIsOverridden mode
	if (serialIsOverridden()) return;
#endif

	prompt();
	
	// flush any pending serial input
	while (serialAvailable()) serialRead();
}

/*
 A high-level serial passthrough implementation. Used by cli to start an
 arbitrary serial passthrough "proxy". Optional callbacks can be given to allow
 for specialized data processing.
 */
void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer
                       *leftC, serialConsumer *rightC)
{
    waitForSerialPortToFinishTransmitting(left);
    waitForSerialPortToFinishTransmitting(right);

    if (!leftC)
        leftC = &nopConsumer;
    if (!rightC)
        rightC = &nopConsumer;

    LED0_OFF;
    LED1_OFF;

    // Either port might be open in a mode other than MODE_RXTX. We rely on
    // serialRxBytesWaiting() to do the right thing for a TX only port. No
    // special handling is necessary OR performed.
    while(1) {
        // TODO: maintain a timestamp of last data received. Use this to
        // implement a guard interval and check for `+++` as an escape sequence
        // to return to CLI command mode.
        // https://en.wikipedia.org/wiki/Escape_sequence#Modem_control
        if (serialRxBytesWaiting(left)) {
            LED0_ON;
            uint8_t c = serialRead(left);
            serialWrite(right, c);
            leftC(c);
            LED0_OFF;
         }
         if (serialRxBytesWaiting(right)) {
             LED0_ON;
             uint8_t c = serialRead(right);
             serialWrite(left, c);
             rightC(c);
             LED0_OFF;
         }
     }
 }

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

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

    LED0_OFF;
    LED1_OFF;

#ifdef DISPLAY
    if (feature(FEATURE_DISPLAY)) {
        displayShowFixedPage(PAGE_GPS);
    }
#endif
    char c;
    while(1) {
        if (serialRxBytesWaiting(gpsPort)) {
            LED0_ON;
            c = serialRead(gpsPort);
            gpsNewData(c);
            serialWrite(gpsPassthroughPort, c);
            LED0_OFF;
        }
        if (serialRxBytesWaiting(gpsPassthroughPort)) {
            LED1_ON;
            c = serialRead(gpsPassthroughPort);
            serialWrite(gpsPort, c);
            LED1_OFF;
        }
#ifdef DISPLAY
        if (feature(FEATURE_DISPLAY)) {
            updateDisplay();
        }
#endif
    }
}

/* getCellInfo() - gets the information from the cell where the module is connected
 *
 * This function gets the information from the cell where the module is connected
 *
 * It stores in 'RSSI' and 'cellID' variables the information from the cell
 *
 * It modifies 'flag' if expected answer is not received after sending a command to GPRS module
 *
 * Returns '1' on success and '0' if error
*/
uint8_t WaspGPRS::getCellInfo()
{
	char command[30];
	uint8_t byteIN[200];
	long previous=millis();
	uint8_t counter=0;
	uint8_t a,b,c=0;
	
	serialFlush(PORT_USED);
	sprintf(command,"AT%s\r\n",AT_GPRS_CELLID);
	printString(command,PORT_USED);
	while( (!serialAvailable(PORT_USED)) && ((millis()-previous)<3000) );
	previous=millis();
	a=0;
	while( (millis()-previous) < 2000 )
	{
		while( serialAvailable(PORT_USED) && (millis()-previous) < 2000 && (a<200) )
		{
			byteIN[a]=serialRead(PORT_USED);
			a++;
		}
	}
	a=0;
	while( counter < 5 )
	{
		while( (byteIN[a]!=',') && (a<200) )
		{
			a++;
		}
		a++;
		counter++;
	}
	if(a>=200) return 0;
	counter=0;
	while( (byteIN[a]!=',') && (a<200) )
	{
		cellID[c]=byteIN[a];
		a++;
		c++;
	}
	a++;
	while( (byteIN[a]!=',') && (a<200) )
	{
		RSSI[b]=byteIN[a];
		delay(10);
		b++;
		a++;
	}
	return 1;
}

uint16_t Arduino::command(uint8_t nid, uint8_t cmd, uint8_t pin, uint8_t val) {
  // Send.
//  printf("Sending command\n");
  serialWrite(MESSAGE_HEADER);
  serialWrite(nid);
  serialWrite(cmd);
  serialWrite(pin);
  serialWrite(val);

  usleep(100); // let it time to reply

  // Receive.
  char rd = serialRead();
  if (rd != MESSAGE_HEADER) {
    printf("Error: wrong message header received from node %d: '%c'\n", nid, rd);
    exit(-1);
  }
  uint8_t h = serialRead();
  uint8_t l = serialRead();

  // Return received value.
  return (uint16_t) ( (h << 8) | l );
}

static void hottCheckSerialData(uint32_t currentMicros)
{
    static bool lookingForRequest = true;

    uint8_t bytesWaiting = serialTotalBytesWaiting(hottPort);

    if (bytesWaiting <= 1) {
        return;
    }

    if (bytesWaiting != 2) {
        flushHottRxBuffer();
        lookingForRequest = true;
        return;
    }

    if (lookingForRequest) {
        lastHoTTRequestCheckAt = currentMicros;
        lookingForRequest = false;
        return;
    } else {
        bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= HOTT_RX_SCHEDULE;

        if (!enoughTimePassed) {
            return;
        }
        lookingForRequest = true;
    }

    uint8_t requestId = serialRead(hottPort);
    uint8_t address = serialRead(hottPort);

    if ((requestId == 0) || (requestId == HOTT_BINARY_MODE_REQUEST_ID) || (address == HOTT_TELEMETRY_NO_SENSOR_ID)) {
        processBinaryModeRequest(address);
    }
}

/**
 * Send a file over the serial port.
 * @param file file to send
 */
int Xmodem::send(File* file) {
  _file = file;
  _packetNumber = 1;
  beginLed();

  while (true) {
    int c = serialRead();
    if (c != -1) {
      resetTimer();
      break;
    }
    delay(100);
  }

  int status = 0;
  while (true) {
    size_t bytesRead = readPacket();
    resetTimer();
    if (bytesRead > 0) {
      status = sendPacket();
      if (_packetNumber % 20 == 0) {
        toggleLed();
      }
      if (status < 0) {
        break;
      }
    }
    if (bytesRead < PACKET_SIZE) {
      break;
    }
  }
  serialWrite(EOT);
  while (serialRead() == -1);
  endLed();
  return status;
}

void sp_process()
{
    char c;
    while((c = serialRead()) != -1)
    {
        if((char_counter > 0) && ((c == '\n') || (c == '\r'))) {  // Line is complete. Then execute!
            line[char_counter] = 0; // treminate string
            status_message(gc_execute_line(line));
            char_counter = 0; // reset line buffer index
        } else if (c <= ' ') { // Throw away whitepace and control characters
        } else if (c >= 'a' && c <= 'z') { // Upcase lowercase
            line[char_counter++] = c-'a'+'A';
        } else {
            line[char_counter++] = c;
        }
    }
}

static bool gpsReceiveData(void)
{
    bool hasNewData = false;

    if (gpsState.gpsPort) {
        while (serialRxBytesWaiting(gpsState.gpsPort)) {
            uint8_t newChar = serialRead(gpsState.gpsPort);
            if (gpsNewFrameNMEA(newChar)) {
                gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
                gpsSol.flags.validVelNE = 0;
                gpsSol.flags.validVelD = 0;
                hasNewData = true;
            }
        }
    }

    return hasNewData;
}

void gpsThread(void)
{
    // read out available GPS bytes
    if (gpsPort) {
        while (serialRxBytesWaiting(gpsPort))
            gpsNewData(serialRead(gpsPort));
    }

    switch (gpsData.state) {
        case GPS_UNKNOWN:
            break;

        case GPS_INITIALIZING:
        case GPS_CHANGE_BAUD:
        case GPS_CONFIGURE:
            gpsInitHardware();
            break;

        case GPS_LOST_COMMUNICATION:
            gpsData.timeouts++;
            if (gpsConfig()->autoBaud) {
                // try another rate
                gpsData.baudrateIndex++;
                gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
            }
            gpsData.lastMessage = millis();
            // TODO - move some / all of these into gpsData
            GPS_numSat = 0;
            DISABLE_STATE(GPS_FIX);
            gpsSetState(GPS_INITIALIZING);
            break;

        case GPS_RECEIVING_DATA:
            // check for no data/gps timeout/cable disconnection etc
            if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
                // remove GPS from capability
                sensorsClear(SENSOR_GPS);
                gpsSetState(GPS_LOST_COMMUNICATION);
            }
            break;
    }
}

//Get power of a specific channel
unsigned short Pololu::getPower(unsigned char channel) {
	if(channel > POLOLU_NUM_MOTORS) {
		Log::logf(Log::WARN, "Invalid PWM channel!\n");
		return 0;
	}

	unsigned char command[2] = {0x90, channel};
	//Command byte, channel byte
	if(serialWrite(&command, sizeof(command)) != sizeof(command)) {
		Log::logf(Log::ERR, "Error writing to Pololu Maestro servo controller!\n");
	}

	unsigned char data[2] = {0};
	if(serialRead(&data, sizeof(data)) == sizeof(data)) {
		return (data[0] << 8) | data[1];
	} else {
		Log::logf(Log::ERR, "Error reading from Pololu Maestro servo controller!\n");
		return 0;
	}
}

void loop(){
  seq.rotation.update(getAnalogValue(SEQUENCER_ROTATE_CONTROL));
  seq.step.update(getAnalogValue(SEQUENCER_STEP_CONTROL));
  seq.fill.update(getAnalogValue(SEQUENCER_FILL_CONTROL));
  seq.update();

#ifdef SERIAL_DEBUG
  if(serialAvailable() > 0){
    serialRead();
    printString("a: [");
    seq.dump();
    printString("] ");
    seq.print();
    if(clockIsHigh())
      printString(" clock high");
    if(resetIsHigh())
      printString(" reset high");
    printNewline();
  }
#endif
}

void handleIbusTelemetry(void)
{
    if (!ibusTelemetryEnabled) {
        return;
    }

    while (serialRxBytesWaiting(ibusSerialPort) > 0) {
        uint8_t c = serialRead(ibusSerialPort);

        if (outboundBytesToIgnoreOnRxCount) {
            outboundBytesToIgnoreOnRxCount--;
            continue;
        }

        pushOntoTail(ibusReceiveBuffer, IBUS_RX_BUF_LEN, c);

        if (isChecksumOkIa6b(ibusReceiveBuffer, IBUS_RX_BUF_LEN)) {
            outboundBytesToIgnoreOnRxCount += respondToIbusRequest(ibusReceiveBuffer);
        }
    }
}

int stm32Sync()
{
	int ret = STM_ERR;
	static unsigned char STM32_INIT = 0x7F;
	unsigned char r;
	int rr;
	int i;

	printf("Sync");
	fflush(stdout);
	for ( i=0; i<MAXTRY; i++ ) {
		printf(".");
		fflush(stdout);
		rr = serialWrite(board.serial_fd, &STM32_INIT, 1);
		if ( rr == SER_OK ) {
			serialFlush(board.serial_fd);
			rr = serialRead(board.serial_fd, &r, sizeof(r));
			if ( rr == SER_OK ) {
				if ( r==STM32_ACK || r==STM32_NACK) {
					ret = STM_OK;
					break;
				}
			}
		}
	}
	printf("\n");

	if ( ret == STM_OK ) {
		printf("Connected to board.\n");
	} else {
		printf("Can not connect to board, ");
		if ( rr == SER_ERR ) {
			printf(" failed.\n");
		} else {
			printf(" timed-out.\n");
		}
	}

	return ret;
}

void gpsThread(void)
{
    // read out available GPS bytes
    if (core.gpsport) {
        while (serialTotalBytesWaiting(core.gpsport))
            gpsNewData(serialRead(core.gpsport));
    }

    switch (gpsData.state) {
        case GPS_UNKNOWN:
            break;

        case GPS_INITIALIZING:
        case GPS_INITDONE:
            gpsInitHardware();
            break;

        case GPS_LOSTCOMMS:
            gpsData.errors++;
            // try another rate
            gpsData.baudrateIndex++;
            gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
            gpsData.lastMessage = millis();
            // TODO - move some / all of these into gpsData
            GPS_numSat = 0;
            f.GPS_FIX = 0;
            gpsSetState(GPS_INITIALIZING);
            break;

        case GPS_RECEIVINGDATA:
            // check for no data/gps timeout/cable disconnection etc
            if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
                // remove GPS from capability
                sensorsClear(SENSOR_GPS);
                gpsSetState(GPS_LOSTCOMMS);
            }
            break;
    }
}