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();
}
}
// evaluate all other incoming serial data
static void evaluateOtherData(uint8_t sr)
{
if (sr == '#')
cliProcess();
else if (sr == mcfg.reboot_character)
systemReset(true); // reboot to bootloader
}
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 handleSerial(void)
{
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
mspProcess();
}
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;
}
}
}
}
// evaluate all other incoming serial data
static void evaluateOtherData(uint8_t sr)
{
switch (sr) {
case '#':
cliProcess();
break;
case 'R':
systemReset(true); // reboot to bootloader
break;
}
}
void taskHandleSerial(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
#ifdef USE_CLI
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
#endif
mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
}
void taskHandleSerial(void)
{
#ifdef USE_CLI
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
#endif
mspSerialProcess();
}
static void taskHandleSerial(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
#ifdef USE_CLI
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
#endif
#ifndef OSD_SLAVE
bool evaluateMspData = ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;
#else
bool evaluateMspData = osdSlaveIsLocked ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;;
#endif
mspSerialProcess(evaluateMspData, mspFcProcessCommand, mspFcProcessReply);
}
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;
}
}
void serialCom(void)
{
uint8_t c;
bool HaveSomePacket = false;
static uint8_t NumberSignCNT = 0, RcharCNT = 0, RetCNT = 0;
// START Common Stuff for serial protocols
if (cfg.rssicut && rssi <= cfg.rssicut) rssi = 0;
// END Common Stuff for serial protocols
if (cliMode) // in cli mode, all uart stuff goes to here. enter cli mode by sending #####
{
cliProcess();
return;
}
if (f.ARMED) // Change Protocol according to armed state
{
switch(cfg.tele_prot) // 0=Keep Multiwii @CurrentUSB Baud, 1=Frsky @9600Baud, 2=Mavlink @CurrentUSB Baud, 3=Mavlink @57KBaud (like stock minimOSD wants it)
{
case 0:
Currentprotocol = PROTOCOL_MWII21;
break;
case 1: // Do Frsky here
sendFRSKYTelemetry();
break;
case 2:
case 3:
Currentprotocol = PROTOCOL_MAVLINK;
break;
}
}
else // Not armed engage autosensing here
{
if (!BlockProtocolChange && ((currentTimeMS - LastValidProtocolTimestampMS) > 4000))
Currentprotocol = PROTOCOL_AUTOSENSE; // Set Protocol to unknown after 4 Sek of garbage or no inputdata
}
switch(Currentprotocol)
{
case PROTOCOL_MAVLINK:
baseflight_mavlink_send_updates(); // Sends Heartbeat as well
break;
case PROTOCOL_AUTOSENSE:
baseflight_mavlink_send_1Hzheartbeat(); // It will time itself so no doubleheartbeat from sendupdates are here
default: // Mwii not specially done here because it just sends when asked to (see below)
break;
}
while (uartAvailable())
{
c = uartRead();
if (Currentprotocol == PROTOCOL_AUTOSENSE) // Check for extra Data when no protocol active (that is only in disarmed state possible)
{
if (c == '#')
{
NumberSignCNT++;
if (NumberSignCNT > 2) cliProcess(); // This is a one way - reset - street
}else NumberSignCNT = 0;
if (c == 'R')
{
RcharCNT++;
if (RcharCNT > 2) systemReset(true); // This is a one way - reset - street
}else RcharCNT = 0;
}
if (!f.ARMED && c == '\r') // Check for 3 Times Return in a row
{
RetCNT++;
if (RetCNT > 2) cliProcess();
}else RetCNT = 0;
switch(Currentprotocol)
{
case PROTOCOL_AUTOSENSE: // Current is Unsure
if(baseflight_mavlink_receive(c))Currentprotocol = PROTOCOL_MAVLINK;
else if(mwii_receive(c)) Currentprotocol = PROTOCOL_MWII21;
if(Currentprotocol != PROTOCOL_AUTOSENSE) HaveSomePacket = true; // Some protocol found
break;
case PROTOCOL_MWII21: // Current is Mwii
HaveSomePacket = mwii_receive(c);
break;
case PROTOCOL_MAVLINK: // Current is Mavlink
HaveSomePacket = baseflight_mavlink_receive(c);
break;
}
if(HaveSomePacket) LastValidProtocolTimestampMS = currentTimeMS;
}
}
void serialCom(bool singlestep) // Singlestep means you can restrict to decoding just one dataset per run
{ // Reason: Reduce priority when other tasks need more.
uint8_t c;
bool HaveSomePacket = false, Skipnow = false;
static uint8_t NumberSignCNT = 0, RcharCNT = 0, RetCNT = 0, SingleStepCnt;
if (singlestep) SingleStepCnt = min(SingleStepCnt+1, 4);
else SingleStepCnt = 0;
if (SingleStepCnt > 3) singlestep = false; // Limit singlestep to max 3. in a row
// START Common Stuff for serial protocols
if (cfg.rssicut && rssi <= cfg.rssicut) rssi = 0;
// END Common Stuff for serial protocols
if (f.ARMED) // Change Protocol according to armed state
{
switch(cfg.tele_prot) // 0=Keep Multiwii @CurrentUSB Baud, 1=Frsky @9600Baud, 2=Mavlink @CurrentUSB Baud, 3=Mavlink @57KBaud (like stock minimOSD wants it)
{
case 0:
Currentprotocol = PROTOCOL_MWII21;
break;
case 1: // Do Frsky here
sendFRSKYTelemetry();
break;
case 2:
case 3:
Currentprotocol = PROTOCOL_MAVLINK;
break;
}
}
else // Not armed engage autosensing here
{
if (AllowProtocolAutosense && ((currentTimeMS - LastValidProtocolTimestampMS) > 4000))
Currentprotocol = PROTOCOL_AUTOSENSE; // Set Protocol to unknown after 4 Sek of garbage or no inputdata
}
switch(Currentprotocol)
{
case PROTOCOL_MAVLINK:
baseflight_mavlink_send_updates(); // Sends Heartbeat as well
break;
case PROTOCOL_AUTOSENSE:
baseflight_mavlink_send_1Hzheartbeat(); // It will time itself so no doubleheartbeat from sendupdates are here
default: // Mwii not specially done here because it just sends when asked to (see below)
break;
}
while (uartAvailable() && !Skipnow)
{
c = uartRead();
if (Currentprotocol == PROTOCOL_AUTOSENSE) // Check for extra Data when no protocol active (that is only in disarmed state possible)
{
if (c == '#')
{
NumberSignCNT++;
if (NumberSignCNT > 2) cliProcess(); // This is a one way - reset - street
}else NumberSignCNT = 0;
if (c == 'R')
{
RcharCNT++;
if (RcharCNT > 2) systemReset(true); // This is a one way - reset - street
}else RcharCNT = 0;
}
if (!f.ARMED && c == '\r') // Check for 3 Times Return in a row
{
RetCNT++;
if (RetCNT > 2) cliProcess();
}else RetCNT = 0;
switch(Currentprotocol)
{
case PROTOCOL_AUTOSENSE: // Current is Unsure
if(baseflight_mavlink_receive(c))Currentprotocol = PROTOCOL_MAVLINK;
else if(mwii_receive(c)) Currentprotocol = PROTOCOL_MWII21;
if(Currentprotocol != PROTOCOL_AUTOSENSE) HaveSomePacket = true; // Some protocol found
break;
case PROTOCOL_MWII21: // Current is Mwii
HaveSomePacket = mwii_receive(c);
break;
case PROTOCOL_MAVLINK: // Current is Mavlink
HaveSomePacket = baseflight_mavlink_receive(c);
break;
}
if(HaveSomePacket)
{
LastValidProtocolTimestampMS = currentTimeMS;
if (singlestep) Skipnow = true;
}
}
}