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
}
