/**********************************************************************
* Function: doStationKeepSM
* @return None
* @remark Steps into the station keeping state machine.
**********************************************************************/
static void doStationKeepSM() {
switch (subState) {
case STATE_STATIONKEEP_RETURN:
// Driving to the station
if (event.flags.navigationDone) {
subState = STATE_STATIONKEEP_IDLE;
Timer_new(TIMER_STATIONKEEP,STATION_KEEP_DELAY);
DBPRINT("Arrived at station.\n");
}
// TODO obstacle detection
break;
case STATE_STATIONKEEP_IDLE:
// Wait to float away from the station
if (Timer_isExpired(TIMER_STATIONKEEP)) {
// Check if we floated too far away from the station
if (Navigation_getLocalDistance(&nedStation) > STATION_TOLERANCE_MAX) {
startStationKeepSM(); // return to station
return;
}
Timer_new(TIMER_STATIONKEEP, STATION_KEEP_DELAY);
}
break;
}
if (event.flags.navigationError) {
setError((error_t)Navigation_getError());
}
}
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(void) {
//initializations
Board_init();
Board_configure(USE_SERIAL | USE_LCD | USE_TIMER);
Interface_init();
//I2C_init(I2C_ID, I2C_CLOCK_FREQ);
dbprint("Interface online.\n");
DELAY(STARTUP_DELAY);
Timer_new(TIMER_TEST, DEBUG_PRINT_DELAY);
//cycle and check if buttons are pressed, if so, turn light on for 3 seconds
while(1) {
#ifdef DO_STUFF
//check to see which button is pressed
if(Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(0,0);
dbprint("Ok=%d, C=%d, R=%d, S=%x,\n Reset=%x, SS=%x\n",
Interface_isOkPressed(), Interface_isCancelPressed(),
Interface_isRescuePressed(), Interface_isStopPressed(),
Interface_isResetPressed(), Interface_isSetStationPressed());
Timer_new(TIMER_TEST, DEBUG_PRINT_DELAY);
}
Interface_runSM();
#endif
}
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(void) {
// Initialize the UART,Timers, and I2C1v
Board_init();
Board_configure(USE_SERIAL | USE_LCD | USE_TIMER);
dbprint("Starting encoders...\n");
I2C_init(I2C_ID, I2C_CLOCK_FREQ);
Encoder_init();
Interface_init();
Timer_new(TIMER_TEST, PRINT_DELAY );
while (!Timer_isExpired(TIMER_TEST)) {
Encoder_runSM();
}
// Not working?
//Encoder_setZeroPitch();
//Encoder_setZeroYaw();
dbprint("Encoders initialized.\n");
DELAY(STARTUP_DELAY)
Timer_new(TIMER_TEST, PRINT_DELAY );
bool clearedCalibrateMessage = TRUE;
Timer_new(TIMER_TEST2,CALIBRATE_HOLD_DELAY);
//Interface_waitLightOnTimer(CALIBRATE_HOLD_DELAY);
Interface_waitLightOnTimer(CALIBRATE_HOLD_DELAY);
LCD_setPosition(0,0);
dbprint("Encoders:\n");
while(1) {
if (Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(1,0);
dbprint(" P=%.1f,\n Y=%.1f\n",Encoder_getPitch(), Encoder_getYaw());
Timer_new(TIMER_TEST, PRINT_DELAY );
}
if (Interface_isOkPressed() && Timer_isExpired(TIMER_TEST2)) {
Encoder_setZeroPitch();
Encoder_setZeroYaw();
LCD_setPosition(3,0);
dbprint("Zeroed encoders.\n");
clearedCalibrateMessage = FALSE;
Interface_waitLightOnTimer(CALIBRATE_HOLD_DELAY);
Interface_readyLightOff();
Timer_new(TIMER_TEST2,CALIBRATE_HOLD_DELAY);
}
if (Timer_isExpired(TIMER_TEST2) && !clearedCalibrateMessage) {
clearedCalibrateMessage = TRUE;
LCD_setPosition(3,0);
dbprint(" ");
Interface_readyLightOn();
}
Encoder_runSM();
Interface_runSM();
}
return (SUCCESS);
}
int main(void)
{
Board_init();
Serial_init();
LCD_init();
Timer_init();
printf("Testing the LCD display.\nYou should see: Hello World!\n");
LCD_writeString("Hello World!\nLine2\nLine3\nLine4\n");
Timer_new(TIMER_TEST,WRITE_DELAY);
while (!Timer_isExpired(TIMER_TEST)) {
asm("nop");
}
printf("Overwriting line 1\n");
LCD_writeString("Testing overtop line1.\n");
Timer_new(TIMER_TEST,WRITE_DELAY);
while (!Timer_isExpired(TIMER_TEST)) {
asm("nop");
}
printf("Clearing Display\n");
LCD_clearDisplay();
Timer_new(TIMER_TEST,WRITE_DELAY);
while (!Timer_isExpired(TIMER_TEST)) {
asm("nop");
}
LCD_writeString("Here.we.go\n");
LCD_writeString("CLEARED!\n");
LCD_writeString("CLEARED2!\n");
LCD_writeString("CLEARED3!\n");
Timer_new(TIMER_TEST,WRITE_DELAY);
while (!Timer_isExpired(TIMER_TEST)) {
asm("nop");
}
LCD_clearDisplay();
LCD_writeString("N: E: \n");
char tmp[10];
LCD_setPosition(0,3);
sprintf(tmp,"%.2f",12.5f);
LCD_writeString(tmp);
LCD_setPosition(0,13);
sprintf(tmp,"%.2f",-0.32f);
LCD_writeString(tmp);
return SUCCESS;
}
int main(){
//Initializations
Board_init();
Serial_init();
Timer_init();
if (Drive_init() != SUCCESS) {
printf("Failed to initialize drive module.\n");
}
I2C_init(TILT_COMPASS_I2C_ID, I2C_CLOCK_FREQ);
if (TiltCompass_init() != SUCCESS) {
printf("Failed to initialize tilt compass module.\n");
}
ENABLE_OUT_TRIS = OUTPUT;
while (1) {
printf("Driving north at full speed.\n");
Timer_new(TIMER_TEST, COMMAND_DELAY);
Drive_forwardHeading(100, 0);
while(!Timer_isExpired(TIMER_TEST)) {
//wait for finish
DELAY(1);
Drive_runSM();
TiltCompass_runSM();
}
Drive_stop();
Drive_runSM();
TiltCompass_runSM();
printf("Driving south at full speed.\n");
Timer_new(TIMER_TEST, COMMAND_DELAY);
Drive_forwardHeading(100, 180);
while(!Timer_isExpired(TIMER_TEST)) {
//wait for finish
DELAY(1);
Drive_runSM();
TiltCompass_runSM();
}
Drive_stop();
Drive_runSM();
TiltCompass_runSM();
delayMillisecond(COMMAND_DELAY);
printf("Waiting for retry...\n");
}
return SUCCESS;
}
/**********************************************************************
* Function: Drive_init
* @return SUCCESS or FAILURE.
* @remark Initializes the drive system state machine.
* @author David Goodman
* @date 2013.03.27
**********************************************************************/
bool Drive_init() {
uint16_t RC_pins = MOTOR_LEFT | MOTOR_RIGHT | RUDDER;
RC_init(RC_pins);
Timer_new(TIMER_DRIVE, 1);
#ifdef DEBUG
// Timers for debug print statements
Timer_new(TIMER_TEST2,1);
Timer_new(TIMER_TEST3,1);
#endif
state = STATE_IDLE;
}
char Accelerometer_init() {
int16_t c = readRegister(WHO_AM_I_ADDRESS); // Read WHO_AM_I register
if (c == WHO_AM_I_VALUE) //c != ERROR ||
{
DBPRINT("Accelerometer is online...\n");
}
else
{
DBPRINT("Accelerometer: Failed to connect (0x%X).\n",(int16_t)c);
return FAILURE;
}
setStandbyMode(); // Must be in standby to change registers
// Set up the full scale range to 2, 4, or 8g.
char fsr = GSCALE;
if(fsr > 8) fsr = 8; // Limit G-Scale
fsr >>= 2; // 00 = 2G, 01 = 4A, 10 = 8G (pg. 20)
writeRegister(XYZ_DATA_CFG_ADDRESS, fsr);
setActiveMode();
Timer_new(TIMER_ACCELEROMETER, UPDATE_DELAY);
#ifdef USE_ACCUMULATOR
resetAccumulator();
#endif
haveReading = FALSE;
return SUCCESS;
}
void Accelerometer_runSM() {
if (Timer_isExpired(TIMER_ACCELEROMETER)) {
updateReadings();
Timer_new(TIMER_ACCELEROMETER, UPDATE_DELAY);
}
}
/**********************************************************************
* Function: Interface_showMessageOnTimer
* @param message_t you want to display on a timer
* @param amount of time in ms that you want the message to remain on
* @return None.
* @remark After the timer expires the next message will be displayed
**********************************************************************/
void Interface_showMessageOnTimer(message_t msgCode, uint16_t ms){
Timer_new(TIMER_LCD_HOLD, ms);
// Prevent a previously held message from sticking after
if (nextMsgCode == NO_MESSAGE)
nextMsgCode = currentMsgCode;
showMessage(msgCode);
}
static void ping(int n, int count) {
Seawolf_loadConfig("../conf/seawolf.conf");
Seawolf_init(Util_format("Pinger %d", n));
Timer* timer = Timer_new();
static char action[16], data[16];
char* num = strdup(Util_format("%d", n));
int i = n;
Notify_filter(FILTER_MATCH, Util_format("PING %d", n));
Util_usleep(1.0);
const int rate = count * (PING_RATE / PRINT_RATE);
while(true) {
Notify_send("PING", Util_format("%d", n));
Notify_get(action, data);
assert(strcmp(action, "PING") == 0);
assert(strcmp(data, num) == 0);
if(i == (rate / count) * n) {
printf("%3d: %.4f\n", n, (Timer_getDelta(timer) / rate) - (1 / PING_RATE));
}
i = (i + 1) % rate;
Util_usleep(1 / PING_RATE);
}
}
int main(int argc, char** argv) {
if(argc != 3) {
fprintf(stderr, "Usage: %s <variable> <file>\n", argv[0]);
exit(1);
}
Seawolf_loadConfig("../conf/seawolf.conf");
Seawolf_init("DataLogger");
char* var = argv[1];
FILE* dump_file = fopen(argv[2], "w");
Timer* time = Timer_new();
Var_get(var);
Notify_filter(FILTER_MATCH, Util_format("UPDATED %s", var));
while(true) {
Notify_get(NULL, NULL);
fprintf(dump_file, "%5.2f %.2f\n", Timer_getTotal(time), Var_get(var));
}
fclose(dump_file);
Seawolf_close();
return 0;
}
/**********************************************************************
* Function: Override_init
* @return None
* @remark Initializes the override board and the interrupt to detect
* when the receiver comes online.
* @author Darrel Deo
* @date 2013.04.01
**********************************************************************/
void Override_init() {
// Initialize override board pins to give Micro control
ENABLE_OUT_TRIS = OUTPUT; // Set pin to be an output (fed to the AND gates)
ENABLE_OUT_LAT = MICRO_HAS_CONTROL; // Initialize control for Microcontroller
// Let override timer expire to disable override
Timer_new(TIMER_OVERRIDE, 1);
while (Override_isTriggered()) {
asm("nop");
}
//Enable the interrupt for the override feature
mPORTBSetPinsDigitalIn(BIT_0); // CN2
mCNOpen(CN_ON | CN_IDLE_CON , CN2_ENABLE , CN_PULLUP_DISABLE_ALL);
uint16_t value = mPORTDRead(); //?
ConfigIntCN(CHANGE_INT_ON | CHANGE_INT_PRI_2);
//CN2 J5-15
//DBPRINT("Override Function has been Initialized\n");
Override_giveMicroControl();
INTEnable(INT_CN,1);
}
/**********************************************************************
* Function: Interface_init
* @param None.
* @return None.
* @remark Initialzies the pins used by the buttons and LEDs.
**********************************************************************/
void Interface_init(){
// Set buttons as inputs
OKAY_BUTTON_TRIS = INPUT;
CANCEL_BUTTON_TRIS = INPUT;
STOP_BUTTON_TRIS = INPUT;
RESCUE_BUTTON_TRIS = INPUT;
SETSTATION_BUTTON_TRIS = INPUT;
// Set LEDs as outputs
READY_LED_TRIS = OUTPUT;
WAIT_LED_TRIS = OUTPUT;
ERROR_LED_TRIS = OUTPUT;
CALIBRATE_BACK_LED_TRIS = OUTPUT;
CALIBRATE_FRONT_LED_TRIS = OUTPUT;
// Reset button counts
buttonCount.okay = 0;
buttonCount.cancel = 0;
buttonCount.stop = 0;
buttonCount.rescue = 0;
buttonCount.setStation = 0;
buttonCount.reset = 0;
buttonReadCount = 0;
// Clear button flags
buttonPressed.bytes[0] = false;
// Turn off all LEDs and all buttons
Interface_clearAll();
Timer_new(TIMER_INTERFACE, WAIT_BETWEEN_CHECKS);
}
Win32Timer *Win32Timer_new () {
Win32Timer *s = malloc(sizeof(Win32Timer));
s->base = Timer_new();
if (QueryPerformanceFrequency((LARGE_INTEGER *) &s->m_frequency) == FALSE ) {
// No performance counter available
s->m_usePerformanceCounter = FALSE;
s->m_mmTimerStart = timeGetTime();
} else {
// Performance counter is available, use it instead of the multimedia timer
s->m_usePerformanceCounter = TRUE;
// Get the current time and store it in pcTimerStart
QueryPerformanceCounter(( LARGE_INTEGER *) &s->m_pcTimerStart);
// Calculate the timer resolution using the timer frequency
s->m_resolution = (float) ( 1.0 / (double) s->m_frequency ) * 1000.0f;
}
s->m_pauseCount = 0;
s->m_pauseTime = 0;
s->getTime = &Win32Timer_getTime;
return s;
}
/**********************************************************************
* 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
}
}
/**********************************************************************
* Function: doHeartbeatMessage
* @return None.
* @remark Occasionally sends a heartbeat message to the ComPAS.
* @author David Goodman
* @date 2013.05.04
**********************************************************************/
static void doHeartbeatMessage() {
if (Timer_isExpired(TIMER_HEARTBEAT)) {
Mavlink_sendHeartbeat();
Timer_new(TIMER_HEARTBEAT, HEARTBEAT_SEND_DELAY);
}
}
int main(void) {
// Initialize the UART,Timers, and I2C1v
Board_init();
Board_configure(USE_SERIAL | USE_LCD | USE_TIMER);
dbprint("Starting encoders...\n");
I2C_init(ENCODER_I2C_ID, I2C_CLOCK_FREQ);
if (Encoder_init() != SUCCESS) {
dbprint("Failed init. encoder\n");
return FAILURE;
}
Timer_new(TIMER_TEST, PRINT_DELAY );
while (!Timer_isExpired(TIMER_TEST)) {
Encoder_runSM();
}
// Not working?
//Encoder_setZeroPitch();
//Encoder_setZeroYaw();
dbprint("Encoders initialized.\n");
DELAY(STARTUP_DELAY);
Timer_new(TIMER_TEST, PRINT_DELAY );
LCD_setPosition(0,0);
dbprint("Encoders:\n");
while(1){
if (Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(1,0);
dbprint(" P=%.1f,\n Y=%.1f\n",Encoder_getPitch(), Encoder_getYaw());
/*dbprint("Encoders:\n P=%d,\n Y=%d\n",
readDevice(SLAVE_PITCH_READ_ADDRESS,SLAVE_PITCH_WRITE_ADDRESS),
readDevice(SLAVE_YAW_READ_ADDRESS,SLAVE_YAW_WRITE_ADDRESS));*/
/*dbprint("Encoders:\n P=%.1f,\n Y=%.1f\n",
readDevice(SLAVE_PITCH_READ_ADDRESS,SLAVE_PITCH_WRITE_ADDRESS) * DEGREE_PER_NUMBER,
readDevice(SLAVE_YAW_READ_ADDRESS,SLAVE_YAW_WRITE_ADDRESS) * DEGREE_PER_NUMBER);*/
Timer_new(TIMER_TEST, PRINT_DELAY );
}
Encoder_runSM();
}
return (SUCCESS);
}
int main() {
ENABLE_OUT_TRIS = OUTPUT; //Set Enable output pin to be an output, fed to the AND gates
ENABLE_OUT_LAT = MICRO; //Initialize control to that of Microcontroller
Board_init();
Serial_init();
Timer_init();
Drive_init();
I2C_init(I2C_BUS_ID, I2C_CLOCK_FREQ);
TiltCompass_init();
printf("Boat initialized.\n");
Drive_stop();
DELAY(5);
#ifdef MOTOR_TEST
Drive_pivot(0);
#endif
#ifdef RUDDER_TEST
Drive_forwardHeading(0.0, desiredHeading);
#endif
int i = 0;
int velocity[] = {1600, 1600, 1600, 1600, 1600};
velocityPulse = velocity[i];
Timer_new(TIMER_TEST,FINISH_DELAY);
while (1) {
if (Timer_isExpired(TIMER_TEST))
{
i++;
if (i == 5){
i = 0;
}
velocityPulse = velocity[i];
printf("CURRENT VELOCITY: %d\n\n",velocityPulse);
Timer_new(TIMER_TEST,FINISH_DELAY);
}
Drive_runSM();
TiltCompass_runSM();
}
Drive_stop();
return SUCCESS;
}
/**********************************************************************
* Function: doBarometerUpdate
* @return None.
* @remark Sends barometer data such as temperature and altitude if
* the timer expired.
* @author David Goodman
* @date 2013.05.04
**********************************************************************/
static void doBarometerUpdate() {
if (Timer_isExpired(TIMER_BAROMETER_SEND)) {
Mavlink_sendBarometerData(Barometer_getTemperature(),
Barometer_getAltitude());
Timer_new(TIMER_BAROMETER_SEND, BAROMETER_SEND_DELAY);
}
}
/**********************************************************************
* Function: Interrupt Service Routine for Override board
* @return None
* @remark ISR that is called when CH3 pings external interrupt
* @author Darrel Deo
* @date 2013.04.01
**********************************************************************/
void __ISR(_CHANGE_NOTICE_VECTOR, ipl2) ChangeNotice_Handler(void){
mPORTDRead(); //?
Timer_new(TIMER_OVERRIDE, OVERRIDE_TIMEOUT_DELAY);
//Clear the interrupt flag that was risen for the external interrupt
//might want to set a timer in here
mCNClearIntFlag();
//INTEnable(INT_CN,0);
}
/**********************************************************************
* Function: startSetOriginSM
* @return None.
* @remark Transitions into the set origin state machine.
* @author David Goodman
* @date 2013.05.04
**********************************************************************/
static void startSetOriginSM() {
state = STATE_SETORIGIN;
subState = STATE_NONE;
// Send status message to command center to request origin coordinate
Mavlink_sendStatus(MAVLINK_REQUEST_ORIGIN);
resendMessageCount = 0;
Timer_new(TIMER_SETORIGIN, RESEND_MESSAGE_DELAY);
DBPRINT("Requesting origin.\n");
}
int main(void) {
//initializations
Board_init();
Serial_init();
Timer_init();
LCD_init();
Interface_init();
printf("INITIALIZATIONS COMPLETE\n");
LCD_writeString("Interface online.\n");
Timer_new(TIMER_TEST, 100);
while (1) {
if (Timer_isExpired(TIMER_TEST)) {
printf("%X -- C:%X F:%X\n",Interface_isOkPressed(), buttonCount.okay, buttonPressed.flag.okay);
Timer_new(TIMER_TEST, 100);
}
Interface_runSM();
}
}
/**********************************************************************
* Function: initializeAtlas
* @return None.
* @remark Initializes the boat's master state machine and all modules.
* @author David Goodman
* @date 2013.04.24
**********************************************************************/
static void initializeAtlas() {
Board_init();
Serial_init();
Timer_init();
#ifdef USE_DRIVE
Drive_init();
#endif
// I2C bus
I2C_init(I2C_BUS_ID, I2C_CLOCK_FREQ);
#ifdef USE_TILTCOMPASS
TiltCompass_init();
#endif
#ifdef USE_GPS
GPS_init();
#endif
#ifdef USE_NAVIGATION
Navigation_init();
#endif
#ifdef USE_OVERRIDE
Override_init();
#endif
#ifdef USE_BAROMETER
Barometer_init();
Timer_new(TIMER_BAROMETER_SEND, BAROMETER_SEND_DELAY);
#endif
// Start calibrating before use
startSetOriginSM();
Timer_new(TIMER_HEARTBEAT, HEARTBEAT_SEND_DELAY);
Mavlink_sendStatus(MAVLINK_STATUS_ONLINE);
}
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
}
void Thermal_init(){
I2C_init(THERMAL_I2C_ID,I2C_CLOCK_FREQ);
count = 0;
readEeprom();
writeTrimmingValue();
writeConfigReg();
readConfigReg();
configCalculationData();
Timer_new(TIMER_THERMAL,READ_DELAY);
}
int main(void) {
// Initialize the UART,Timers, and I2C1
//int row = 0;
Board_init();
Timer_init();
Thermal_init();
Serial_init();
/*Timer_new(TIMER_TEST,100);
while(!Timer_isExpired(TIMER_TEST);*/
#ifdef DEBUG_TEST2
printf("Initialized Thermal module.\n");
#endif
uint8_t row = 0, col = 0;
Timer_new(TIMER_TEST,PRINT_DELAY);
while(1) {
if (Timer_isExpired(TIMER_TEST)) {
#ifdef DEBUG_TEST2
printf("Sending thermal data...\n");
#endif
sendSerial32(0xFFFF1234);
for(col = 0; col < TOTAL_PIXEL_COLS; col++) {
for(row = 0; row < TOTAL_PIXEL_ROWS; row++){
uint8_t pixel = row + (col * TOTAL_PIXEL_ROWS);
sendSerialFloat(finalPixelTempF[pixel]);
}
sendSerial32(END_COL_SEQUENCE);
}
#ifdef DEBUG_TEST2
printf("\nFinished sending thermal data.\n");
#endif
Timer_new(TIMER_TEST,PRINT_DELAY);
} // Timer_isExpired
Thermal_runSM();
} // while
}
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: Drive_runSM
* @return None.
* @remark Executes a cycle of the drive system state machine.
* @author David Goodman
* @date 2013.03.27
**********************************************************************/
void Drive_runSM() {
switch (state) {
case STATE_IDLE:
// Do nothing
break;
case STATE_DRIVE:
// Just driving, do nothing
break;
case STATE_TRACK:
if (Timer_isExpired(TIMER_DRIVE)) {
updateRudder();
Timer_new(TIMER_DRIVE, TRACK_UPDATE_DELAY);
}
break;
} // switch
}
