void PantographDevice::MoveToGoal(float x_goal_new, float y_goal_new) {
SetGoal(x_goal_new, y_goal_new);
while (!flag_goal_reached) {
ExecuteControl();
delay(loop_dt);
}
}
void
NNFeedOverview(Connection *conn)
{
int artLen;
int l = 0;
char *art = MBNormalize(&conn->co_ArtBuf, &artLen);
char *line;
int appr = 0;
char ch;
int err = 0;
char *newsgroups = NULL;
char *msgid = NULL;
char *subject = NULL;
char *date = NULL;
char *xref = NULL;
char *control = NULL;
char *supers = NULL; /* Supersedes */
/*
* Scan headers, look for Newsgroups: line, Subject:, Date:, From:, and
* Message-Id:. If any are missing, the article is bad. If there is an
* Xref: line, save that too and use it to calculate line numbers if
* Xref operation is enabled.
*
* We allow an LF-only line to terminate the headers as well as CR+LF,
* because some news systems are totally broken.
*/
for (line = art; line < art + artLen; line += l + 1) {
for (l = line - art; l < artLen; ++l) {
if (art[l] == '\n') {
if (l + 1 >= artLen || /* past end of article */
l == line - art || /* blank line */
(art[l+1] != ' ' && art[l+1] != '\t') /* !header ext */
) {
break;
}
}
}
l -= line - art;
ch = tolower(*line);
if (l == 0 || (l == 1 && line[0] == '\r')) {
/* out of headers */
break;
} else if (ch == 'n' && strncasecmp(line, "Newsgroups:", 11) == 0) {
newsgroups = zallocStrTrim2(&conn->co_MemPool, ',', line + 11, l - 11);
} else if (ch == 'm' && strncasecmp(line, "Message-ID:", 11) == 0) {
msgid = zallocStrTrim2(&conn->co_MemPool, 0, line + 11, l - 11);
} else if (ch == 's' && strncasecmp(line, "Subject:", 8) == 0) {
subject = zallocStrTrim2(&conn->co_MemPool, 0, line + 8, l - 8);
} else if (ch == 'd' && strncasecmp(line, "Date:", 5) == 0) {
date = zallocStrTrim2(&conn->co_MemPool, 0, line + 5, l - 5);
} else if (ch == 'x' && strncasecmp(line, "Xref:", 5) == 0) {
xref = zallocStrTrim2(&conn->co_MemPool, ',', line + 5, l - 5);
} else if (ch == 'c' && strncasecmp(line, "Control:", 8) == 0) {
control = zallocStrTrim2(&conn->co_MemPool, 0, line + 8, l - 8);
} else if (ch == 's' && strncasecmp(line, "Supersedes:", 11) == 0) {
supers = zallocStrTrim2(&conn->co_MemPool, 0, line + 11, l - 11);
} else if (ch == 'a' && strncasecmp(line, "Approved:", 9) == 0) {
appr = 1;
}
}
if (conn->co_Flags & COF_POSTTOOBIG) {
conn->co_Auth.dr_PostFailCount++;
if (conn->co_Flags & COF_IHAVE) {
MBLogPrintf(conn, &conn->co_TMBuf, "437 Rejected, too big\r\n");
} else {
MBLogPrintf(conn, &conn->co_TMBuf, "439 %s too big\r\n", conn->co_IHaveMsgId);
}
conn->co_Flags &= ~COF_POSTTOOBIG;
} else if (newsgroups == NULL || msgid == NULL || subject == NULL ||
date == NULL || strcmp(msgid, "<>") == 0
) {
/*
* failure
*/
conn->co_Auth.dr_PostFailCount++;
if (conn->co_Flags & COF_IHAVE) {
MBLogPrintf(conn, &conn->co_TMBuf, "437 Rejected, headers missing\r\n");
} else {
MBLogPrintf(conn, &conn->co_TMBuf, "439 %s\r\n", conn->co_IHaveMsgId);
}
} else if (conn->co_ByteCounter == 0.0 && conn->co_BytesHeader == 0) {
conn->co_Auth.dr_PostFailCount++;
if (conn->co_Flags & COF_IHAVE) {
MBLogPrintf(conn, &conn->co_TMBuf, "437 Rejected, Bytes header missing for header-only feed\r\n");
} else {
MBLogPrintf(conn, &conn->co_TMBuf, "439 %s headerOnlyFeed requires Bytes header\r\n", conn->co_IHaveMsgId);
}
} else if (FindCancelCache(msgid) == 0) {
char logbuf[1024];
conn->co_Auth.dr_PostFailCount++;
snprintf(logbuf, sizeof(logbuf), "%s cancel cache", msgid);
LogCmd(conn, '-', logbuf);
if (DRIncomingLogPat != NULL)
LogIncoming("%s - %s%s", conn->co_Auth.dr_Host, "", logbuf);
if (conn->co_Flags & COF_IHAVE) {
MBLogPrintf(conn, &conn->co_TMBuf, "437 Article Already Cancelled\r\n");
} else {
MBLogPrintf(conn, &conn->co_TMBuf, "439 %s Article Already Cancelled\r\n", conn->co_IHaveMsgId);
}
} else {
/*
* write out overview information
*/
char *group;
char *ngroup = NULL;
ArtNumAss *ANABase = NULL;
/*
* if it is a control message, we don't really care what the newsgroups
* line says. instead, we cobble up "control.<type>" or just "control"
*/
if (conn->co_Flags & COF_WASCONTROL) {
char cmsgtype[64];
char *cptr;
cptr = control;
while (*cptr == ' ' || *cptr == '\t') {
cptr++;
}
snprintf(cmsgtype, sizeof(cmsgtype), "control%s%s", *cptr ? "." : "", cptr);
if (((cptr = strchr(cmsgtype, ' '))) || ((cptr = strchr(cmsgtype, '\t')))) {
*cptr = '\0';
}
zfreeStr(&conn->co_MemPool, &newsgroups);
newsgroups = zallocStr(&conn->co_MemPool, cmsgtype);
}
if (DebugOpt)
printf("Feed overview %s %s\n", msgid, newsgroups);
/*
* pass 1 - assign article numbers
*/
for (group = newsgroups; *group; group = ngroup) {
char c;
char whspc = 0;
char *whspptr;
/* Strip leading spaces */
while (*group == ' ' || *group == '\t') {
group++;
}
if (! *group) {
break;
}
for (ngroup = group; *ngroup && *ngroup != ','; ++ngroup)
;
c = *ngroup;
*ngroup = 0;
/*
* Skip groups with names that are too long
*/
if (ngroup - group > MAXGNAME)
continue;
/* Strip trailing space or tab from group name */
whspptr = strpbrk(group, " \t");
if (whspptr) {
whspc = *whspptr;
*whspptr = 0;
}
AssignArticleNo(conn, &ANABase, group, xref, appr, art, artLen, msgid);
/* Repair string back to its former state */
if (whspptr) {
*whspptr = whspc;
}
*ngroup = c;
if (*ngroup == ',')
++ngroup;
}
/*
* Supersedes is allowed on non-control messages. We execute the
* cancel AND post the article. Note: we do not allow supersedes
* on Control: messages. (XXX is this still true with the new logic? JG)
*/
if (supers) {
if (DebugOpt)
printf("has Supersedes: %s %s\n", msgid, newsgroups);
ExecuteSupersedes(conn, supers, art, artLen);
}
err = 0;
for (group = newsgroups; *group; group = ngroup) {
char c;
char whspc = 0;
char *whspptr;
for (ngroup = group; *ngroup && *ngroup != ','; ++ngroup)
;
c = *ngroup;
*ngroup = 0;
/*
* Skip groups with names that are too long
*/
if (ngroup - group > MAXGNAME)
continue;
/* Strip trailing space or tab from group name */
whspptr = strpbrk(group, " \t");
if (whspptr) {
whspc = *whspptr;
*whspptr = 0;
}
err += WriteOverview(conn, ANABase, group, xref, art, artLen, msgid);
/* Repair string back to its former state */
if (whspptr) {
*whspptr = whspc;
}
*ngroup = c;
if (*ngroup == ',')
++ngroup;
}
while (ANABase) {
ArtNumAss *an = ANABase;
ANABase = an->an_Next;
zfree(&conn->co_MemPool, an, sizeof(ArtNumAss));
}
if (conn->co_Flags & COF_WASCONTROL) {
if (DebugOpt)
printf("Control message: %s %s\n", msgid, newsgroups);
LogCmd(conn, 'c', control);
if (DRIncomingLogPat != NULL)
LogIncoming("%s c %s %s", conn->co_Auth.dr_Host,
msgid, control);
ExecuteControl(conn, control, art, artLen);
}
if (!err) {
conn->co_Auth.dr_PostCount++;
if (conn->co_Flags & COF_IHAVE) {
MBLogPrintf(conn, &conn->co_TMBuf, "235\r\n");
} else {
MBLogPrintf(conn, &conn->co_TMBuf, "239 %s\r\n", conn->co_IHaveMsgId);
}
}
}
zfreeStr(&conn->co_MemPool, &newsgroups);
zfreeStr(&conn->co_MemPool, &msgid);
zfreeStr(&conn->co_MemPool, &subject);
zfreeStr(&conn->co_MemPool, &date);
zfreeStr(&conn->co_MemPool, &xref);
zfreeStr(&conn->co_MemPool, &control);
zfreeStr(&conn->co_MemPool, &conn->co_IHaveMsgId);
MBFree(&conn->co_ArtBuf);
NNCommand(conn);
}
/****************************************************************************
Function
RunLobbyistSM
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
Deals with the high level state transitions of the Lobbyist state machine.
****************************************************************************/
ES_Event RunLobbyistSM( ES_Event ThisEvent )
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
switch ( CurrentState )
{
case InitPState : // If current state is initial Psedudo State
if ( ThisEvent.EventType == ES_INIT ) // only respond to ES_Init
{
//Prep the timer hardware.
printf("Init Steering \r\n");
InitSteering(); // Initialize the steering systems
printf("Init Packets \r\n");
InitPackets(); // Initialize packet array
printf("Init XBee \r\n");
InitXBee(); // Initialize the comms
printf("Init Lift Fan \r\n");
InitLiftFan(); // Initialize the lift fan
//Initialize the DMC.
CurrentState = WaitingForPair;
}
break;
case WaitingForPair :
switch ( ThisEvent.EventType )
{
//If the event is a pair request, change state to pairing and wait for the key.
case MESSAGE_RECEIVED:
// Read incoming message
message = GetMessageData();
// Check that message is a pair request and that it's meant for us
if(*message == PAIR_REQUEST_HEADER && CheckLobbyistNumber() && GetApiIdentifier() == INCOMING_PACKET_API_IDENTIFIER){ // Ensure that this is not a Tx success receive
printf("\r\n Current Message Size: %d ", GetMessageDataSize());
// If we passed the checks, we're now paired. Start timers.
printf("\n\r\nPair request received in unpaired state.");
ES_Timer_InitTimer(UNPAIR_TIMER, UNPAIR_TIME); //Start the 45s timer
ES_Timer_InitTimer(PAIR_FAIL_TIMER, ONE_SEC); //Start the 1s timer
TurnOnLiftFan(); //Start the lift fan
//Update DMC
// Recognize our new pac daddy
pairedAddress = GetSenderAddress() ;
// Send an ACK back to the sender
TransmitStatusPacket(PAIRED);
// Set the pair time
PairTimeLeft = 45;
CurrentState = PairedAwaitEncryptionKey;
}
break;
default :
;
}
break;
case PairedAwaitEncryptionKey:
switch(ThisEvent.EventType){
case MESSAGE_RECEIVED:
// Check sender and message
message = GetMessageData();
uint16_t sender = GetSenderAddress() ;
if(sender == pairedAddress && *message == KEY_SEND_HEADER){
printf("\n\r\nKey received in pairing state.");
//Save the key
SetKey((message+1)); // Make sure to +1 to start after header byte
ES_Timer_InitTimer(PAIR_FAIL_TIMER, ONE_SEC); //Restart the 1s timer
TransmitStatusPacket(PAIRED); //transmit status with the pairing bit set.
CurrentState = PairedAwaitControl;
printf("\r\n In paired await control");
}
break;
case ES_TIMEOUT: // if there is a timeout on the 1s timer
printf("\n\r\n1s timeout in pairing - moving to awaiting pair.");
TurnOffLiftFan(); //Turn off the lift fan
//Update DMC to avaiable for pairing
ES_Timer_StopTimer(UNPAIR_TIMER); //Disable the 45s pairing timer
ES_Timer_StopTimer(PAIR_FAIL_TIMER); //Disable the 1s transmit timeout timer
TransmitStatusPacket(UNPAIRED); //Transmit status with the pairing bit cleared.
CurrentState = WaitingForPair;
break;
}
break;
case PairedAwaitControl:
switch(ThisEvent.EventType){
case MESSAGE_RECEIVED:
// See who sent the message
uint16_t sender = GetSenderAddress() ;
if(sender == pairedAddress){ // Only attempt decrypt if from sender
message = DecryptMessage(GetMessageData(),GetMessageDataSize());
RotateCipher();
// printf("\r\n Rotating Key");
if(*message == CNTRL_REQUEST_HEADER){
//printf("\n\r\nCommand received in the paired state.");
ES_Timer_InitTimer(PAIR_FAIL_TIMER, ONE_SEC); //Restart the control command timer.
ExecuteControl(); //Execute the control command.
TransmitStatusPacket(PAIRED);// ACK YAY. Transmit the status with pairing bit set.
} else {
printf("\n\r Not a valid control packet received, Header: %d", *message);
}
}
break;
case ES_TIMEOUT: //Either the 1s or 45s timer
case MANUAL_UNPAIR: // Or if the pair is manual
case DECRYPT_FAIL: // Or if the decryption failed
if(ThisEvent.EventParam == UNPAIR_TIMER){
// Keep track of how long is left on UNPAIR_TIMER
PairTimeLeft--;
printf("\n\r\nTime until unpair: %ds", PairTimeLeft);
// If timer has run for 45 s
if(PairTimeLeft == 0){
CurrentState = WaitingForPair;
TurnOffLiftFan(); //Turn off the lift fan
UpdateSteering(OFF,OFF); //Turn off the propulsion fans
//Update DMC to avaiable for pairing
ES_Timer_StopTimer(UNPAIR_TIMER); //Disable the 45s pairing timer
ES_Timer_StopTimer(PAIR_FAIL_TIMER); //Disable the 1s transmit timeout timer
printf("\n\r\nUnpairing.");
// If timer is not at 0, reinitialize the timer again for 1s to keep counting
}else{
ES_Timer_InitTimer(UNPAIR_TIMER, UNPAIR_TIME); //Start the 45s timer
}
} else {
// If we have an event NOT on the 45s timer, unpair
printf("\n\r\nTimeout in the paired state. Moving to waiting for pair \r\n");
TurnOffLiftFan(); //Turn off the lift fan
UpdateSteering(OFF,OFF); //Turn off the propulsion fans
//Update DMC to avaiable for pairing
ES_Timer_StopTimer(UNPAIR_TIMER); //Disable the 45s pairing timer
ES_Timer_StopTimer(PAIR_FAIL_TIMER); //Disable the 1s transmit timeout timer
if(ThisEvent.EventType == DECRYPT_FAIL){ // If this event came from a decryption failure //// WEHERE DOES THIS COME FROM?
TransmitStatusPacket(DECRYPT_FAILURE); // Transmit status with the decryption fail bit set
} else {
TransmitStatusPacket(UNPAIRED); //Transmit status with the pairing bit cleared.
}
CurrentState = WaitingForPair;
}
break;
}
break;
default :
break;
}
return ReturnEvent;
}
