diff --git a/master/master/config.h b/master/master/config.h
--- a/master/master/config.h
+++ b/master/master/config.h
@@ -23,7 +23,7 @@
 #define MODULE_ID '1'
 #define BOARDTEMP_ADDR 0x90
 
-#define HEATER_THRESHOLD 70
+#define HEATER_THRESHOLD 25
 
 // --------------------------------------------------------------------------
 // Error Codes config (led.c, used throughout code)
diff --git a/master/master/lib/led.c b/master/master/lib/led.c
--- a/master/master/lib/led.c
+++ b/master/master/lib/led.c
@@ -23,6 +23,12 @@ void led_setup()
 		*(ledList[i].direction) |= (1<<ledList[i].pin); // set pin to output
 		*(ledList[i].port) &= ~(1<<ledList[i].pin); // set pin low
 	}
+	
+	// Beep on startup
+	led_on(LED_POWER);
+	led_on(LED_BUZZ);
+	_delay_ms(1);
+	led_off(LED_BUZZ);
 }
 
 // Turn the specified LED on
diff --git a/master/master/lib/logger.c b/master/master/lib/logger.c
--- a/master/master/lib/logger.c
+++ b/master/master/lib/logger.c
@@ -148,10 +148,6 @@ void logger_closeLog()
 }
 
 
-
-
-
-
 // INTERNAL FUNCTIONS
 
 // Opens a file so it can be read/written
diff --git a/master/master/lib/sensordata.c b/master/master/lib/sensordata.c
--- a/master/master/lib/sensordata.c
+++ b/master/master/lib/sensordata.c
@@ -12,8 +12,11 @@
 
 #include "../config.h"
 #include <stdio.h>
+#include <stdbool.h>
 #include "sensordata.h"
+#include "slavesensors.h"
 #include "boardtemp.h"
+#include "looptime.h"
 #include "gps.h"
 
 int16_t slaves[MAX_NUM_SLAVES][MAX_NUM_SENSORS];
@@ -53,4 +56,54 @@ char commentBuffer[128];
 char* slavesensors_getAPRScomment() {
 	snprintf(commentBuffer,128, "T%d S%s V%s H%s", sensors_getBoardTemp(), get_sv(), get_speedKnots(), get_hdop());
 	return commentBuffer;
+}
+
+
+char logbuf[128];
+bool dataWasReady = false;
+
+void sensordata_logvalues() {
+	// Generate CSV header after we have queried all slaves once
+	if(slavesensors_dataReady()) {
+	
+		// Only generate/write header the first time data is ready
+		if(!dataWasReady) {
+			char csvHeader[128];
+			csvHeader[0] = 0x00;
+		
+			// Add master data headers
+			snprintf(csvHeader, 128, "Time,BoardTemp,Lat,Lon,HDOP,Speed,GPS SV,");
+		
+			// Add slave data headers
+			for(uint8_t i=0; i<MAX_NUM_SLAVES; i++) {
+				for(uint8_t j=0; j<MAX_NUM_SENSORS; j++) {
+					int16_t tmp = sensordata_get(i, j);
+					if(tmp != -32768) {
+						// FIXME: will the 128 here really provide safety? might want to subtract the strlen
+						snprintf(csvHeader + strlen(csvHeader), 128,"%s-%s,", slavesensors_slavename(i), slavesensors_getLabel(j));
+					}
+				}
+			}
+		
+			// Terminate header string and write to SD card
+			snprintf(csvHeader + strlen(csvHeader), 128,"\r\n");
+			logger_log(csvHeader);
+			dataWasReady = true;
+		}
+	
+		// Write CSV sensor values to SD card
+		logbuf[0] = 0x00;
+		snprintf(logbuf, 128, "%lu,%d,%u,%s,%s,%s,%s,%s", time_millis(), sensors_getBoardTemp(),get_timestamp(),get_latitude(),get_longitude(),get_speedKnots(),get_hdop(), get_course());
+		for(int i=0; i<MAX_NUM_SLAVES; i++) {
+			for(int j=0; j<MAX_NUM_SENSORS; j++) {
+				int16_t tmp = sensordata_get(i, j);
+				if(tmp != -32768) {
+					snprintf(logbuf + strlen(logbuf),128," %d,", tmp);
+				}
+			
+			}
+		}
+		snprintf(logbuf + strlen(logbuf),128,"\r\n");
+		logger_log(logbuf);
+	}
 }
\ No newline at end of file
diff --git a/master/master/lib/sensordata.h b/master/master/lib/sensordata.h
--- a/master/master/lib/sensordata.h
+++ b/master/master/lib/sensordata.h
@@ -22,5 +22,6 @@ int16_t sensordata_get(uint8_t nodeID, u
 void sensordata_setBoardTemp(uint8_t slaveID, int16_t value);
 int16_t sensordata_getBoardTemp(uint8_t slaveID);
 char* slavesensors_getAPRScomment();
+void sensordata_logvalues();
 
 #endif /* SENSORDATA_H_ */
\ No newline at end of file
diff --git a/master/master/master.c b/master/master/master.c
--- a/master/master/master.c
+++ b/master/master/master.c
@@ -27,9 +27,7 @@
 #include "lib/led.h"
 #include "lib/logger.h"
 #include "lib/watchdog.h"
-
 #include "lib/gps.h"
-
 #include "lib/i2c.h"
 #include "lib/boardtemp.h"
 #include "lib/heater.h"
@@ -44,34 +42,25 @@ int main(void)
 	time_setup();
 	watchdog_setup(); // enables interrupts
 	led_setup();
-	
-	led_on(LED_POWER);
-	led_on(LED_BUZZ);
-	_delay_ms(1);
-	led_off(LED_BUZZ);
-	
 	gps_setup();
 	serial0_setup();
 	serial1_setup();
 	i2c_init();
-	sensordata_setup(); // must happen before sensors/logger/afsk
+	sensordata_setup(); // must happen before slavesensors/logger/AFSK
 	slavesensors_setup();
 	logger_setup();
 	afsk_setup();
-	//\f
 	
 	#ifdef DEBUG_OUTPUT
-	serial0_sendString("\r\n---------------------------------\r\n");
-	serial0_sendString("HAB Controller 1.0 - Initialized!\r\n");
-	serial0_sendString("---------------------------------\r\n");
+		serial0_sendString("\r\n---------------------------------\r\n");
+		serial0_sendString("HAB Controller 1.0 - Initialized!\r\n");
+		serial0_sendString("---------------------------------\r\n");
 	#endif
 	
 	serial0_sendString("\r\nHello.\r\n\r\n");
 	
-	slavesensors_network_scan(); // This can take a little while
-	
-	// Buffer for string operations
-	char logbuf[128];
+	// Blocking ZigBee node discovery
+	slavesensors_network_scan();
 	
 	// Software timers	
 	uint32_t lastAprsBroadcast = 0;
@@ -79,11 +68,10 @@ int main(void)
 	uint32_t lastLedCycle = 0;
 	uint32_t lastDataReq = 0;
 	
-	bool dataWasReady = false;
-	
 	// Result of last parser run
 	int parseResult = PARSERESULT_NODATA;
 	
+	// FIXME: Probably don't need this.
 	serial1_ioff();
 	
 	while(1)
@@ -93,8 +81,10 @@ int main(void)
 		if(time_millis() - lastLedCycle > LEDCYCLE_RATE) {
 			led_spin();
 			
+			// Enable GPS serial interrupts if we aren't doing AFSK
 			if(!afsk_busy())
 				serial1_ion();
+				
 			lastLedCycle = time_millis();	
 		}
 		
@@ -105,11 +95,6 @@ int main(void)
 			
 			heater_regulateTemp();
 			
-			// Print out GPS debug
-			//snprintf(debugBuf, 128, "GPS> time: %s lat: %s lon: %s speed: %s hdop: %s course: %s\r\n",
-			//get_timestamp(),get_latitude(),get_longitude(),get_speedKnots(),get_hdop(), get_course());
-			//serial0_sendString(debugBuf);
-			
 			// Turn on sideboard LED if we have a fix
 			if(strcmp("99.99", get_hdop()) == 0) {
 				led_off(LED_SIDEBOARD);
@@ -118,54 +103,10 @@ int main(void)
 				led_on(LED_SIDEBOARD);
 			}
 			
-			sensors_readBoardTemp(); // i2c read, 400k
+			sensors_readBoardTemp();
 		
-			// If we've gotten data from all slaves once, we're ready to make a CSV header and start logging
-			if(slavesensors_dataReady()) {
-				if(!dataWasReady) {
-					char csvHeader[128];
-					csvHeader[0] = 0x00;
-					
-					// Add master data headers
-					snprintf(csvHeader, 128, "Time,BoardTemp,Lat,Lon,HDOP,Speed,GPS SV,");
-					
-					for(uint8_t i=0; i<MAX_NUM_SLAVES; i++) {
-						for(uint8_t j=0; j<MAX_NUM_SENSORS; j++) {
-							int16_t tmp = sensordata_get(i, j);
-							if(tmp != -32768) {
-								// FIXME: will the 128 here really provide safety? might want to subtract the strlen
-								snprintf(csvHeader + strlen(csvHeader), 128,"%s-%s,", slavesensors_slavename(i), slavesensors_getLabel(j));	
-							}
-						}
-					}
-					snprintf(csvHeader + strlen(csvHeader), 128,"\r\n");	
-					serial0_sendString(csvHeader);
-					logger_log(csvHeader);
-					dataWasReady = true;
-				}
-		
-				logbuf[0] = 0x00;
-				snprintf(logbuf, 128, "%lu,%d,%u,%s,%s,%s,%s,%s", time_millis(), sensors_getBoardTemp(),get_timestamp(),get_latitude(),get_longitude(),get_speedKnots(),get_hdop(), get_course());
-				for(int i=0; i<MAX_NUM_SLAVES; i++) {
-					for(int j=0; j<MAX_NUM_SENSORS; j++) {
-						int16_t tmp = sensordata_get(i, j);
-						if(tmp != -32768) {
-							snprintf(logbuf + strlen(logbuf),128," %d,", tmp);
-						}
-					
-					}
-				}
-				snprintf(logbuf + strlen(logbuf),128,"\r\n");
-				serial0_sendString(logbuf);			
-				logger_log(logbuf);
-			
-			}			
-			
-			// Print out logger debug
-			#ifdef DEBUG_OUTPUT
-			//serial0_sendString("LOG> ");
-			//serial0_sendString(logbuf);
-			#endif
+			// Write CSV header and log data values
+			sensordata_logvalues();			
 			
 			led_off(LED_CYCLE);
 			lastLog = time_millis();
@@ -174,10 +115,11 @@ int main(void)
 		
 		// Periodic: Data Request
 		if(time_millis() - lastDataReq > DATAREQUEST_RATE)  {
+			
 			// Start getting values for next transmission
 			if(slavesensors_isrequesting())
 			{
-				// TODO: something is terribly wrong
+				// TODO: something is terribly wrong. Timeout?
 			}
 			else
 			{
@@ -191,17 +133,23 @@ int main(void)
 		// Periodic: APRS transmission
 		if(time_millis() - lastAprsBroadcast > APRS_TRANSMIT_PERIOD) 
 		{
+			// Ensure we aren't already transmitting
 			while(afsk_busy());
+			
+			// Turn off interrupts and transmit APRS sentence
 			serial1_ioff();
 			aprs_send(); // non-blocking
 			
 			lastAprsBroadcast = time_millis();
 		}			
 		
-
+		// Parse any serial data in the XBee software buffer
 		parseResult = serparser_parse();
 		slavesensors_process(parseResult);
+		
+		// Parse any NMEA messages in the GPS software buffer
 		parse_gps_transmission();
+		
 		wdt_reset();
     }
 }
\ No newline at end of file