void serialCom(void)
{
uint8_t c;
static uint8_t offset;
static uint8_t dataSize;
static enum _serial_state {
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD,
} c_state = IDLE;
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
while (serialTotalBytesWaiting(core.mainport)) {
c = serialRead(core.mainport);
if (c_state == IDLE) {
c_state = (c == '$') ? HEADER_START : IDLE;
if (c_state == IDLE)
evaluateOtherData(c); // evaluate all other incoming serial data
} else if (c_state == HEADER_START) {
c_state = (c == 'M') ? HEADER_M : IDLE;
} else if (c_state == HEADER_M) {
c_state = (c == '<') ? HEADER_ARROW : IDLE;
} else if (c_state == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
c_state = IDLE;
continue;
}
dataSize = c;
offset = 0;
checksum = 0;
indRX = 0;
checksum ^= c;
c_state = HEADER_SIZE; // the command is to follow
} else if (c_state == HEADER_SIZE) {
cmdMSP = c;
checksum ^= c;
c_state = HEADER_CMD;
} else if (c_state == HEADER_CMD && offset < dataSize) {
checksum ^= c;
inBuf[offset++] = c;
} else if (c_state == HEADER_CMD && offset >= dataSize) {
if (checksum == c) { // compare calculated and transferred checksum
evaluateCommand(); // we got a valid packet, evaluate it
}
c_state = IDLE;
}
}
if (!cliMode && feature(FEATURE_TELEMETRY)) { // The first condition should never evaluate to true but I'm putting it here anyway - silpstream
sendTelemetry();
}
}
void serialCom(void)
{
uint8_t c;
static uint8_t offset;
static uint8_t dataSize;
static enum _serial_state {
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD,
} c_state = IDLE;
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
while (serialTotalBytesWaiting(core.mainport)) {
c = serialRead(core.mainport);
if (c_state == IDLE) {
c_state = (c == '$') ? HEADER_START : IDLE;
if (c_state == IDLE && !f.ARMED)
evaluateOtherData(c); // if not armed evaluate all other incoming serial data
} else if (c_state == HEADER_START) {
c_state = (c == 'M') ? HEADER_M : IDLE;
} else if (c_state == HEADER_M) {
c_state = (c == '<') ? HEADER_ARROW : IDLE;
} else if (c_state == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
c_state = IDLE;
continue;
}
dataSize = c;
offset = 0;
checksum = 0;
indRX = 0;
checksum ^= c;
c_state = HEADER_SIZE; // the command is to follow
} else if (c_state == HEADER_SIZE) {
cmdMSP = c;
checksum ^= c;
c_state = HEADER_CMD;
} else if (c_state == HEADER_CMD && offset < dataSize) {
checksum ^= c;
inBuf[offset++] = c;
} else if (c_state == HEADER_CMD && offset >= dataSize) {
if (checksum == c) { // compare calculated and transferred checksum
evaluateCommand(); // we got a valid packet, evaluate it
}
c_state = IDLE;
}
}
}
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;
}
}
}
}
bool mwii_receive(char c)
{
static uint8_t offset, dataSize, c_state = 0;
switch(c_state)
{
case 0:
if (c == '$') c_state++;
else c_state = 0;
break;
case 1:
if (c == 'M') c_state++;
else c_state = 0;
break;
case 2:
if (c == '<') c_state++;
else c_state = 0;
break;
case 3:
if (c > INBUF_SIZE) c_state = 0;
else c_state++;
dataSize = c;
offset = checksum = indRX = 0;
checksum ^= c;
break;
case 4:
cmdMSP = c;
checksum ^= c;
c_state++;
break;
case 5:
if (offset < dataSize)
{
checksum ^= c;
inBuf[offset++] = c;
}
else
{
c_state = 0;
if (checksum == c) // compare calculated and transferred checksum
{
evaluateCommand(); // we got a valid packet, evaluate it
return true;
}
}
break;
}
return false;
}
int main (int argc, char** argv, char** envp){
/* Record the directory where the executable files are. */
init_dir = (char *)malloc(sizeof(char)*MAX_LINE);
getcwd(init_dir,sizeof(char)*MAX_LINE);
init_dir[strlen(init_dir)] = '\0';
/* Prompt for input until ctrl-d or exit have been entered */
char *line = (char *)malloc(sizeof(char)*MAX_LINE);
fprintf(stdout,"$>");
while(fgets(line,MAX_LINE,stdin)){
line[strlen(line)-1] = '\0'; // Nip the buf
int num_tokens = countTokens(line);
char **tokens = (char **) malloc(sizeof(char *) * num_tokens);
tokens = gettokens(line);
/* exit was entered into the terminal so free heap space and quit. */
if (*tokens != NULL && strcmp("exit",tokens[0]) == 0){
freeStringArray(tokens);
free(line);
free(init_dir);
exit(0);
}
/* Nothing entered in stdin. Continue prompting */
if(*tokens == NULL){
fprintf(stdout,"$> ");
continue;
}
if (DEBUG)
fprintf(stderr,"about to eval.\n");
evaluateCommand(tokens);
/* free memory */
line = (char *)realloc(line,sizeof(char)*MAX_LINE);
freeStringArray(tokens);
fprintf(stdout,"$> ");
}
return 0;
}
void serialCom() {
uint8_t c,n;
static uint8_t offset[UART_NUMBER];
static uint8_t dataSize[UART_NUMBER];
static enum _serial_state {
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD,
} c_state[UART_NUMBER];// = IDLE;
for(n=0;n<UART_NUMBER;n++) {
#if !defined(PROMINI)
CURRENTPORT=n;
#endif
#define GPS_COND
#if defined(GPS_SERIAL)
#if defined(GPS_PROMINI)
#define GPS_COND
#else
#undef GPS_COND
#define GPS_COND && (GPS_SERIAL != CURRENTPORT)
#endif
#endif
#define SPEK_COND
#if defined(SPEKTRUM) && (UART_NUMBER > 1)
#define SPEK_COND && (SPEK_SERIAL_PORT != CURRENTPORT)
#endif
#define SBUS_COND
#if defined(SBUS) && (UART_NUMBER > 1)
#define SBUS_COND && (SBUS_SERIAL_PORT != CURRENTPORT)
#endif
uint8_t cc = SerialAvailable(CURRENTPORT);
while (cc-- GPS_COND SPEK_COND SBUS_COND) {
uint8_t bytesTXBuff = SerialUsedTXBuff(CURRENTPORT); // indicates the number of occupied bytes in TX buffer
if (bytesTXBuff > TX_BUFFER_SIZE - 50 ) return; // ensure there is enough free TX buffer to go further (50 bytes margin)
c = SerialRead(CURRENTPORT);
#ifdef SUPPRESS_ALL_SERIAL_MSP
// no MSP handling, so go directly
evaluateOtherData(c);
#else
// regular data handling to detect and handle MSP and other data
if (c_state[CURRENTPORT] == IDLE) {
c_state[CURRENTPORT] = (c=='$') ? HEADER_START : IDLE;
if (c_state[CURRENTPORT] == IDLE) evaluateOtherData(c); // evaluate all other incoming serial data
} else if (c_state[CURRENTPORT] == HEADER_START) {
c_state[CURRENTPORT] = (c=='M') ? HEADER_M : IDLE;
} else if (c_state[CURRENTPORT] == HEADER_M) {
c_state[CURRENTPORT] = (c=='<') ? HEADER_ARROW : IDLE;
} else if (c_state[CURRENTPORT] == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
c_state[CURRENTPORT] = IDLE;
continue;
}
dataSize[CURRENTPORT] = c;
offset[CURRENTPORT] = 0;
checksum[CURRENTPORT] = 0;
indRX[CURRENTPORT] = 0;
checksum[CURRENTPORT] ^= c;
c_state[CURRENTPORT] = HEADER_SIZE; // the command is to follow
} else if (c_state[CURRENTPORT] == HEADER_SIZE) {
cmdMSP[CURRENTPORT] = c;
checksum[CURRENTPORT] ^= c;
c_state[CURRENTPORT] = HEADER_CMD;
} else if (c_state[CURRENTPORT] == HEADER_CMD && offset[CURRENTPORT] < dataSize[CURRENTPORT]) {
checksum[CURRENTPORT] ^= c;
inBuf[offset[CURRENTPORT]++][CURRENTPORT] = c;
} else if (c_state[CURRENTPORT] == HEADER_CMD && offset[CURRENTPORT] >= dataSize[CURRENTPORT]) {
if (checksum[CURRENTPORT] == c) { // compare calculated and transferred checksum
evaluateCommand(); // we got a valid packet, evaluate it
}
c_state[CURRENTPORT] = IDLE;
cc = 0; // no more than one MSP per port and per cycle
}
#endif // SUPPRESS_ALL_SERIAL_MSP
}
}
}
void SerialCom(void){
/* code about processing cmd from GUI */
evaluateCommand();
}
void serialCom() {
uint8_t c,cc,port,state,bytesTXBuff;
static uint8_t offset[UART_NUMBER];
static uint8_t dataSize[UART_NUMBER];
static uint8_t c_state[UART_NUMBER];
uint32_t timeMax; // limit max time in this function in case of GPS
timeMax = micros();
for(port=0;port<UART_NUMBER;port++) {
CURRENTPORT=port;
#define RX_COND
#if defined(SERIAL_RX) && (UART_NUMBER > 1)
#define RX_COND && (RX_SERIAL_PORT != port)
#endif
cc = SerialAvailable(port);
while (cc-- RX_COND) {
bytesTXBuff = SerialUsedTXBuff(port); // indicates the number of occupied bytes in TX buffer
if (bytesTXBuff > TX_BUFFER_SIZE - 50 ) return; // ensure there is enough free TX buffer to go further (50 bytes margin)
c = SerialRead(port);
#ifdef SUPPRESS_ALL_SERIAL_MSP
evaluateOtherData(c); // no MSP handling, so go directly
#else //SUPPRESS_ALL_SERIAL_MSP
state = c_state[port];
// regular data handling to detect and handle MSP and other data
if (state == IDLE) {
if (c=='$') state = HEADER_START;
else evaluateOtherData(c); // evaluate all other incoming serial data
}
else if (state == HEADER_START) {
state = (c=='M') ? HEADER_M : IDLE;
}
else if (state == HEADER_M) {
state = (c=='<') ? HEADER_ARROW : IDLE;
}
else if (state == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
state = IDLE;
continue;
}
dataSize[port] = c;
checksum[port] = c;
offset[port] = 0;
indRX[port] = 0;
state = HEADER_SIZE; // the command is to follow
}
else if (state == HEADER_SIZE) {
cmdMSP[port] = c;
checksum[port] ^= c;
state = HEADER_CMD;
}
else if (state == HEADER_CMD) {
if (offset[port] < dataSize[port]) {
checksum[port] ^= c;
inBuf[offset[port]++][port] = c;
}
else {
if (checksum[port] == c) // compare calculated and transferred checksum
evaluateCommand(cmdMSP[port]); // we got a valid packet, evaluate it
state = IDLE;
cc = 0; // no more than one MSP per port and per cycle
}
}
c_state[port] = state;
// SERIAL: try to detect a new nav frame based on the current received buffer
#if defined(GPS_SERIAL)
if (GPS_SERIAL == port) {
static uint32_t GPS_last_frame_seen; //Last gps frame seen at this time, used to detect stalled gps communication
if (GPS_newFrame(c)) {
//We had a valid GPS data frame, so signal task scheduler to switch to compute
if (GPS_update == 1) GPS_update = 0;
else GPS_update = 1; //Blink GPS update
GPS_last_frame_seen = timeMax;
GPS_Frame = 1;
}
// Check for stalled GPS, if no frames seen for 1.2sec then consider it LOST
if ((timeMax - GPS_last_frame_seen) > 1200000) {
//No update since 1200ms clear fix...
f.GPS_FIX = 0;
GPS_numSat = 0;
}
}
if (micros()-timeMax>250) return; // Limit the maximum execution time of serial decoding to avoid time spike
#endif
#endif // SUPPRESS_ALL_SERIAL_MSP
} // while
} // for
}
void serialCom() {
uint8_t c,cc,port,state,bytesTXBuff;
static uint8_t offset[UART_NUMBER];
static uint8_t dataSize[UART_NUMBER];
static uint8_t c_state[UART_NUMBER];
for(port=0;port<UART_NUMBER;port++) {
CURRENTPORT=port;
#define GPS_COND
#if defined(GPS_SERIAL) && (UART_NUMBER > 1)
#define GPS_COND && (GPS_SERIAL != port)
#endif
#define RX_COND
#if (defined(SPEKTRUM) || defined(SBUS) || defined(SUMD)) && (UART_NUMBER > 1)
#define RX_COND && (RX_SERIAL_PORT != port)
#endif
cc = SerialAvailable(port);
while (cc-- GPS_COND RX_COND) {
bytesTXBuff = SerialUsedTXBuff(port); // indicates the number of occupied bytes in TX buffer
if (bytesTXBuff > TX_BUFFER_SIZE - 50 ) return; // ensure there is enough free TX buffer to go further (50 bytes margin)
c = SerialRead(port);
#ifdef SUPPRESS_ALL_SERIAL_MSP
// no MSP handling, so go directly
evaluateOtherData(c);
#else
state = c_state[port];
// regular data handling to detect and handle MSP and other data
if (state == IDLE) {
if (c=='$') state = HEADER_START;
else evaluateOtherData(c); // evaluate all other incoming serial data
} else if (state == HEADER_START) {
state = (c=='M') ? HEADER_M : IDLE;
} else if (state == HEADER_M) {
state = (c=='<') ? HEADER_ARROW : IDLE;
} else if (state == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
state = IDLE;
continue;
}
dataSize[port] = c;
offset[port] = 0;
checksum[port] = 0;
indRX[port] = 0;
checksum[port] ^= c;
state = HEADER_SIZE; // the command is to follow
} else if (state == HEADER_SIZE) {
cmdMSP[port] = c;
checksum[port] ^= c;
state = HEADER_CMD;
} else if (state == HEADER_CMD) {
if (offset[port] < dataSize[port]) {
checksum[port] ^= c;
inBuf[offset[port]++][port] = c;
} else {
if (checksum[port] == c) // compare calculated and transferred checksum
evaluateCommand(); // we got a valid packet, evaluate it
state = IDLE;
cc = 0; // no more than one MSP per port and per cycle
}
}
c_state[port] = state;
#endif // SUPPRESS_ALL_SERIAL_MSP
}
}
}
void serialCom(void)
{
uint8_t c;
static uint8_t offset;
static uint8_t dataSize;
static enum _serial_state
{
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD,
} c_state = IDLE;
// in cli mode, all uart stuff goes to here. enter cli mode by sending #
if (cliMode)
{
cliProcess();
return;
}
while (uartAvailable())
{
c = uartRead();
if (c_state == IDLE)
{
c_state = (c == '$') ? HEADER_START : IDLE;
if (c_state == IDLE && !f.ARMED) // Idea CGiesen, only allow cli and reset when disarmed
evaluateOtherData(c); // evaluate all other incoming serial data
}
else if (c_state == HEADER_START)
{
c_state = (c == 'M') ? HEADER_M : IDLE;
}
else if (c_state == HEADER_M)
{
c_state = (c == '<') ? HEADER_ARROW : IDLE;
}
else if (c_state == HEADER_ARROW)
{
if (c > INBUF_SIZE) // now we are expecting the payload size
{
c_state = IDLE;
continue;
}
dataSize = c;
offset = 0;
checksum = 0;
indRX = 0;
checksum ^= c;
c_state = HEADER_SIZE; // the command is to follow
guiConnected = true;
}
else if (c_state == HEADER_SIZE)
{
cmdMSP = c;
checksum ^= c;
c_state = HEADER_CMD;
}
else if (c_state == HEADER_CMD && offset < dataSize)
{
checksum ^= c;
inBuf[offset++] = c;
}
else if (c_state == HEADER_CMD && offset >= dataSize)
{
if (checksum == c) // compare calculated and transferred checksum
{
evaluateCommand(); // we got a valid packet, evaluate it
}
c_state = IDLE;
}
}
if (!cliMode && !uartAvailable() && feature(FEATURE_TELEMETRY) && f.ARMED) // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
{
sendTelemetry();
return;
}
}
bool mwii_receive(char c)
{
bool giveback = false;
static uint8_t offset;
static uint8_t dataSize;
static enum _serial_state
{
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD,
} c_state = IDLE;
if (c_state == IDLE)
{
c_state = (c == '$') ? HEADER_START : IDLE;
}
else
if (c_state == HEADER_START)
{
c_state = (c == 'M') ? HEADER_M : IDLE;
}
else
if (c_state == HEADER_M)
{
c_state = (c == '<') ? HEADER_ARROW : IDLE;
}
else
if (c_state == HEADER_ARROW)
{
if (c > INBUF_SIZE) // now we are expecting the payload size
{
c_state = IDLE;
return false;
}
dataSize = c;
offset = checksum = indRX = 0;
checksum ^= c;
c_state = HEADER_SIZE; // the command is to follow // guiConnected = true;
}
else
if (c_state == HEADER_SIZE)
{
cmdMSP = c;
checksum ^= c;
c_state = HEADER_CMD;
}
else
if (c_state == HEADER_CMD && offset < dataSize)
{
checksum ^= c;
inBuf[offset++] = c;
}
else
if (c_state == HEADER_CMD && offset >= dataSize)
{
c_state = IDLE;
if (checksum == c) // compare calculated and transferred checksum
{
evaluateCommand(); // we got a valid packet, evaluate it
giveback = true;
}
}
return giveback;
}
void evaluateInstance(utree const &ast) {
for ( utree::const_iterator it = ast.begin(), ie = ast.end();
it != ie; ++it ) {
evaluateCommand(*it);
}
}
