/**********************************************************************
* Function: doSetOriginSM
* @return None
* @remark Steps into the set origin state machine.
**********************************************************************/
static void doSetOriginSM() {
// Waiting for origin message from command center
if (event.flags.haveSetOriginMessage) {
GeocentricCoordinate ecefOrigin;
ecefOrigin.x = Mavlink_newMessage.gpsGeocentricData.x;
ecefOrigin.y = Mavlink_newMessage.gpsGeocentricData.y;
ecefOrigin.z = Mavlink_newMessage.gpsGeocentricData.z;
Navigation_setOrigin(&ecefOrigin);
handleAcknowledgement();
haveOrigin = TRUE;
event.flags.setOriginDone = TRUE;
DBPRINT("Set new origin: X=%.1f, Y=%.1f, Z=%.1f\n",
ecefOrigin.x, ecefOrigin.y, ecefOrigin.z);
}
else {
// Resend request if timer expires
if (Timer_isExpired(TIMER_SETORIGIN)) {
// Resend request origin if timed out
if (resendMessageCount >= RESEND_MESSAGE_LIMIT) {
// Sent too many times
setError(ERROR_NO_ORIGIN);
return;
}
else {
DBPRINTF("Resending origin request.\n");
Mavlink_sendRequestOrigin(NO_ACK); // just want message
Timer_new(TIMER_SETORIGIN, RESEND_MESSAGE_DELAY);
resendMessageCount++;
}
} // timer expired
}
}
int main(){
Board_init();
Board_configure(USE_TIMER);
DELAY(10);
DBPRINT("Starting XBee...\n");
if (Xbee_init(XBEE_UART_ID) != SUCCESS) {
DBPRINT("Failed XBee init.\n");
return FAILURE;
}
DBPRINT("XBee initialized.\n");
DELAY(STARTUP_DELAY);
Timer_new(TIMER_TEST, PRINT_DELAY);
unsigned long int sent = 0;
unsigned long int got = 0;
while(1){
Xbee_runSM();
if (Timer_isExpired(TIMER_TEST)) {
//dbprint("XBee: Sent=%ld, Got=%ld\n", ++sent, got);
Mavlink_sendRequestOrigin();
Timer_new(TIMER_TEST, PRINT_DELAY);
}
if (Mavlink_hasNewMessage()) {
++got;
}
}
return SUCCESS;
}
