/********************************************************************** * 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; }