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;
}
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");
}
}
}
int main(){
Board_init();
Serial_init();
printf("Welcome to Xbee Test1\n");
printf("UART INIT\n");
Timer_init();
printf("Timers Init\n");
if(Xbee_init(XBEE_UART_ID) == FAILURE){
printf("Xbee Failed to initilize\n");
return FAILURE;
}
printf("XBEE Initialized\n");
// Master sends packets and listens for responses
#ifdef XBEE_1
Mavlink_send_Test_data(XBEE_UART_ID, 1);
Timer_new(TIMER_TIMEOUT, DELAY_TIMEOUT);
Timer_new(TIMER_STATUS, DELAY_STATUS);
while(1){
Xbee_runSM();
//lost a packet, report it, and restart
if(Timer_isActive(TIMER_TIMEOUT) != TRUE){
Mavlink_send_Test_data(XBEE_UART_ID, 1);
count_lost++;
Timer_new(TIMER_TIMEOUT, DELAY_TIMEOUT);
printf("lost_packet: %d\n", get_time());
}
//Printout the status
if(Timer_isActive(TIMER_STATUS) != TRUE){
Timer_new(TIMER_STATUS, DELAY_STATUS);
printf("Status: %d,%d [Recieved,Lost] TIME: %d\n", count_recieved, count_lost, get_time());
}
}
#else
while(1){
Xbee_runSM();
}
#endif
}
/*
* Entry point
*/
int main(void) {
Watchdog_init();
DBPRINT("Watchdog initialized.\n");
while (1) {
doWatchdog();
#ifdef USE_XBEE
Xbee_runSM();
#endif
}
return (EXIT_SUCCESS);
}
int main(){
Board_init();
Board_configure(USE_SERIAL | USE_TIMER);
//Board_configure(USE_LCD | 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);
LCD_setPosition(0,0);
Timer_new(TIMER_TEST, PRINT_DELAY);
unsigned long int sent = 0;
unsigned long int got = 0;
LCD_clearDisplay();
LCD_setPosition(2,0);
dbprint("Received: %ld", got);
while(1){
Xbee_runSM();
if (Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(0,0);
dbprint("Sent XBee msg.\nSent: %ld\n", ++sent);
Mavlink_sendSaveStation(WANT_ACK);
Timer_new(TIMER_TEST, PRINT_DELAY);
}
if (Mavlink_hasNewMessage()) {
LCD_setPosition(2,0);
dbprint("Received: %ld", ++got);
}
}
return SUCCESS;
}
/**********************************************************************
* Function: doMasterSM
* @return None.
* @remark Executes one cycle of the boat's master state machine.
* @author David Goodman
* @date 2013.03.28
**********************************************************************/
static void doMasterSM() {
checkEvents();
#ifdef USE_TILTCOMPASS
TiltCompass_runSM();
#endif
#ifdef USE_GPS
GPS_runSM();
#endif
#ifdef USE_NAVIGATION
Navigation_runSM();
#ifdef USE_ERROR_CORRECTION
gpsCorrectionUpdate();
#endif
#endif
#ifdef USE_DRIVE
Drive_runSM();
#endif
#ifdef USE_XBEE
Xbee_runSM();
#endif
#ifdef USE_BAROMETER
Barometer_runSM();
doBarometerUpdate(); // send barometer data
#endif
switch (state) {
case STATE_SETSTATION:
doSetStationSM();
if (event.flags.haveStartRescueMessage) {
startRescueSM();
}
else if (event.flags.setStationDone) {
if (haveOrigin)
startStationKeepSM();
else
startOverrideSM();
}
break;
case STATE_SETORIGIN:
doSetOriginSM();
if (event.flags.setOriginDone)
startOverrideSM();
break;
case STATE_STATIONKEEP:
doStationKeepSM();
if (event.flags.haveStartRescueMessage)
startRescueSM();
else if (!haveStation)
setError(ERROR_NO_STATION);
break;
case STATE_OVERRIDE:
if (!wantOverride) {
//setError(ERROR_NO_ORIGIN);
if (!haveOrigin)
startSetOriginSM(); // do we ant infinite startup loop?
else if (event.flags.haveStartRescueMessage)
startRescueSM();
else if (event.flags.haveSetStationMessage )
startSetStationSM();
else if (haveOrigin && haveStation)
startStationKeepSM();
// Use autonomous controls
if (haveOrigin && (haveStation
|| event.flags.haveStartRescueMessage)) {
Override_giveMicroControl();
DBPRINT("Micro has control.\n");
#ifdef USE_SIREN
Siren_blueLightOff();
#endif
}
}
break;
case STATE_RESCUE:
doRescueSM();
if (event.flags.haveStartRescueMessage) {
startRescueSM();
}
else if (event.flags.haveReturnStationMessage) {
if (haveStation)
startStationKeepSM();
else
setError(ERROR_NO_STATION);
}
// Turn off rescue siren (red)
if (event.flags.haveError || state != STATE_RESCUE) {
#ifdef USE_SIREN
Siren_redLightOff();
#endif
}
break;
}
// ------- Caught by most states -----------
if (state != STATE_RESCUE) {
if (event.flags.haveSetStationMessage)
startSetStationSM();
}
if (state != STATE_OVERRIDE) {
if (event.flags.haveError) {
startOverrideSM();
overrideShutdown = TRUE;
}
if (wantOverride)
startOverrideSM();
}
if (event.flags.haveResetMessage)
resetAtlas();
}
/**
* 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
}
