int main(){ Board_init(); Serial_init(); Timer_init(); Xbee_init(XBEE_UART_ID); printf("Xbee Test 2\n"); while(1){ Xbee_runSM(); //if(!UART_isTransmitEmpty(UART1_ID)); //printf("%d\t",Mavlink_returnACKStatus(messageName_start_rescue)); if(!UART_isReceiveEmpty(UART1_ID)){ Serial_getChar(); Mavlink_send_start_rescue(UART2_ID, TRUE, 0xFF, 0x34FD, 0xAB54); Timer_new(TIMER_TIMEOUT, DELAY_TIMEOUT); printf("\nSENT\n"); } else if(Timer_isActive(TIMER_TIMEOUT) != TRUE && Mavlink_returnACKStatus(messageName_start_rescue) == ACK_STATUS_WAIT){ //else if(Timer_isActive(TIMER_TIMEOUT) != TRUE){ Mavlink_editACKStatus(messageName_start_rescue, ACK_STATUS_DEAD); printf("ACK DEAD\n"); } else if(Mavlink_returnACKStatus(messageName_start_rescue) == ACK_STATUS_DEAD){ Mavlink_send_start_rescue(UART2_ID, TRUE, 0xFF, 0x34FD, 0xAB54); Timer_new(TIMER_TIMEOUT, DELAY_TIMEOUT); } else if(Mavlink_returnACKStatus(messageName_start_rescue) == ACK_STATUS_RECIEVED){ Mavlink_editACKStatus(messageName_start_rescue, ACK_STATUS_NO_ACK); printf("GPS SENT AND ACKOWLEGED\n"); } } }
/** * Function: runMasterSM * @return None. * @remark Executes one cycle of the command center's state machine. * @author David Goodman * @date 2013.03.09 */ void runMasterSM() { //Magnetometer_runSM(); // Record these button presses since we don't know // if they will be pressed after runSM lockPressed = isLockPressed(); zeroPressed = isZeroPressed(); if(lockPressed || zeroPressed){ #ifdef USE_ENCODERS Encoder_runSM(); #endif if(lockPressed) { printf("Lock was pressed.\n"); #ifdef USE_NAVIGATION #ifdef USE_ENCODERS Encoder_enableZeroAngle(); Encoder_runSM(); Coordinate ned; // = Coordinate_new(ned, 0, 0 ,0); if (Navigation_getProjectedCoordinate(&ned, Encoder_getYaw(), Encoder_getPitch(), height)) { printf("Desired coordinate -- N: %.6f, E: %.6f, D: %.2f (m)\n", ned.x, ned.y, ned.z); #ifdef USE_XBEE Mavlink_send_start_rescue(XBEE_UART_ID, TRUE, 0,ned.x, ned.y); #endif } else { printf("Failed to obtain desired NED coordinate.\n"); } Encoder_disableZeroAngle(); #else printf("Navigation module is disabled.\n"); #endif #endif } else if (zeroPressed) { // Zero was pressed #ifdef USE_ENCODERS Encoder_setZeroAngle(); #endif useLevel = TRUE; #ifdef USE_MAGNETOMETER Magnetometer_runSM(); heading = Magnetometer_getDegree(); updateHeading(); #endif //printf("Zeroing...\n"); } else { } } if (!zeroPressed) { if (useLevel) printf("Done zeroing.\n"); useLevel = FALSE; } #ifdef USE_ACCELEROMETER Accelerometer_runSM(); updateAccelerometerLEDs(); #endif #ifdef USE_NAVIGATION Navigation_runSM(); #endif #ifdef USE_XBEE Xbee_runSM(); #endif }