diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h
index 2b222a7ee0..9db7354b0f 100644
--- a/ArduBoat/ControllerBoat.h
+++ b/ArduBoat/ControllerBoat.h
@@ -8,7 +8,7 @@
 #ifndef CONTROLLERBOAT_H_
 #define CONTROLLERBOAT_H_
 
-#include "../APO/AP_Controller.h"
+#include "../APO/APO.h"
 
 namespace apo {
 
@@ -16,7 +16,7 @@ class ControllerBoat: public AP_Controller {
 public:
     ControllerBoat(AP_Navigator * nav, AP_Guide * guide,
                    AP_HardwareAbstractionLayer * hal) :
-        AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode),
+        AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl),
         pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
                steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
         pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
@@ -49,7 +49,7 @@ private:
                          _guide->getGroundSpeedCommand()
                          - _nav->getGroundSpeed(), dt);
     }
-    void setMotorsActive() {
+    void setMotors() {
         // turn all motors off if below 0.1 throttle
         if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
             setAllRadioChannelsToNeutral();
@@ -58,6 +58,10 @@ private:
             _hal->rc[ch_str]->setPosition(_strCmd);
         }
     }
+    void handleFailsafe() {
+        // failsafe is to turn off
+        setMode(MAV_MODE_LOCKED);
+    }
 
     // attributes
     enum {
diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde
index af858b6dfd..0e2eef8c2f 100644
--- a/ArduPlane/Log.pde
+++ b/ArduPlane/Log.pde
@@ -52,9 +52,9 @@ print_log_menu(void)
 {
 	int log_start;
 	int log_end;
-	int temp;
+	int temp;	
+	
 	uint16_t num_logs = get_num_logs();
-//Serial.print("num logs: ");	Serial.println(num_logs, DEC);
 
 	Serial.printf_P(PSTR("logs enabled: "));
 
@@ -82,7 +82,7 @@ print_log_menu(void)
 	Serial.println();
 
 	if (num_logs == 0) {
-		Serial.printf_P(PSTR("\nNo logs\nType 'dump 0'.\n\n"));
+		Serial.printf_P(PSTR("\nNo logs\n\n"));
 	}else{
 		Serial.printf_P(PSTR("\n%d logs\n"), num_logs);
 
@@ -109,17 +109,18 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 	last_log_num = g.log_last_filenumber;
 	if ((argc != 2) || (dump_log <= (last_log_num - get_num_logs())) || (dump_log > last_log_num)) {
 		Serial.printf_P(PSTR("bad log number\n"));
+		Log_Read(0, 4095);
 		return(-1);
 	}
 
 	get_log_boundaries(dump_log, dump_log_start, dump_log_end);
-	Serial.printf_P(PSTR("Dumping Log number %d,    start page %d,   end page %d\n"),
+	Serial.printf_P(PSTR("Dumping Log %d,    start pg %d,   end pg %d\n"),
 				  dump_log,
 				  dump_log_start,
 				  dump_log_end);
 
 	Log_Read(dump_log_start, dump_log_end);
-	Serial.printf_P(PSTR("Log read complete\n"));
+	Serial.printf_P(PSTR("Done\n"));
     return 0;
 }
 
@@ -204,6 +205,7 @@ static byte get_num_logs(void)
 	if(g.log_last_filenumber < 1) return 0;
 
 	DataFlash.StartRead(1);
+	
 	if(DataFlash.GetFileNumber() == 0XFFFF) return 0;
 
 	lastpage = find_last();
@@ -211,7 +213,7 @@ static byte get_num_logs(void)
 	last = DataFlash.GetFileNumber();
 	DataFlash.StartRead(lastpage + 2);
 	first = DataFlash.GetFileNumber();
-	if(first == 0xFFFF) {
+	if(first > last) {
 		DataFlash.StartRead(1);
 		first = DataFlash.GetFileNumber();
 	}
@@ -285,130 +287,118 @@ static int find_last(void)
 // This function finds the last page of a particular log file
 static int find_last_log_page(uint16_t log_number)
 {
-	int16_t bottom_page;
-	int16_t top_page;
-	int16_t bottom_page_file;
-	int16_t bottom_page_filepage;
-	int16_t top_page_file;
-	int16_t top_page_filepage;
-	int16_t look_page;
-	int16_t look_page_file;
-	int16_t look_page_filepage;
-	int16_t check;
-	bool	XLflag = false;
 
-	// First see if the logs are empty
-	DataFlash.StartRead(1);
-	if(DataFlash.GetFileNumber() == 0XFFFF) {
+	uint16_t bottom_page;
+	uint16_t bottom_page_file;
+	uint16_t bottom_page_filepage;
+	uint16_t top_page;
+	uint16_t top_page_file;
+	uint16_t top_page_filepage;
+	uint16_t look_page;
+	uint16_t look_page_file;
+	uint16_t look_page_filepage;
+
+	bottom_page = 1;
+	DataFlash.StartRead(bottom_page);
+	bottom_page_file = DataFlash.GetFileNumber();
+	bottom_page_filepage = DataFlash.GetFilePage();
+	
+	// First see if the logs are empty.  If so we will exit right away.
+	if(bottom_page_file == 0XFFFF) {
 	return 0;
 	}
+	
+	top_page = DF_LAST_PAGE;
+	DataFlash.StartRead(top_page);
+	top_page_file = DataFlash.GetFileNumber();
+	top_page_filepage = DataFlash.GetFilePage();
 
-	// Next, see if logs wrap the top of the dataflash
-	DataFlash.StartRead(DF_LAST_PAGE);
-	if(DataFlash.GetFileNumber() == 0xFFFF)
-	{
-	// This case is that we have not wrapped the top of the dataflash
-		top_page = DF_LAST_PAGE;
-		bottom_page = 1;
-		while((top_page - bottom_page) > 1) {
-			look_page = (top_page + bottom_page) / 2;
-			DataFlash.StartRead(look_page);
-			if(DataFlash.GetFileNumber() > log_number)
-				top_page = look_page;
-			else
+
+	while((top_page - bottom_page) > 1) {
+		look_page = ((long)top_page + (long)bottom_page) / 2L;
+	
+		DataFlash.StartRead(look_page);
+		look_page_file = DataFlash.GetFileNumber();
+		look_page_filepage = DataFlash.GetFilePage();
+		
+		// We have a lot of different logic cases dependant on if the log space is overwritten
+		// and where log breaks might occur relative to binary search points.
+		// Someone could make work up a logic table and simplify this perhaps, or perhaps
+		// it is easier to interpret as is.
+		
+		if (look_page_file == 0xFFFF) {
+			top_page = look_page;
+			top_page_file = look_page_file;
+			top_page_filepage = look_page_filepage;
+			
+		} else if (look_page_file == log_number && bottom_page_file == log_number && top_page_file == log_number) {
+		// This case is typical if a single file fills the log and partially overwrites itself
+			if (bottom_page_filepage < top_page_filepage) {
 				bottom_page = look_page;
-		}
-		 return bottom_page;
-
-	} else {
-	// The else case is that the logs do wrap the top of the dataflash
-		bottom_page = 1;
-		top_page = DF_LAST_PAGE;
-		DataFlash.StartRead(bottom_page);
-		bottom_page_file = DataFlash.GetFileNumber();
-		bottom_page_filepage = DataFlash.GetFilePage();
-		DataFlash.StartRead(top_page);
-		top_page_file = DataFlash.GetFileNumber();
-		top_page_filepage = DataFlash.GetFilePage();
-
-		// Check is we have exactly filled the dataflash but not wrapped.  If so we can exit quickly.
-		if(top_page_file == log_number && bottom_page_file != log_number) {
-		return top_page_file;
-		}
-
-		// Check if the top is 1 file higher than we want.  If so we can exit quickly.
-		if(top_page_file == log_number+1) {
-		return top_page - top_page_filepage;
-		}
-
-		// Check if the file has partially overwritten itself
-		if(top_page_filepage >= DF_LAST_PAGE) {
-			XLflag = true;
-		} else {
-			top_page = top_page - top_page_filepage;
-			DataFlash.StartRead(top_page);
-			top_page_file = DataFlash.GetFileNumber();
-		}
-		if(top_page_file == log_number) {
-			bottom_page = top_page;
-			top_page = DF_LAST_PAGE;
-			DataFlash.StartRead(top_page);
-			top_page_filepage = DataFlash.GetFilePage();
-			if(XLflag) bottom_page = 1;
-
-			while((top_page - bottom_page) > 1) {
-				look_page = (top_page + bottom_page) / 2;
-				DataFlash.StartRead(look_page);
-				if(DataFlash.GetFilePage() < top_page_filepage)
-				{
-					top_page = look_page;
-					top_page_filepage = DataFlash.GetFilePage();
-				} else {
-					bottom_page = look_page;
-				}
+				bottom_page_file = look_page_file;
+				bottom_page_filepage = look_page_filepage;
+			} else {
+				top_page = look_page;
+				top_page_file = look_page_file;
+				top_page_filepage = look_page_filepage;
 			}
-			return bottom_page;
-		}
-
-
-		// Step down through the files to find the one we want
-		bottom_page = top_page;
-		bottom_page_filepage = top_page_filepage;
-		do
-		{
-            int16_t last_bottom_page_file;
-			top_page = bottom_page;
-			bottom_page = bottom_page - bottom_page_filepage;
-			if(bottom_page < 1) bottom_page = 1;
-			DataFlash.StartRead(bottom_page);
-            last_bottom_page_file = bottom_page_file;
-			bottom_page_file = DataFlash.GetFileNumber();
-			bottom_page_filepage = DataFlash.GetFilePage();
-            if (bottom_page_file == last_bottom_page_file &&
-                bottom_page_filepage == 0) {
-                /* no progress can be made - give up. The log may be corrupt */
-                return -1;
-            }
-		} while (bottom_page_file != log_number && bottom_page != 1);
-
-		// Deal with stepping down too far due to overwriting a file
-		while((top_page - bottom_page) > 1) {
-			look_page = (top_page + bottom_page) / 2;
-			DataFlash.StartRead(look_page);
-			if(DataFlash.GetFileNumber() < log_number)
-				top_page = look_page;
-			else
+			
+		} else if (look_page_file == log_number && look_page_file ==bottom_page_file) {
+			if (bottom_page_filepage < look_page_filepage) {
 				bottom_page = look_page;
+				bottom_page_file = look_page_file;
+				bottom_page_filepage = look_page_filepage;
+			} else {
+				top_page = look_page;
+				top_page_file = look_page_file;
+				top_page_filepage = look_page_filepage;
+			}
+			
+		} else if (look_page_file == log_number) {
+			bottom_page = look_page;
+			bottom_page_file = look_page_file;
+			bottom_page_filepage = look_page_filepage;
+			
+		} else if(look_page_file < log_number && bottom_page_file > look_page_file && bottom_page_file <= log_number) {
+			top_page = look_page;
+			top_page_file = look_page_file;
+			top_page_filepage = look_page_filepage;
+		} else if(look_page_file < log_number) {
+			bottom_page = look_page;
+			bottom_page_file = look_page_file;
+			bottom_page_filepage = look_page_filepage;
+			
+		} else if(look_page_file > log_number && top_page_file < look_page_file && top_page_file >= log_number) {
+			bottom_page = look_page;
+			bottom_page_file = look_page_file;
+			bottom_page_filepage = look_page_filepage;
+		} else {
+			top_page = look_page;
+			top_page_file = look_page_file;
+			top_page_filepage = look_page_filepage;
 		}
-
-		// The -1 return is for the case where a very short power up increments the log
-		// number counter but no log file is actually created.  This happens if power is
-		// removed before the first page is written to flash.
-		if(bottom_page ==1 && DataFlash.GetFileNumber() != log_number) return -1;
-
-		 return bottom_page;
+		
+	// End while
 	}
+	
+	if (bottom_page_file == log_number && top_page_file == log_number) {
+		if( bottom_page_filepage < top_page_filepage)
+			return top_page;
+		else
+			return bottom_page;
+	} else if (bottom_page_file == log_number) {
+		return bottom_page;
+	} else if (top_page_file == log_number) {
+		return top_page;
+	} else {
+		return -1;
+	}
+	
 }
+	
+
+
+
 
 
 // Write an attitude packet. Total length : 10 bytes
diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h
index 75a2b76a73..1989f8de8e 100644
--- a/ArduRover/ControllerCar.h
+++ b/ArduRover/ControllerCar.h
@@ -16,7 +16,7 @@ class ControllerCar: public AP_Controller {
 public:
     ControllerCar(AP_Navigator * nav, AP_Guide * guide,
                   AP_HardwareAbstractionLayer * hal) :
-        AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode),
+        AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl),
         pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
                steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
         pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
@@ -84,10 +84,14 @@ private:
         _strCmd = steering;
         _thrustCmd = thrust;
     }
-    void setMotorsActive() {
+    void setMotors() {
         _hal->rc[ch_str]->setPosition(_strCmd);
         _hal->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd);
     }
+    void handleFailsafe() {
+        // turn off
+        setMode(MAV_MODE_LOCKED);
+    }
 
     // attributes
     enum {
diff --git a/ArduRover/ControllerTank.h b/ArduRover/ControllerTank.h
index 0c091d6d70..a72edad6c7 100644
--- a/ArduRover/ControllerTank.h
+++ b/ArduRover/ControllerTank.h
@@ -16,7 +16,7 @@ class ControllerTank: public AP_Controller {
 public:
     ControllerTank(AP_Navigator * nav, AP_Guide * guide,
                    AP_HardwareAbstractionLayer * hal) :
-        AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9),ch_mode),
+        AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9),ch_mode,k_cntrl),
         pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
                steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut),
         pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP,
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4c54fc7836..171b8b0e9f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -86,8 +86,8 @@ macro(add_sketch SKETCH_NAME BOARD PORT)
 	# files
 	set(SKETCH_CPP ${CMAKE_CURRENT_BINARY_DIR}/${SKETCH_NAME}/${SKETCH_NAME}.cpp)
 	set(SKETCH_PDE ${CMAKE_CURRENT_SOURCE_DIR}/${SKETCH_NAME}/${SKETCH_NAME}.pde)
-	message(STATUS "SKETCH_PDE:\n${SKETCH_PDE}")
-	message(STATUS "SKETCH_CPP:\n${SKETCH_CPP}")
+    #message(STATUS "SKETCH_PDE:\n${SKETCH_PDE}")
+    #message(STATUS "SKETCH_CPP:\n${SKETCH_CPP}")
 
 	# settings
 	set(${SKETCH_NAME}_BOARD ${BOARD})
@@ -109,12 +109,12 @@ macro(add_sketch SKETCH_NAME BOARD PORT)
 	string(FIND "${STR1}" "\n" POS2)
 	math(EXPR POS3 "${POS1}+${POS2}")
 	string(SUBSTRING "${FILE}" 0 ${POS3} FILE_HEAD)
-	message(STATUS "FILE_HEAD:\n${FILE_HEAD}")
+    #message(STATUS "FILE_HEAD:\n${FILE_HEAD}")
 
 	# find the body of the main pde
 	math(EXPR BODY_LENGTH "${FILE_LENGTH}-${POS3}-1")
 	string(SUBSTRING "${FILE}" "${POS3}+1" "${BODY_LENGTH}" FILE_BODY)
-	message(STATUS "BODY:\n${FILE_BODY}")
+    #message(STATUS "BODY:\n${FILE_BODY}")
 
 	# write the file head
 	file(APPEND ${SKETCH_CPP} "${FILE_HEAD}")
@@ -122,11 +122,11 @@ macro(add_sketch SKETCH_NAME BOARD PORT)
 
 	# write prototypes
 	foreach(PDE ${PDE_SOURCES})
-		message(STATUS "pde: ${PDE}")
+        #message(STATUS "pde: ${PDE}")
 		file(READ ${PDE} FILE)
 		string(REGEX MATCHALL "[\n]([a-zA-Z]+[ ])*[_a-zA-Z0-9]+([ ]*[\n][\t]*|[ ])[_a-zA-Z0-9]+[ ]?[\n]?[\t]*[ ]*[(]([\t]*[ ]*[*]?[ ]?[a-zA-Z0-9_][,]?[ ]*[\n]?)*[)]" PROTOTYPES ${FILE})
 		foreach(PROTOTYPE ${PROTOTYPES})	
-			message(STATUS "\tprototype: ${PROTOTYPE};")
+            #message(STATUS "\tprototype: ${PROTOTYPE};")
 			file(APPEND ${SKETCH_CPP} "${PROTOTYPE};")
 		endforeach()
 	endforeach()
diff --git a/README.txt b/README.txt
index 50d0d38478..c72fe9aa77 100644
--- a/README.txt
+++ b/README.txt
@@ -14,9 +14,17 @@ Building using cmake
 -----------------------------------------------
  - mkdir build
  - cd build
- - cmake ..
+ - cmake .. -DBOARD=mega -PORT=/dev/ttyUSB0
+    You can select from mega/mega2560.
+    If you have arduino installed in a non-standard location you by specify it by using:
+        -DARDUINO_SDK_PATH=/path/to/arduino ..
  - make (will build every sketch)
  - make ArduPlane (will build just ArduPlane etc.)
+ - make ArduPloat-upload (will upload the sketch)
+
+    If you have a sync error during upload reset the board/power cycle the board
+    before the upload starts.
+
  
 Building using eclipse
 -----------------------------------------------
@@ -36,9 +44,13 @@ Building using eclipse
         Note: Unix can be substituted for MinGW/ MSYS/ NMake (for windows)
             (see http://www.vtk.org/Wiki/Eclipse_CDT4_Generator)
 
-        PORT is the port for uploading to the board, COM0 etc on windows.
-        BOARD is your board type, mega for the 1280 or mega2560 for the 2560 boards.
-    
+        input options:
+
+            CMAKE_BUILD_TYPE choose from DEBUG, RELEASE etc.
+            PORT is the port for uploading to the board, COM0 etc on windows. /dev/ttyUSB0 etc. on linux
+            BOARD is your board type, mega for the 1280 or mega2560 for the 2560 boards.
+            ARDUINO_SDK_PATH if it is not in default path can specify as /path/to/arduino
+        
     Importing the Eclipse Build Project:
 
         Import project using Menu File->Import
diff --git a/Tools/ArduPPM/ATMega328p/Encoder-PPM.c b/Tools/ArduPPM/ATMega328p/Encoder-PPM.c
index 1d8159e57e..630052061f 100644
--- a/Tools/ArduPPM/ATMega328p/Encoder-PPM.c
+++ b/Tools/ArduPPM/ATMega328p/Encoder-PPM.c
@@ -1,5 +1,5 @@
 // ------------------------------------------------------------------------------------------------------------------------------------------------------------
-// ArduPPM Version v0.9.86
+// ArduPPM Version v0.9.87
 // ------------------------------------------------------------------------------------------------------------------------------------------------------------
 // ARDUCOPTER 2 : PPM ENCODER for AT Mega 328p and APM v1.4 Boards
 // By:John Arne Birkeland - 2011
@@ -34,6 +34,7 @@
 //	0.9.84 : Corrected pin and port names in Encoder-PPM.c according to #define for Mega32U2 compatibility
 //	0.9.85 : Added brownout reset detection flag
 //	0.9.86 : Added a #define to disable Radio Passthrough mode (hardware failsafe for Arduplane)
+//	0.9.87 : #define correction for radio passthrough (was screwed up).
 // ------------------------------------------------------------------------------------------------------------------------------------------------------------
 // PREPROCESSOR DIRECTIVES
 // ------------------------------------------------------------------------------------------------------------------------------------------------------------
@@ -46,7 +47,7 @@
 #define ERROR_DETECTION_WINDOW	3000 * LOOP_TIMER_10MS			// Detection window for error detection (default to 30s)
 #define	ERROR_CONDITION_DELAY	500 * LOOP_TIMER_10MS			// Servo error condition LED delay (LED blinking duration)
 
-#define PASSTHROUGH_MODE		ENABLED	// Set it to "DISABLED" to remove radio passthrough mode (hardware failsafe for Arduplane)
+#define PASSTHROUGH_MODE_ENABLED	// Comment this line to remove CH8 radio passthrough mode support (hardware failsafe for Arduplane)
 #define PASSTHROUGH_CHANNEL		8 * 2	// Channel for passthrough mode selection
 #define PASSTHROUGH_CHANNEL_OFF_US		ONE_US * 1600 - PPM_PRE_PULSE	// Passthrough off threshold
 #define PASSTHROUGH_CHANNEL_ON_US		ONE_US * 1800 - PPM_PRE_PULSE	// Passthrough on threshold
@@ -97,7 +98,7 @@ int main(void)
 	static uint16_t servo_error_condition_timer=0; 		// Servo error condition timer
 	static uint16_t blink_led_timer = 0; 		// Blink led timer
 	
-	#if PASSTHROUGH_MODE ==	ENABLED
+	#ifdef PASSTHROUGH_MODE_ENABLED
 	static uint8_t mux_timer = 0;				// Mux timer
 	static uint8_t mux_counter = 0;				// Mux counter
 	static int8_t mux_check = 0;
@@ -314,7 +315,7 @@ int main(void)
 	PWM_LOOP: // SERVO_PWM_MODE
 	while( 1 )
 	{
-		#if PASSTHROUGH_MODE ==	ENABLED
+		#ifdef PASSTHROUGH_MODE_ENABLED
 		// ------------------------------------------------------------------------------
 		// Radio passthrough control (mux chip A/B control)
 		// ------------------------------------------------------------------------------
diff --git a/Tools/ArduPPM/ATMega328p/manual.txt b/Tools/ArduPPM/ATMega328p/manual.txt
index b4fb772833..0e7d851b89 100644
--- a/Tools/ArduPPM/ATMega328p/manual.txt
+++ b/Tools/ArduPPM/ATMega328p/manual.txt
@@ -1,7 +1,7 @@
 
 Arducoder is the new 8 channels ppm encoder code for ArduPilot Mega / Arducopter boards.
 
-It is compatible with APM v1.x board (ATMEGA 328p), Phonedrone and futur boards using ATmega32u2
+It is compatible with APM v1.x board (ATMEGA 328p), Phonedrone (using ATmega32u2) and futur boards.
 
 Emphasis has been put on code simplicity and reliability.
 
@@ -13,18 +13,19 @@ Emphasis has been put on code simplicity and reliability.
 --------------------------------------------------------------------------------------------------
 
 --------------------------------------------------
-	Warning - Warning - Warning - Warning
+	Warning
 --------------------------------------------------
 
 
-Code has not yet been extensively tested in the field.
+Code has not yet been massively tested in the field.
+
+Nevertheless extensive tests have been done in the lab with a limited set of different receivers, and a limited number of users did report very sucessfull results.
 
-Nevertheless extensive tests have been done in the lab with a limited set of different receivers.
 
 Carefully check that your radio is working perfectly before you fly.
 
 
-If you see the blue status LED blinking blinking very fast, this is an indication that something is wrong in the decoding.
+If you see the blue status LED blinking very fast very often, this is an indication that something is wrong in the decoding. Rare decoding errors do not hurt.
 
 
 If you have problems, please report the problem and what brand/modell receiver you are using.
@@ -44,10 +45,12 @@ Blue status LED is used for status reports :
 
 
 ------------------------------------------
-	Passthrough mode (mux)
+	Radio Passthrough mode (mux) 
 ------------------------------------------
 
-Passthrough mode is trigged by channel 8 > 1800 us.
+This mode is described as hardware failsafe in Arduplane terminology.
+
+Radio Passthrough mode is trigged by channel 8 > 1800 us. 
 
 Blue status LED has different behavior in passthrough mode :
 
@@ -64,7 +67,9 @@ If all contact with the receiver is lost, an internal failsafe is trigged after
 
 Default failsafe values are :
 
-Throttle channel low (channel 3 = 1000 us)
+Throttle channel low (channel 3 = 900 us)
+
+Mode Channel set to flight mode 4 (channel 5 = 1555 us)
 
 All other channels set to midstick (1500 us)
 
diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h
index 368a323020..f6e17a46b0 100644
--- a/apo/ControllerPlane.h
+++ b/apo/ControllerPlane.h
@@ -16,7 +16,7 @@ class ControllerPlane: public AP_Controller {
 public:
     ControllerPlane(AP_Navigator * nav, AP_Guide * guide,
                     AP_HardwareAbstractionLayer * hal) :
-        AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode),
+        AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode,k_cntrl),
         _trimGroup(k_trim, PSTR("trim_")),
         _rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")),
         _needsTrim(false),
@@ -100,7 +100,7 @@ private:
         _rudder += _rdrTrim;
         _throttle += _thrTrim;
     }
-    void setMotorsActive() {
+    void setMotors() {
         // turn all motors off if below 0.1 throttle
         if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
             setAllRadioChannelsToNeutral();
@@ -111,6 +111,12 @@ private:
             _hal->rc[ch_thrust]->setPosition(_throttle);
         }
     }
+    void handleFailsafe() {
+        // note if testing and communication is lost the motors will not shut off,
+        // it will attempt to land
+        _guide->setMode(MAV_NAV_LANDING);
+        setMode(MAV_MODE_AUTO);
+    }
 
     // attributes
     enum {
diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h
index 920c878abe..3a920464a3 100644
--- a/apo/ControllerQuad.h
+++ b/apo/ControllerQuad.h
@@ -18,7 +18,7 @@ class ControllerQuad: public AP_Controller {
 public:
     ControllerQuad(AP_Navigator * nav, AP_Guide * guide,
                    AP_HardwareAbstractionLayer * hal) :
-        AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode),
+        AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode, k_cntrl),
         pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1,
                 PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU,
                 PID_ATT_LIM, PID_ATT_DFCUT),
@@ -31,12 +31,10 @@ public:
         pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1,
                    PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D,
                    PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT),
-        pidPN(new AP_Var_group(k_pidPN, PSTR("NORTH_")), 1, PID_POS_P,
-              PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
-        pidPE(new AP_Var_group(k_pidPE, PSTR("EAST_")), 1, PID_POS_P,
-              PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
+        pidTilt(new AP_Var_group(k_pidTilt, PSTR("TILT_")), 1, PID_TILT_P,
+              PID_TILT_I, PID_TILT_D, PID_TILT_AWU, PID_TILT_LIM, PID_TILT_DFCUT),
         pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P,
-              PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_DFCUT),
+              PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_Z_DFCUT),
         _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
         _cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) {
         _hal->debug->println_P(PSTR("initializing quad controller"));
@@ -88,27 +86,18 @@ private:
     void autoLoop(const float dt) {
         autoPositionLoop(dt);
         autoAttitudeLoop(dt);
-
-        // XXX currently auto loop not tested, so
-        // put vehicle in standby
-        _hal->setState(MAV_STATE_STANDBY);
     }
     void autoPositionLoop(float dt) {
-        float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt);
-        float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt);
-        float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt);
+        // XXX need to add waypoint coordinates
+        //float cmdNorthTilt = pidPN.update(_guide->getPNError(),_nav->getVN(),dt);
+        //float cmdEastTilt = pidPE.update(_guide->getPEError(),_nav->getVE(),dt);
+        float cmdTilt = pidTilt.update(_guide->getDistanceToNextWaypoint(),dt);
+        float cmdDown = pidPD.update(_guide->getPDError(),_nav->getVD(),dt);
 
-        // "transform-to-body"
-        {
-            float trigSin = sin(-_nav->getYaw());
-            float trigCos = cos(-_nav->getYaw());
-            _cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin;
-            _cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos;
-            // note that the north tilt is negative of the pitch
-        }
-        _cmdYawRate = 0;
-
-        _thrustMix = THRUST_HOVER_OFFSET + cmdDown;
+        _cmdPitch = -cmdTilt*cos(_guide->getHeadingError());
+        _cmdRoll = cmdTilt*sin(_guide->getHeadingError());
+        _cmdYawRate = pidYaw.update(_guide->getHeadingError(),_nav->getYawRate(),dt); // always points to next waypoint
+        _thrustMix = THRUST_HOVER_OFFSET - cmdDown;
 
         // "thrust-trim-adjust"
         if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393;
@@ -116,6 +105,9 @@ private:
 
         if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393;
         else _thrustMix /= cos(_cmdPitch);
+
+        // debug for position loop
+        _hal->debug->printf_P(PSTR("cmd: tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdTilt,cmdDown,_cmdPitch,_cmdRoll);
     }
     void autoAttitudeLoop(float dt) {
         _rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(),
@@ -124,7 +116,7 @@ private:
                                     _nav->getPitchRate(), dt);
         _yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt);
     }
-    void setMotorsActive() {
+    void setMotors() {
         // turn all motors off if below 0.1 throttle
         if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
             setAllRadioChannelsToNeutral();
@@ -136,6 +128,11 @@ private:
         }
     }
 
+    void handleFailsafe() {
+        // turn off
+        setMode(MAV_MODE_LOCKED);
+    }
+
     // attributes
     /**
      * note that these are not the controller radio channel numbers, they are just
@@ -167,8 +164,7 @@ private:
     enum {
         k_pidGroundSpeed2Throttle = k_controllersStart,
         k_pidStr,
-        k_pidPN,
-        k_pidPE,
+        k_pidTilt,
         k_pidPD,
         k_pidRoll,
         k_pidPitch,
@@ -177,7 +173,8 @@ private:
     };
     BlockPIDDfb pidRoll, pidPitch, pidYaw;
     BlockPID pidYawRate;
-    BlockPIDDfb pidPN, pidPE, pidPD;
+    BlockPID pidTilt;
+    BlockPIDDfb pidPD;
     float _thrustMix, _pitchMix, _rollMix, _yawMix;
     float _cmdRoll, _cmdPitch, _cmdYawRate;
 };
diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h
index 670aa18e05..cda3666c84 100644
--- a/apo/QuadArducopter.h
+++ b/apo/QuadArducopter.h
@@ -10,9 +10,9 @@
 
 // vehicle options
 static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
-static const apo::halMode_t halMode = apo::MODE_LIVE;
+static const apo::halMode_t halMode = apo::MODE_HIL_CNTL;
 static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
-static const uint8_t heartBeatTimeout = 3;
+static const uint8_t heartBeatTimeout = 0;
 
 // algorithm selection
 #define CONTROLLER_CLASS ControllerQuad
@@ -30,7 +30,7 @@ static const uint8_t heartBeatTimeout = 3;
 static const uint32_t debugBaud = 57600;
 static const uint32_t telemBaud = 57600;
 static const uint32_t gpsBaud = 38400;
-static const uint32_t hilBaud = 57600;
+static const uint32_t hilBaud = 115200;
 
 // optional sensors
 static const bool gpsEnabled = false;
@@ -61,17 +61,18 @@ static const float loop2Rate = 1; 	// gcs slow
 static const float loop3Rate = 0.1;
 
 // position control loop
-static const float PID_POS_P = 0;
-static const float PID_POS_I = 0;
-static const float PID_POS_D = 0;
-static const float PID_POS_LIM = 0; // about 5 deg
-static const float PID_POS_AWU = 0; // about 5 deg
-static const float PID_POS_Z_P = 0;
+static const float PID_TILT_P = 0.1;
+static const float PID_TILT_I = 0;
+static const float PID_TILT_D = 0.1;
+static const float PID_TILT_LIM = 0.04; // about 2 deg
+static const float PID_TILT_AWU = 0.02; // about 1 deg
+static const float PID_TILT_DFCUT = 10; // cut derivative feedback at 10 hz
+static const float PID_POS_Z_P = 0.1;
 static const float PID_POS_Z_I = 0;
-static const float PID_POS_Z_D = 0;
-static const float PID_POS_Z_LIM = 0;
+static const float PID_POS_Z_D = 0.2;
+static const float PID_POS_Z_LIM = 0.1;
 static const float PID_POS_Z_AWU = 0;
-static const float PID_POS_DFCUT = 10; // cut derivative feedback at 10 hz
+static const float PID_POS_Z_DFCUT = 10; // cut derivative feedback at 10 hz
 
 // attitude control loop
 static const float PID_ATT_P = 0.17;
diff --git a/apo/apo.pde b/apo/apo.pde
index a39d6e9f7f..4abd88c96e 100644
--- a/apo/apo.pde
+++ b/apo/apo.pde
@@ -8,7 +8,6 @@
 #include <AP_ADC.h>
 #include <AP_DCM.h>
 #include <AP_Compass.h>
-#include <Wire.h>
 #include <AP_GPS.h>
 #include <AP_IMU.h>
 #include <APM_BMP085.h>
@@ -16,8 +15,8 @@
 #include <APO.h>
 
 // Vehicle Configuration
-//#include "QuadArducopter.h"
-#include "PlaneEasystar.h"
+#include "QuadArducopter.h"
+//#include "PlaneEasystar.h"
 
 // ArduPilotOne Default Setup
 #include "APO_DefaultSetup.h"
diff --git a/cmake/modules/FindArduino.cmake b/cmake/modules/FindArduino.cmake
index e53873e9ad..dc2b18659a 100644
--- a/cmake/modules/FindArduino.cmake
+++ b/cmake/modules/FindArduino.cmake
@@ -280,7 +280,10 @@ function(setup_arduino_core VAR_NAME BOARD_ID)
     if(BOARD_CORE AND NOT TARGET ${CORE_LIB_NAME})
         set(BOARD_CORE_PATH ${ARDUINO_CORES_PATH}/${BOARD_CORE})
         find_sources(CORE_SRCS ${BOARD_CORE_PATH} True)
-		list(REMOVE_ITEM CORE_SRCS "${BOARD_CORE_PATH}/main.cxx")
+
+        # Debian/Ubuntu fix
+        list(REMOVE_ITEM CORE_SRCS "${BOARD_CORE_PATH}/main.cxx")
+
         add_library(${CORE_LIB_NAME} ${CORE_SRCS})
         set(${VAR_NAME} ${CORE_LIB_NAME} PARENT_SCOPE)
     endif()
@@ -317,7 +320,7 @@ function(find_arduino_libraries VAR_NAME SRCS)
                 get_property(LIBRARY_SEARCH_PATH
                              DIRECTORY     # Property Scope
                              PROPERTY LINK_DIRECTORIES)
-						 foreach(LIB_SEARCH_PATH ${LIBRARY_SEARCH_PATH} ${ARDUINO_LIBRARIES_PATH} ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/libraries)
+                foreach(LIB_SEARCH_PATH ${LIBRARY_SEARCH_PATH} ${ARDUINO_LIBRARIES_PATH} ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/libraries)
                     if(EXISTS ${LIB_SEARCH_PATH}/${INCLUDE_NAME}/${CMAKE_MATCH_1})
                         list(APPEND ARDUINO_LIBS ${LIB_SEARCH_PATH}/${INCLUDE_NAME})
                         break()
@@ -340,23 +343,27 @@ endfunction()
 #
 # Creates an Arduino library, with all it's library dependencies.
 #
-#		"LIB_NAME"_RECURSE controls if the library will recurse
-# 		when looking for source files
+#      ${LIB_NAME}_RECURSE controls if the library will recurse
+#      when looking for source files.
 #
 
 # For known libraries can list recurse here
 set(Wire_RECURSE True)
+set(Ethernet_RECURSE True)
 function(setup_arduino_library VAR_NAME BOARD_ID LIB_PATH)
     set(LIB_TARGETS)
+
     get_filename_component(LIB_NAME ${LIB_PATH} NAME)
     set(TARGET_LIB_NAME ${BOARD_ID}_${LIB_NAME})
     if(NOT TARGET ${TARGET_LIB_NAME})
-		string(REGEX REPLACE ".*/" "" LIB_SHORT_NAME ${LIB_NAME})
-		#message(STATUS "short name: ${LIB_SHORT_NAME} recures: ${${LIB_SHORT_NAME}_RECURSE}")
-		if (NOT DEFINED ${LIB_SHORT_NAME}_RECURSE)
-			set(${LIB_SHORT_NAME}_RECURSE False)
-		endif()
-		find_sources(LIB_SRCS ${LIB_PATH} ${${LIB_SHORT_NAME}_RECURSE})
+        string(REGEX REPLACE ".*/" "" LIB_SHORT_NAME ${LIB_NAME})
+
+        # Detect if recursion is needed
+        if (NOT DEFINED ${LIB_SHORT_NAME}_RECURSE)
+            set(${LIB_SHORT_NAME}_RECURSE False)
+        endif()
+
+        find_sources(LIB_SRCS ${LIB_PATH} ${${LIB_SHORT_NAME}_RECURSE})
         if(LIB_SRCS)
 
             message(STATUS "Generating Arduino ${LIB_NAME} library")
@@ -393,7 +400,7 @@ endfunction()
 #
 function(setup_arduino_libraries VAR_NAME BOARD_ID SRCS)
     set(LIB_TARGETS)
-    find_arduino_libraries(TARGET_LIBS ${SRCS})
+    find_arduino_libraries(TARGET_LIBS "${SRCS}")
     foreach(TARGET_LIB ${TARGET_LIBS})
         setup_arduino_library(LIB_DEPS ${BOARD_ID} ${TARGET_LIB}) # Create static library instead of returning sources
         list(APPEND LIB_TARGETS ${LIB_DEPS})
@@ -443,16 +450,19 @@ function(setup_arduino_upload BOARD_ID TARGET_NAME PORT)
     if(DEFINED ${TARGET_NAME}_AFLAGS)
         set(AVRDUDE_FLAGS ${${TARGET_NAME}_AFLAGS})
     endif()
+    if (${${BOARD_ID}.upload.protocol} STREQUAL "stk500") 
+        set(${BOARD_ID}.upload.protocol "stk500v1")
+    endif()
     add_custom_target(${TARGET_NAME}-upload
-                     ${ARDUINO_AVRDUDE_PROGRAM} 
-                         -U flash:w:${TARGET_NAME}.hex
-                         ${AVRDUDE_FLAGS}
-                         -C ${ARDUINO_AVRDUDE_CONFIG_PATH}
-                         -p ${${BOARD_ID}.build.mcu}
-                         -c ${${BOARD_ID}.upload.protocol}
-                         -b ${${BOARD_ID}.upload.speed}
-                         -P ${PORT}
-                     DEPENDS ${TARGET_NAME})
+                     ${ARDUINO_AVRDUDE_PROGRAM}
+                        ${AVRDUDE_FLAGS}
+                         -C${ARDUINO_AVRDUDE_CONFIG_PATH}
+                         -p${${BOARD_ID}.build.mcu} 
+                         -c${${BOARD_ID}.upload.protocol} 
+                         -P${PORT} -b${${BOARD_ID}.upload.speed}
+                         #-D
+                         -Uflash:w:${CMAKE_BINARY_DIR}/${TARGET_NAME}.hex:i
+                         DEPENDS ${TARGET_NAME})
     if(NOT TARGET upload)
         add_custom_target(upload)
     endif()
@@ -468,20 +478,21 @@ endfunction()
 # Finds all C/C++ sources located at the specified path.
 #
 function(find_sources VAR_NAME LIB_PATH RECURSE)
-	set(FILE_SEARCH_LIST
-		${LIB_PATH}/*.cpp
-		${LIB_PATH}/*.c
-		${LIB_PATH}/*.cc
-		${LIB_PATH}/*.cxx
-		${LIB_PATH}/*.h
-		${LIB_PATH}/*.hh
-		${LIB_PATH}/*.hxx)
-	if (RECURSE)
-		file(GLOB_RECURSE LIB_FILES ${FILE_SEARCH_LIST})
-	else()
-		file(GLOB LIB_FILES ${FILE_SEARCH_LIST})
-	endif()
-	#message(STATUS "${LIB_PATH} recurse: ${RECURSE}")
+    set(FILE_SEARCH_LIST
+        ${LIB_PATH}/*.cpp
+        ${LIB_PATH}/*.c
+        ${LIB_PATH}/*.cc
+        ${LIB_PATH}/*.cxx
+        ${LIB_PATH}/*.h
+        ${LIB_PATH}/*.hh
+        ${LIB_PATH}/*.hxx)
+
+    if(RECURSE)
+        file(GLOB_RECURSE LIB_FILES ${FILE_SEARCH_LIST})
+    else()
+        file(GLOB LIB_FILES ${FILE_SEARCH_LIST})
+    endif()
+
     if(LIB_FILES)
         set(${VAR_NAME} ${LIB_FILES} PARENT_SCOPE)
     endif()
@@ -522,75 +533,79 @@ endfunction()
 
 
 # Setting up Arduino enviroment settings
-if(NOT ARDUINO_FOUND)
-    find_file(ARDUINO_CORES_PATH
-              NAMES cores
-              PATHS ${ARDUINO_SDK_PATH}
-              PATH_SUFFIXES hardware/arduino)
+find_file(ARDUINO_CORES_PATH
+    NAMES cores
+    PATHS ${ARDUINO_SDK_PATH}
+    PATH_SUFFIXES hardware/arduino
+    NO_DEFAULT_PATH) 
 
-    find_file(ARDUINO_LIBRARIES_PATH
-              NAMES libraries
-              PATHS ${ARDUINO_SDK_PATH})
+find_file(ARDUINO_LIBRARIES_PATH
+    NAMES libraries
+    PATHS ${ARDUINO_SDK_PATH}
+    NO_DEFAULT_PATH) 
 
-    find_file(ARDUINO_BOARDS_PATH
-              NAMES boards.txt
-              PATHS ${ARDUINO_SDK_PATH}
-              PATH_SUFFIXES hardware/arduino)
+find_file(ARDUINO_BOARDS_PATH
+    NAMES boards.txt
+    PATHS ${ARDUINO_SDK_PATH}
+    PATH_SUFFIXES hardware/arduino
+    NO_DEFAULT_PATH) 
 
-    find_file(ARDUINO_PROGRAMMERS_PATH
-              NAMES programmers.txt
-              PATHS ${ARDUINO_SDK_PATH}
-              PATH_SUFFIXES hardware/arduino)
+find_file(ARDUINO_PROGRAMMERS_PATH
+    NAMES programmers.txt
+    PATHS ${ARDUINO_SDK_PATH}
+    PATH_SUFFIXES hardware/arduino
+    NO_DEFAULT_PATH) 
 
-    find_file(ARDUINO_REVISIONS_PATH
-              NAMES revisions.txt
-              PATHS ${ARDUINO_SDK_PATH})
+find_file(ARDUINO_REVISIONS_PATH
+    NAMES revisions.txt
+    PATHS ${ARDUINO_SDK_PATH}
+    NO_DEFAULT_PATH) 
 
-    find_file(ARDUINO_VERSION_PATH
-              NAMES lib/version.txt
-              PATHS ${ARDUINO_SDK_PATH})
+find_file(ARDUINO_VERSION_PATH
+    NAMES lib/version.txt
+    PATHS ${ARDUINO_SDK_PATH}
+    NO_DEFAULT_PATH) 
 
-    find_program(ARDUINO_AVRDUDE_PROGRAM
-                 NAMES avrdude
-                 PATHS ${ARDUINO_SDK_PATH}
-                 PATH_SUFFIXES hardware/tools)
+find_program(ARDUINO_AVRDUDE_PROGRAM
+    NAMES avrdude
+    PATHS ${ARDUINO_SDK_PATH}
+    PATH_SUFFIXES hardware/tools
+    NO_DEFAULT_PATH) 
 
-    find_program(ARDUINO_AVRDUDE_CONFIG_PATH
-                 NAMES avrdude.conf
-                 PATHS ${ARDUINO_SDK_PATH} /etc/avrdude
-                 PATH_SUFFIXES hardware/tools
-                               hardware/tools/avr/etc)
+find_file(ARDUINO_AVRDUDE_CONFIG_PATH
+    NAMES avrdude.conf
+    PATHS ${ARDUINO_SDK_PATH} /etc/avrdude
+    PATH_SUFFIXES hardware/tools
+                  hardware/tools/avr/etc
+    NO_DEFAULT_PATH)
 
-     set(ARDUINO_OBJCOPY_EEP_FLAGS -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0
-         CACHE STRING "")
-     set(ARDUINO_OBJCOPY_HEX_FLAGS -O ihex -R .eeprom
-         CACHE STRING "")
-     set(ARDUINO_AVRDUDE_FLAGS -V -F
-         CACHE STRING "Arvdude global flag list.")
+set(ARDUINO_OBJCOPY_EEP_FLAGS -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0
+    CACHE STRING "")
+set(ARDUINO_OBJCOPY_HEX_FLAGS -O ihex -R .eeprom
+    CACHE STRING "")
+set(ARDUINO_AVRDUDE_FLAGS -V -F
+    CACHE STRING "Arvdude global flag list.")
 
-     if(ARDUINO_SDK_PATH)
-         detect_arduino_version(ARDUINO_SDK_VERSION)
-         set(ARDUINO_SDK_VERSION ${ARDUINO_SDK_VERSION} CACHE STRING "Arduino SDK Version")
-     endif(ARDUINO_SDK_PATH)
+if(ARDUINO_SDK_PATH)
+    detect_arduino_version(ARDUINO_SDK_VERSION)
+    set(ARDUINO_SDK_VERSION ${ARDUINO_SDK_VERSION} CACHE STRING "Arduino SDK Version")
+endif(ARDUINO_SDK_PATH)
 
-    include(FindPackageHandleStandardArgs)
-    find_package_handle_standard_args(Arduino
-                                      REQUIRED_VARS ARDUINO_SDK_PATH
-                                                    ARDUINO_SDK_VERSION
-                                      VERSION_VAR ARDUINO_SDK_VERSION)
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Arduino
+                                  REQUIRED_VARS ARDUINO_SDK_PATH
+                                                ARDUINO_SDK_VERSION
+                                  VERSION_VAR ARDUINO_SDK_VERSION)
 
 
-     mark_as_advanced(ARDUINO_CORES_PATH
-                      ARDUINO_SDK_VERSION
-                      ARDUINO_LIBRARIES_PATH
-                      ARDUINO_BOARDS_PATH
-                      ARDUINO_PROGRAMMERS_PATH
-                      ARDUINO_REVISIONS_PATH
-                      ARDUINO_AVRDUDE_PROGRAM
-                      ARDUINO_AVRDUDE_CONFIG_PATH
-                      ARDUINO_OBJCOPY_EEP_FLAGS
-                      ARDUINO_OBJCOPY_HEX_FLAGS)
-    load_board_settings()
-
-endif()
-
+mark_as_advanced(ARDUINO_CORES_PATH
+    ARDUINO_SDK_VERSION
+    ARDUINO_LIBRARIES_PATH
+    ARDUINO_BOARDS_PATH
+    ARDUINO_PROGRAMMERS_PATH
+    ARDUINO_REVISIONS_PATH
+    ARDUINO_AVRDUDE_PROGRAM
+    ARDUINO_AVRDUDE_CONFIG_PATH
+    ARDUINO_OBJCOPY_EEP_FLAGS
+    ARDUINO_OBJCOPY_HEX_FLAGS)
+load_board_settings()
diff --git a/cmake/toolchains/Arduino.cmake b/cmake/toolchains/Arduino.cmake
index 68c58a72e9..897ea4fa0c 100644
--- a/cmake/toolchains/Arduino.cmake
+++ b/cmake/toolchains/Arduino.cmake
@@ -7,7 +7,7 @@ set(CMAKE_CXX_COMPILER avr-g++)
 #                              C Flags                                        #
 #=============================================================================#
 if (NOT DEFINED ARDUINO_C_FLAGS)
-	set(ARDUINO_C_FLAGS "-ffunction-sections -fdata-sections")
+    set(ARDUINO_C_FLAGS "-ffunction-sections -fdata-sections")
 endif()
 set(CMAKE_C_FLAGS                "-g -Os       ${ARDUINO_C_FLAGS}" CACHE STRING "")
 set(CMAKE_C_FLAGS_DEBUG          "-g           ${ARDUINO_C_FLAGS}" CACHE STRING "")
@@ -19,7 +19,7 @@ set(CMAKE_C_FLAGS_RELWITHDEBINFO "-0s -g       -w ${ARDUINO_C_FLAGS}" CACHE STRI
 #                             C++ Flags                                       #
 #=============================================================================#
 if (NOT DEFINED ARDUINO_CXX_FLAGS)
-	set(ARDUINO_CXX_FLAGS "${ARDUINO_C_FLAGS} -fno-exceptions")
+    set(ARDUINO_CXX_FLAGS "${ARDUINO_C_FLAGS} -fno-exceptions")
 endif()
 set(CMAKE_CXX_FLAGS                "-g -Os       ${ARDUINO_CXX_FLAGS}" CACHE STRING "")
 set(CMAKE_CXX_FLAGS_DEBUG          "-g           ${ARDUINO_CXX_FLAGS}" CACHE STRING "")
@@ -31,7 +31,7 @@ set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-0s -g       ${ARDUINO_CXX_FLAGS}" CACHE STR
 #                       Executable Linker Flags                               #
 #=============================================================================#
 if (NOT DEFINED ARDUINO_LINKER_FLAGS)
-	set(ARDUINO_LINKER_FLAGS "-Wl,--gc-sections")
+    set(ARDUINO_LINKER_FLAGS "-Wl,--gc-sections")
 endif()
 set(CMAKE_EXE_LINKER_FLAGS                "${ARDUINO_LINKER_FLAGS}" CACHE STRING "")
 set(CMAKE_EXE_LINKER_FLAGS_DEBUG          "${ARDUINO_LINKER_FLAGS}" CACHE STRING "")
diff --git a/cmake/updated-arduino-cmake.sh b/cmake/updated-arduino-cmake.sh
index 3c86d4b728..c8c26b6781 100755
--- a/cmake/updated-arduino-cmake.sh
+++ b/cmake/updated-arduino-cmake.sh
@@ -1,5 +1,5 @@
 #!/bin/bash
-git clone git@github.com:jgoppert/arduino-cmake.git tmp
+git clone git://github.com/jgoppert/arduino-cmake.git tmp
 cp -rf tmp/cmake/modules/* modules
 cp -rf tmp/cmake/toolchains/* toolchains
 rm -rf tmp
diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h
index 360b93dada..3fe10bb203 100644
--- a/libraries/APO/APO.h
+++ b/libraries/APO/APO.h
@@ -11,6 +11,7 @@
 #include "AP_Autopilot.h"
 #include "AP_Guide.h"
 #include "AP_Controller.h"
+#include "AP_ControllerBlock.h"
 #include "AP_HardwareAbstractionLayer.h"
 #include "AP_MavlinkCommand.h"
 #include "AP_Navigator.h"
diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp
index f319a6d91e..6236e590c4 100644
--- a/libraries/APO/AP_ArmingMechanism.cpp
+++ b/libraries/APO/AP_ArmingMechanism.cpp
@@ -3,8 +3,8 @@
  *
  */
 
-
 #include "AP_ArmingMechanism.h"
+#include "AP_Controller.h"
 #include "AP_RcChannel.h"
 #include "../FastSerial/FastSerial.h"
 #include "AP_HardwareAbstractionLayer.h"
@@ -15,7 +15,7 @@ namespace apo {
 void AP_ArmingMechanism::update(const float dt) {
     //_hal->debug->printf_P(PSTR("ch1: %f\tch2: %f\n"),_hal->rc[_ch1]->getRadioPosition(), _hal->rc[_ch2]->getRadioPosition());
     // arming
-    if ( (_hal->getState() != MAV_STATE_ACTIVE) &&
+    if ( (_controller->getState() != MAV_STATE_ACTIVE) &&
             (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
             (_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) {
 
@@ -23,14 +23,13 @@ void AP_ArmingMechanism::update(const float dt) {
         if (_armingClock<0) _armingClock = 0;
 
         if (_armingClock++ >= 100) {
-            _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
-            _hal->setState(MAV_STATE_ACTIVE);
+            _controller->setMode(MAV_MODE_READY);
         } else {
             _hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming"));
         }
     }
     // disarming
-    else if ( (_hal->getState() == MAV_STATE_ACTIVE) &&
+    else if ( (_controller->getState() == MAV_STATE_ACTIVE) &&
               (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
               (_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) {
 
@@ -38,8 +37,7 @@ void AP_ArmingMechanism::update(const float dt) {
         if (_armingClock>0) _armingClock = 0;
 
         if (_armingClock-- <= -100) {
-            _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
-            _hal->setState(MAV_STATE_STANDBY);
+            _controller->setMode(MAV_MODE_LOCKED);
         } else {
             _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming"));
         }
@@ -47,8 +45,8 @@ void AP_ArmingMechanism::update(const float dt) {
     // reset arming clock and report status
     else if (_armingClock != 0) {
         _armingClock = 0;
-        if (_hal->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
-        else if (_hal->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
+        if (_controller->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
+        else if (_controller->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
     }
 }
 
diff --git a/libraries/APO/AP_ArmingMechanism.h b/libraries/APO/AP_ArmingMechanism.h
index e747a7831e..8ff3e56105 100644
--- a/libraries/APO/AP_ArmingMechanism.h
+++ b/libraries/APO/AP_ArmingMechanism.h
@@ -12,6 +12,7 @@
 namespace apo {
 
 class AP_HardwareAbstractionLayer;
+class AP_Controller;
 
 class AP_ArmingMechanism {
 
@@ -25,10 +26,10 @@ public:
      * @param ch2Min: disarms below this
      * @param ch2Max: arms above this
      */
-    AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal,
+    AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal, AP_Controller * controller,
                        uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min,
-                       float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2),
-        _ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) {
+                       float ch2Max) : _armingClock(0), _hal(hal), _controller(controller), 
+        _ch1(ch1), _ch2(ch2), _ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) {
     }
 
     /**
@@ -52,6 +53,7 @@ public:
 private:
 
     AP_HardwareAbstractionLayer * _hal;
+    AP_Controller * _controller;
     int8_t _armingClock;
     uint8_t _ch1; /// typically throttle channel
     uint8_t _ch2; /// typically yaw channel
diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp
index 03f13aaa86..858695ad3d 100644
--- a/libraries/APO/AP_Autopilot.cpp
+++ b/libraries/APO/AP_Autopilot.cpp
@@ -37,7 +37,6 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
     _controller(controller), _hal(hal),
     callbackCalls(0) {
 
-    hal->setState(MAV_STATE_BOOT);
     hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
     hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
 
@@ -50,7 +49,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
     /*
      * Calibration
      */
-    hal->setState(MAV_STATE_CALIBRATING);
+    controller->setState(MAV_STATE_CALIBRATING);
     hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
     hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
 
@@ -110,6 +109,8 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
                           AP_MavlinkCommand::home.getLon()*rad2Deg,
                           AP_MavlinkCommand::home.getCommand());
     guide->setCurrentIndex(0);
+    controller->setMode(MAV_MODE_LOCKED);
+    controller->setState(MAV_STATE_STANDBY);
 
     /*
      * Attach loops, stacking for priority
@@ -122,7 +123,6 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
 
     hal->debug->println_P(PSTR("running"));
     hal->gcs->sendText(SEVERITY_LOW, PSTR("running"));
-    hal->setState(MAV_STATE_STANDBY);
 }
 
 void AP_Autopilot::callback(void * data) {
@@ -232,6 +232,7 @@ void AP_Autopilot::callback2(void * data) {
         apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
         apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
         //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION);
+        apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION);
         apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
     }
 
diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp
index a18b91594a..9ccd483ee2 100644
--- a/libraries/APO/AP_CommLink.cpp
+++ b/libraries/APO/AP_CommLink.cpp
@@ -105,11 +105,18 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
         break;
     }
 
+    case MAVLINK_MSG_ID_LOCAL_POSITION: {
+        mavlink_msg_local_position_send(_channel, timeStamp,
+                _navigator->getPN(),_navigator->getPE(), _navigator->getPD(),
+                _navigator->getVN(), _navigator->getVE(), _navigator->getVD());
+        break;
+    }
+
     case MAVLINK_MSG_ID_GPS_RAW: {
         mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(),
                                  _navigator->getLat() * rad2Deg,
                                  _navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0,
-                                 _navigator->getGroundSpeed(),
+                                 _navigator->getGroundSpeed()*1000.0,
                                  _navigator->getYaw() * rad2Deg);
         break;
     }
@@ -124,21 +131,23 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
      */
 
     case MAVLINK_MSG_ID_SCALED_IMU: {
-        /*
-         * accel/gyro debug
-         */
-        /*
-         Vector3f accel = _hal->imu->get_accel();
-         Vector3f gyro = _hal->imu->get_gyro();
-         Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"),
-         accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z);
-         */
-        Vector3f accel = _hal->imu->get_accel();
-        Vector3f gyro = _hal->imu->get_gyro();
-        mavlink_msg_raw_imu_send(_channel, timeStamp, 1000 * accel.x,
-                                 1000 * accel.y, 1000 * accel.z, 1000 * gyro.x,
-                                 1000 * gyro.y, 1000 * gyro.z, _hal->compass->mag_x,
-                                 _hal->compass->mag_y, _hal->compass->mag_z); // XXX THIS IS NOT SCALED FOR MAG
+        int16_t xmag, ymag, zmag;
+        xmag = ymag = zmag = 0;
+        if (_hal->compass) {
+            // XXX THIS IS NOT SCALED
+            xmag = _hal->compass->mag_x;
+            ymag = _hal->compass->mag_y;
+            zmag = _hal->compass->mag_z;
+        }
+        mavlink_msg_scaled_imu_send(_channel, timeStamp,
+            _navigator->getXAccel()*1e3,
+            _navigator->getYAccel()*1e3,
+            _navigator->getZAccel()*1e3,
+            _navigator->getRollRate()*1e3,
+            _navigator->getPitchRate()*1e3,
+            _navigator->getYawRate()*1e3,
+            xmag, ymag, zmag);
+        break;
     }
 
     case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
@@ -174,7 +183,7 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
             batteryVoltage = _hal->batteryMonitor->getVoltage()*1000;
         }
         mavlink_msg_sys_status_send(_channel, _controller->getMode(),
-                                    _guide->getMode(), _hal->getState(), _hal->load * 10,
+                                    _guide->getMode(), _controller->getState(), _hal->load * 10,
                                     batteryVoltage, batteryPercentage, _packetDrops);
         break;
     }
@@ -328,6 +337,29 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
         break;
     }
 
+    case MAVLINK_MSG_ID_HIL_STATE: {
+        // decode
+        mavlink_hil_state_t packet;
+        mavlink_msg_hil_state_decode(msg, &packet);
+        _navigator->setTimeStamp(timeStamp);
+        _navigator->setRoll(packet.roll);
+        _navigator->setPitch(packet.pitch);
+        _navigator->setYaw(packet.yaw);
+        _navigator->setRollRate(packet.rollspeed);
+        _navigator->setPitchRate(packet.pitchspeed);
+        _navigator->setYawRate(packet.yawspeed);
+        _navigator->setVN(packet.vx/ 1e2);
+        _navigator->setVE(packet.vy/ 1e2);
+        _navigator->setVD(packet.vz/ 1e2);
+        _navigator->setLat_degInt(packet.lat);
+        _navigator->setLon_degInt(packet.lon);
+        _navigator->setAlt(packet.alt / 1e3);
+        _navigator->setXAccel(packet.xacc/ 1e3);
+        _navigator->setYAccel(packet.xacc/ 1e3);
+        _navigator->setZAccel(packet.xacc/ 1e3);
+        break; 
+    } 
+
     case MAVLINK_MSG_ID_ATTITUDE: {
         // decode
         mavlink_attitude_t packet;
@@ -364,31 +396,66 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
             AP_Var::save_all();
             break;
 
-        case MAV_ACTION_CALIBRATE_RC:
+        case MAV_ACTION_MOTORS_START:
+            _controller->setMode(MAV_MODE_READY);
+            break;
+
         case MAV_ACTION_CALIBRATE_GYRO:
         case MAV_ACTION_CALIBRATE_MAG:
         case MAV_ACTION_CALIBRATE_ACC:
         case MAV_ACTION_CALIBRATE_PRESSURE:
+            _controller->setMode(MAV_MODE_LOCKED);
+            _navigator->calibrate();
+            break;
+
+        case MAV_ACTION_EMCY_KILL:
+        case MAV_ACTION_CONFIRM_KILL:
+        case MAV_ACTION_MOTORS_STOP:
+        case MAV_ACTION_SHUTDOWN:
+            _controller->setMode(MAV_MODE_LOCKED);
+            break;
+
+        case MAV_ACTION_LAUNCH:
+        case MAV_ACTION_TAKEOFF:
+            _guide->setMode(MAV_NAV_LIFTOFF);
+            break;
+
+        case MAV_ACTION_LAND:
+            _guide->setCurrentIndex(0);
+            _guide->setMode(MAV_NAV_LANDING);
+            break;
+
+        case MAV_ACTION_EMCY_LAND:
+            _guide->setMode(MAV_NAV_LANDING);
+            break;
+
+        case MAV_ACTION_LOITER:
+        case MAV_ACTION_HALT:
+            _guide->setMode(MAV_NAV_LOITER);
+            break;
+
+        case MAV_ACTION_SET_AUTO:
+            _controller->setMode(MAV_MODE_AUTO);
+            break;
+
+        case MAV_ACTION_SET_MANUAL:
+            _controller->setMode(MAV_MODE_MANUAL);
+            break;
+
+        case MAV_ACTION_RETURN:
+            _guide->setMode(MAV_NAV_RETURNING);
+            break;
+
+        case MAV_ACTION_NAVIGATE:
+        case MAV_ACTION_CONTINUE:
+            _guide->setMode(MAV_NAV_WAYPOINT);
+            break;
+
+        case MAV_ACTION_CALIBRATE_RC:
         case MAV_ACTION_REBOOT:
         case MAV_ACTION_REC_START:
         case MAV_ACTION_REC_PAUSE:
         case MAV_ACTION_REC_STOP:
-        case MAV_ACTION_TAKEOFF:
-        case MAV_ACTION_LAND:
-        case MAV_ACTION_NAVIGATE:
-        case MAV_ACTION_LOITER:
-        case MAV_ACTION_MOTORS_START:
-        case MAV_ACTION_CONFIRM_KILL:
-        case MAV_ACTION_EMCY_KILL:
-        case MAV_ACTION_MOTORS_STOP:
-        case MAV_ACTION_SHUTDOWN:
-        case MAV_ACTION_CONTINUE:
-        case MAV_ACTION_SET_MANUAL:
-        case MAV_ACTION_SET_AUTO:
-        case MAV_ACTION_LAUNCH:
-        case MAV_ACTION_RETURN:
-        case MAV_ACTION_EMCY_LAND:
-        case MAV_ACTION_HALT:
             sendText(SEVERITY_LOW, PSTR("action not implemented"));
             break;
         default:
@@ -582,6 +649,14 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
             sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK);
             _receivingCmds = false;
             _guide->setNumberOfCommands(_cmdNumberRequested);
+
+            // make sure curernt waypoint still exists
+            if (_cmdNumberRequested > _guide->getCurrentIndex()) {
+                _guide->setCurrentIndex(0);
+                mavlink_msg_waypoint_current_send(_channel,
+                                          _guide->getCurrentIndex());
+            }
+
             //sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
         } else if (_cmdRequestIndex > _cmdNumberRequested) {
             _receivingCmds = false;
diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp
index 48c6f6e475..54ae93ddbd 100644
--- a/libraries/APO/AP_Controller.cpp
+++ b/libraries/APO/AP_Controller.cpp
@@ -5,16 +5,15 @@
  *      Author: jgoppert
  */
 
-#include "AP_Controller.h"
 #include "../FastSerial/FastSerial.h"
 #include "AP_ArmingMechanism.h"
 #include "AP_BatteryMonitor.h"
 #include "AP_HardwareAbstractionLayer.h"
-#include "../AP_Common/include/menu.h"
 #include "AP_RcChannel.h"
 #include "../GCS_MAVLink/include/mavlink_types.h"
 #include "constants.h"
 #include "AP_CommLink.h"
+#include "AP_Controller.h"
 
 namespace apo {
 
@@ -24,7 +23,7 @@ AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide,
     _nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism),
     _group(key, name ? : PSTR("CNTRL_")),
     _chMode(chMode),
-    _mode(&_group, 1, MAV_MODE_LOCKED, PSTR("MODE")) {
+    _mode(MAV_MODE_LOCKED), _state(MAV_STATE_BOOT) {
     setAllRadioChannelsToNeutral();
 }
 
@@ -45,82 +44,61 @@ void AP_Controller::setAllRadioChannelsManually() {
 void AP_Controller::update(const float dt) {
     if (_armingMechanism) _armingMechanism->update(dt);
 
-    // determine flight mode
+    // handle emergencies
     //
     // check for heartbeat from gcs, if not found go to failsafe
     if (_hal->heartBeatLost()) {
-        _mode = MAV_MODE_FAILSAFE;
+        setMode(MAV_MODE_FAILSAFE);
         _hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
-        // if battery less than 5%, go to failsafe
+        
+    // if battery less than 5%, go to failsafe
     } else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) {
-        _mode = MAV_MODE_FAILSAFE;
+        setMode(MAV_MODE_FAILSAFE);
         _hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
-        // manual/auto switch
-    } else {
-        // if all emergencies cleared, fall back to standby
-        if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY);
-        if (_hal->rc[_chMode]->getRadioPosition() > 0) _mode = MAV_MODE_MANUAL;
-        else _mode = MAV_MODE_AUTO;
     }
 
-    // handle flight modes
-    switch(_mode) {
-
-    case MAV_MODE_LOCKED: {
-        _hal->setState(MAV_STATE_STANDBY);
-        break;
+    // handle modes
+    //
+    // if in locked mode
+    if (getMode() == MAV_MODE_LOCKED) {
+        // if state is not stanby then set it to standby and alert gcs
+        if (getState()!=MAV_STATE_STANDBY) {
+            setState(MAV_STATE_STANDBY);
+            _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
+        }
     }
+    // if not locked
+    else {
+        // if state is not active, set it to active and alert gcs
+        if (getState()!=MAV_STATE_ACTIVE) {
+            setState(MAV_STATE_ACTIVE);
+            _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
+        }
 
-    case MAV_MODE_FAILSAFE: {
-        _hal->setState(MAV_STATE_EMERGENCY);
-        break;
-    }
+        // if in auto mode and manual switch set, change to manual
+        if (_hal->rc[_chMode]->getRadioPosition() > 0) setMode(MAV_MODE_MANUAL);
+        else setMode(MAV_MODE_AUTO);
 
-    case MAV_MODE_MANUAL: {
-        manualLoop(dt);
-        break;
-    }
-
-    case MAV_MODE_AUTO: {
-        autoLoop(dt);
-        break;
-    }
-
-    default: {
-        _hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
-        _hal->setState(MAV_STATE_EMERGENCY);
-    }
+        // handle all possible modes
+        if (getMode()==MAV_MODE_MANUAL) {
+            manualLoop(dt);
+        } else if (getMode()==MAV_MODE_AUTO) {
+            autoLoop(dt);
+        } else {
+            _hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
+            setMode(MAV_MODE_FAILSAFE);
+        }
     }
 
     // this sends commands to motors
-    setMotors();
-}
-
-void AP_Controller::setMotors() {
-    switch (_hal->getState()) {
-
-    case MAV_STATE_ACTIVE: {
+    if(getState()==MAV_STATE_ACTIVE) {
         digitalWrite(_hal->aLedPin, HIGH);
-        setMotorsActive();
-        break;
-    }
-    case MAV_STATE_EMERGENCY: {
+        setMotors();
+    } else {
         digitalWrite(_hal->aLedPin, LOW);
-        setMotorsEmergency();
-        break;
-    }
-    case MAV_STATE_STANDBY: {
-        digitalWrite(_hal->aLedPin,LOW);
-        setMotorsStandby();
-        break;
-    }
-    default: {
-        setMotorsStandby();
-    }
-
+        setAllRadioChannelsToNeutral();
     }
 }
 
-
 } // namespace apo
 // vim:ts=4:sw=4:expandtab
diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h
index 4c5fe8a260..cd3c10a81e 100644
--- a/libraries/APO/AP_Controller.h
+++ b/libraries/APO/AP_Controller.h
@@ -19,307 +19,127 @@
 #ifndef AP_Controller_H
 #define AP_Controller_H
 
-#include <inttypes.h>
-#include "../GCS_MAVLink/GCS_MAVLink.h"
-#include <math.h>
+// inclusions
+#include "../AP_Common/AP_Common.h"
 #include "../AP_Common/AP_Var.h"
-#include "AP_Var_keys.h"
-
-class AP_Var_group;
+#include <inttypes.h>
+#include <math.h>
+#include "../GCS_MAVLink/GCS_MAVLink.h"
 
 namespace apo {
 
+// forward declarations within apo
 class AP_HardwareAbstractionLayer;
 class AP_Guide;
 class AP_Navigator;
 class Menu;
 class AP_ArmingMechanism;
 
-/// Controller class
+/// 
+// The control system class.
+// Given where the vehicle wants to go and where it is, 
+// this class is responsible for sending commands to the 
+// motors. It is also responsible for monitoring manual
+// input.
 class AP_Controller {
 public:
+    ///
+    // The controller constructor.
+    // Creates the control system.
+    // @nav the navigation system
+    // @guide the guidance system
+    // @hal the hardware abstraction layer
+    // @armingMechanism the device that controls arming/ disarming
+    // @chMode the channel that the mode switch is on
+    // @key the unique key for the control system saved AP_Var variables
+    // @name the name of the control system
     AP_Controller(AP_Navigator * nav, AP_Guide * guide,
                   AP_HardwareAbstractionLayer * hal,
                   AP_ArmingMechanism * armingMechanism,
                   const uint8_t _chMode,
-                  const uint16_t key = k_cntrl,
+                  const uint16_t key,
                   const prog_char_t * name = NULL);
-    virtual void update(const float dt);
+
+    ///
+    // The loop callback function.
+    // The callback function for the controller loop.
+    // This is inherited from loop.
+    // This function cannot be overriden.
+    // @dt The loop update interval.
+    void update(const float dt);
+
+    ///
+    // This sets all radio outputs to neutral.
+    // This function cannot be overriden.
     void setAllRadioChannelsToNeutral();
+
+    ///
+    // This sets all radio outputs using the radio input.
+    // This function cannot be overriden.
     void setAllRadioChannelsManually();
-    virtual void setMotors();
-    virtual void setMotorsActive() = 0;
-    virtual void setMotorsEmergency() {
-        setAllRadioChannelsToNeutral();
-    };
-    virtual void setMotorsStandby() {
-        setAllRadioChannelsToNeutral();
-    };
-    virtual void manualLoop(const float dt) {
-        setAllRadioChannelsToNeutral();
-    };
-    virtual void autoLoop(const float dt) {
-        setAllRadioChannelsToNeutral();
-    };
-    AP_Uint8 getMode() {
+
+    ///
+    // Sets the motor pwm outputs.
+    // This function sets the motors given the control system outputs.
+    // This function must be defined. There is no default implementation.
+    virtual void setMotors() = 0;
+
+    ///
+    // The manual control loop function.
+    // This uses radio to control the aircraft. 
+    // This function must be defined. There is no default implementation.
+    // @dt The loop update interval.
+    virtual void manualLoop(const float dt) = 0;
+
+    ///
+    // The automatic control update function.
+    // This loop is responsible for taking the 
+    // vehicle to a waypoint.
+    // This function must be defined. There is no default implementation.
+    // @dt The loop update interval.
+    virtual void autoLoop(const float dt) = 0;
+
+    ///
+    // Handles failsafe events.
+    // This function is responsible for setting the mode of the vehicle during
+    // a failsafe event (low battery, loss of gcs comms, ...).
+    // This function must be defined. There is no default implementation.
+    virtual void handleFailsafe() = 0;
+
+    ///
+    // The mode accessor.
+    // @return The current vehicle mode.
+    MAV_MODE getMode() {
         return _mode;
     }
+    ///
+    // The mode setter.
+    // @mode The  mode to set the vehicle to.
+    void setMode(MAV_MODE mode) {
+        _mode = mode;
+    }
+    ///
+    // The state acessor.
+    // @return The current state of the vehicle.
+    MAV_STATE getState() const { 
+        return _state;
+    }
+    ///
+    // state setter
+    // @sate The state to set the vehicle to.
+    void setState(MAV_STATE state) {
+        _state = state;
+    }
+
 protected:
-    AP_Navigator * _nav;
-    AP_Guide * _guide;
-    AP_HardwareAbstractionLayer * _hal;
-    AP_ArmingMechanism * _armingMechanism;
-    uint8_t _chMode;
-    AP_Var_group _group;
-    AP_Uint8 _mode;
-};
-
-class AP_ControllerBlock {
-public:
-    AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart,
-                       uint8_t groupLength) :
-        _group(group), _groupStart(groupStart),
-        _groupEnd(groupStart + groupLength) {
-    }
-    uint8_t getGroupEnd() {
-        return _groupEnd;
-    }
-protected:
-    AP_Var_group * _group; /// helps with parameter management
-    uint8_t _groupStart;
-    uint8_t _groupEnd;
-};
-
-class BlockLowPass: public AP_ControllerBlock {
-public:
-    BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut,
-                 const prog_char_t * fCutLabel = NULL) :
-        AP_ControllerBlock(group, groupStart, 1),
-        _fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")),
-        _y(0) {
-    }
-    float update(const float & input, const float & dt) {
-        float RC = 1 / (2 * M_PI * _fCut); // low pass filter
-        _y = _y + (input - _y) * (dt / (dt + RC));
-        return _y;
-    }
-protected:
-    AP_Float _fCut;
-    float _y;
-};
-
-class BlockSaturation: public AP_ControllerBlock {
-public:
-    BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax,
-                    const prog_char_t * yMaxLabel = NULL) :
-        AP_ControllerBlock(group, groupStart, 1),
-        _yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) {
-    }
-    float update(const float & input) {
-
-        // pid sum
-        float y = input;
-
-        // saturation
-        if (y > _yMax)
-            y = _yMax;
-        if (y < -_yMax)
-            y = -_yMax;
-        return y;
-    }
-protected:
-    AP_Float _yMax; /// output saturation
-};
-
-class BlockDerivative {
-public:
-    BlockDerivative() :
-        _lastInput(0), firstRun(true) {
-    }
-    float update(const float & input, const float & dt) {
-        float derivative = (input - _lastInput) / dt;
-        _lastInput = input;
-        if (firstRun) {
-            firstRun = false;
-            return 0;
-        } else
-            return derivative;
-    }
-protected:
-    float _lastInput; /// last input
-    bool firstRun;
-};
-
-class BlockIntegral {
-public:
-    BlockIntegral() :
-        _i(0) {
-    }
-    float update(const float & input, const float & dt) {
-        _i += input * dt;
-        return _i;
-    }
-protected:
-    float _i; /// integral
-};
-
-class BlockP: public AP_ControllerBlock {
-public:
-    BlockP(AP_Var_group * group, uint8_t groupStart, float kP,
-           const prog_char_t * kPLabel = NULL) :
-        AP_ControllerBlock(group, groupStart, 1),
-        _kP(group, groupStart, kP, kPLabel ? : PSTR("p")) {
-    }
-
-    float update(const float & input) {
-        return _kP * input;
-    }
-protected:
-    AP_Float _kP; /// proportional gain
-};
-
-class BlockI: public AP_ControllerBlock {
-public:
-    BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax,
-           const prog_char_t * kILabel = NULL,
-           const prog_char_t * iMaxLabel = NULL) :
-        AP_ControllerBlock(group, groupStart, 2),
-        _kI(group, groupStart, kI, kILabel ? : PSTR("i")),
-        _blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")),
-        _eI(0) {
-    }
-
-    float update(const float & input, const float & dt) {
-        // integral
-        _eI += input * dt;
-        _eI = _blockSaturation.update(_eI);
-        return _kI * _eI;
-    }
-protected:
-    AP_Float _kI; /// integral gain
-    BlockSaturation _blockSaturation; /// integrator saturation
-    float _eI; /// integral of input
-};
-
-class BlockD: public AP_ControllerBlock {
-public:
-    BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut,
-           const prog_char_t * kDLabel = NULL,
-           const prog_char_t * dFCutLabel = NULL) :
-        AP_ControllerBlock(group, groupStart, 2),
-        _blockLowPass(group, groupStart, dFCut,
-                      dFCutLabel ? : PSTR("dFCut")),
-        _kD(group, _blockLowPass.getGroupEnd(), kD,
-            kDLabel ? : PSTR("d")), _eP(0) {
-    }
-    float update(const float & input, const float & dt) {
-        // derivative with low pass
-        float _eD = _blockLowPass.update((_eP - input) / dt, dt);
-        // proportional, note must come after derivative
-        // because derivatve uses _eP as previous value
-        _eP = input;
-        return _kD * _eD;
-    }
-protected:
-    BlockLowPass _blockLowPass;
-    AP_Float _kD; /// derivative gain
-    float _eP; /// input
-};
-
-class BlockPID: public AP_ControllerBlock {
-public:
-    BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
-             float kD, float iMax, float yMax, uint8_t dFcut) :
-        AP_ControllerBlock(group, groupStart, 6),
-        _blockP(group, groupStart, kP),
-        _blockI(group, _blockP.getGroupEnd(), kI, iMax),
-        _blockD(group, _blockI.getGroupEnd(), kD, dFcut),
-        _blockSaturation(group, _blockD.getGroupEnd(), yMax) {
-    }
-
-    float update(const float & input, const float & dt) {
-        return _blockSaturation.update(
-                   _blockP.update(input) + _blockI.update(input, dt)
-                   + _blockD.update(input, dt));
-    }
-protected:
-    BlockP _blockP;
-    BlockI _blockI;
-    BlockD _blockD;
-    BlockSaturation _blockSaturation;
-};
-
-class BlockPI: public AP_ControllerBlock {
-public:
-    BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
-            float iMax, float yMax) :
-        AP_ControllerBlock(group, groupStart, 4),
-        _blockP(group, groupStart, kP),
-        _blockI(group, _blockP.getGroupEnd(), kI, iMax),
-        _blockSaturation(group, _blockI.getGroupEnd(), yMax) {
-    }
-
-    float update(const float & input, const float & dt) {
-
-        float y = _blockP.update(input) + _blockI.update(input, dt);
-        return _blockSaturation.update(y);
-    }
-protected:
-    BlockP _blockP;
-    BlockI _blockI;
-    BlockSaturation _blockSaturation;
-};
-
-class BlockPD: public AP_ControllerBlock {
-public:
-    BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
-            float kD, float iMax, float yMax, uint8_t dFcut) :
-        AP_ControllerBlock(group, groupStart, 4),
-        _blockP(group, groupStart, kP),
-        _blockD(group, _blockP.getGroupEnd(), kD, dFcut),
-        _blockSaturation(group, _blockD.getGroupEnd(), yMax) {
-    }
-
-    float update(const float & input, const float & dt) {
-
-        float y = _blockP.update(input) + _blockD.update(input, dt);
-        return _blockSaturation.update(y);
-    }
-protected:
-    BlockP _blockP;
-    BlockD _blockD;
-    BlockSaturation _blockSaturation;
-};
-
-/// PID with derivative feedback (ignores reference derivative)
-class BlockPIDDfb: public AP_ControllerBlock {
-public:
-    BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
-                float kD, float iMax, float yMax, float dFCut,
-                const prog_char_t * dFCutLabel = NULL,
-                const prog_char_t * dLabel = NULL) :
-        AP_ControllerBlock(group, groupStart, 5),
-        _blockP(group, groupStart, kP),
-        _blockI(group, _blockP.getGroupEnd(), kI, iMax),
-        _blockSaturation(group, _blockI.getGroupEnd(), yMax),
-        _blockLowPass(group, _blockSaturation.getGroupEnd(), dFCut,
-                      dFCutLabel ? : PSTR("dFCut")),
-        _kD(group, _blockLowPass.getGroupEnd(), kD, dLabel ? : PSTR("d"))
-    {
-    }
-    float update(const float & input, const float & derivative,
-                 const float & dt) {
-
-        float y = _blockP.update(input) + _blockI.update(input, dt) - _kD
-                  * _blockLowPass.update(derivative,dt);
-        return _blockSaturation.update(y);
-    }
-protected:
-    BlockP _blockP;
-    BlockI _blockI;
-    BlockSaturation _blockSaturation;
-    BlockLowPass _blockLowPass;
-    AP_Float _kD; /// derivative gain
+    AP_Navigator * _nav;                    /// navigator
+    AP_Guide * _guide;                      /// guide
+    AP_HardwareAbstractionLayer * _hal;     /// hardware abstraction layer
+    AP_ArmingMechanism * _armingMechanism;  /// controls arming/ disarming
+    uint8_t _chMode;                        /// the channel the mode switch is on
+    AP_Var_group _group;                    /// holds controller parameters
+    MAV_MODE _mode;                         /// vehicle mode (auto, guided, manual, failsafe, ...)
+    MAV_STATE _state;                       /// vehicle state (active, standby, boot, calibrating ...)
 };
 
 } // apo
diff --git a/libraries/APO/AP_ControllerBlock.cpp b/libraries/APO/AP_ControllerBlock.cpp
new file mode 100644
index 0000000000..526c3ae4d9
--- /dev/null
+++ b/libraries/APO/AP_ControllerBlock.cpp
@@ -0,0 +1,179 @@
+/*
+ * AP_ControllerBlock.cpp
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#include "AP_ControllerBlock.h"
+#include <math.h>
+
+namespace apo {
+
+AP_ControllerBlock::AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart,
+                   uint8_t groupLength) :
+    _group(group), _groupStart(groupStart),
+    _groupEnd(groupStart + groupLength) {
+}
+
+BlockLowPass::BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut,
+             const prog_char_t * fCutLabel) :
+    AP_ControllerBlock(group, groupStart, 1),
+    _fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")),
+    _y(0) {
+}
+float BlockLowPass::update(const float & input, const float & dt) {
+    float RC = 1 / (2 * M_PI * _fCut); // low pass filter
+    _y = _y + (input - _y) * (dt / (dt + RC));
+    return _y;
+}
+
+BlockSaturation::BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax,
+                const prog_char_t * yMaxLabel) :
+    AP_ControllerBlock(group, groupStart, 1),
+    _yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) {
+}
+float BlockSaturation::update(const float & input) {
+
+    // pid sum
+    float y = input;
+
+    // saturation
+    if (y > _yMax)
+        y = _yMax;
+    if (y < -_yMax)
+        y = -_yMax;
+    return y;
+}
+
+BlockDerivative::BlockDerivative() :
+    _lastInput(0), firstRun(true) {
+}
+float BlockDerivative::update(const float & input, const float & dt) {
+    float derivative = (input - _lastInput) / dt;
+    _lastInput = input;
+    if (firstRun) {
+        firstRun = false;
+        return 0;
+    } else
+        return derivative;
+}
+
+BlockIntegral::BlockIntegral() :
+    _i(0) {
+}
+float BlockIntegral::update(const float & input, const float & dt) {
+    _i += input * dt;
+    return _i;
+}
+
+BlockP::BlockP(AP_Var_group * group, uint8_t groupStart, float kP,
+       const prog_char_t * kPLabel) :
+    AP_ControllerBlock(group, groupStart, 1),
+    _kP(group, groupStart, kP, kPLabel ? : PSTR("p")) {
+}
+
+float BlockP::update(const float & input) {
+    return _kP * input;
+}
+
+BlockI::BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax,
+       const prog_char_t * kILabel,
+       const prog_char_t * iMaxLabel) :
+    AP_ControllerBlock(group, groupStart, 2),
+    _kI(group, groupStart, kI, kILabel ? : PSTR("i")),
+    _blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")),
+    _eI(0) {
+}
+
+float BlockI::update(const float & input, const float & dt) {
+    // integral
+    _eI += input * dt;
+    _eI = _blockSaturation.update(_eI);
+    return _kI * _eI;
+}
+
+BlockD::BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut,
+       const prog_char_t * kDLabel,
+       const prog_char_t * dFCutLabel) :
+    AP_ControllerBlock(group, groupStart, 2),
+    _blockLowPass(group, groupStart, dFCut,
+                  dFCutLabel ? : PSTR("dFCut")),
+    _kD(group, _blockLowPass.getGroupEnd(), kD,
+        kDLabel ? : PSTR("d")), _eP(0) {
+}
+float BlockD::update(const float & input, const float & dt) {
+    // derivative with low pass
+    float _eD = _blockLowPass.update((input - _eP) / dt, dt);
+    // proportional, note must come after derivative
+    // because derivatve uses _eP as previous value
+    _eP = input;
+    return _kD * _eD;
+}
+
+BlockPID::BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
+         float kD, float iMax, float yMax, uint8_t dFcut) :
+    AP_ControllerBlock(group, groupStart, 6),
+    _blockP(group, groupStart, kP),
+    _blockI(group, _blockP.getGroupEnd(), kI, iMax),
+    _blockD(group, _blockI.getGroupEnd(), kD, dFcut),
+    _blockSaturation(group, _blockD.getGroupEnd(), yMax) {
+}
+
+float BlockPID::update(const float & input, const float & dt) {
+    return _blockSaturation.update(
+               _blockP.update(input) + _blockI.update(input, dt)
+               + _blockD.update(input, dt));
+}
+
+BlockPI::BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
+        float iMax, float yMax) :
+    AP_ControllerBlock(group, groupStart, 4),
+    _blockP(group, groupStart, kP),
+    _blockI(group, _blockP.getGroupEnd(), kI, iMax),
+    _blockSaturation(group, _blockI.getGroupEnd(), yMax) {
+}
+
+float BlockPI::update(const float & input, const float & dt) {
+
+    float y = _blockP.update(input) + _blockI.update(input, dt);
+    return _blockSaturation.update(y);
+}
+
+BlockPD::BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
+        float kD, float iMax, float yMax, uint8_t dFcut) :
+    AP_ControllerBlock(group, groupStart, 4),
+    _blockP(group, groupStart, kP),
+    _blockD(group, _blockP.getGroupEnd(), kD, dFcut),
+    _blockSaturation(group, _blockD.getGroupEnd(), yMax) {
+}
+
+float BlockPD::update(const float & input, const float & dt) {
+
+    float y = _blockP.update(input) + _blockD.update(input, dt);
+    return _blockSaturation.update(y);
+}
+
+BlockPIDDfb::BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
+            float kD, float iMax, float yMax, float dFCut,
+            const prog_char_t * dFCutLabel,
+            const prog_char_t * dLabel) :
+    AP_ControllerBlock(group, groupStart, 5),
+    _blockP(group, groupStart, kP),
+    _blockI(group, _blockP.getGroupEnd(), kI, iMax),
+    _blockSaturation(group, _blockI.getGroupEnd(), yMax),
+    _blockLowPass(group, _blockSaturation.getGroupEnd(), dFCut,
+                  dFCutLabel ? : PSTR("dFCut")),
+    _kD(group, _blockLowPass.getGroupEnd(), kD, dLabel ? : PSTR("d"))
+{
+}
+float BlockPIDDfb::update(const float & input, const float & derivative,
+             const float & dt) {
+
+    float y = _blockP.update(input) + _blockI.update(input, dt) - _kD
+              * _blockLowPass.update(derivative,dt);
+    return _blockSaturation.update(y);
+}
+
+} // namespace apo
+// vim:ts=4:sw=4:expandtab
diff --git a/libraries/APO/AP_ControllerBlock.h b/libraries/APO/AP_ControllerBlock.h
new file mode 100644
index 0000000000..012a34a374
--- /dev/null
+++ b/libraries/APO/AP_ControllerBlock.h
@@ -0,0 +1,237 @@
+/*
+ * AP_ControllerBlock.h
+ * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef AP_ControllerBlock_H
+#define AP_ControllerBlock_H
+
+// inclusions
+#include "../AP_Common/AP_Common.h"
+#include "../AP_Common/AP_Var.h"
+
+// ArduPilotOne namespace
+namespace apo {
+
+///
+// The abstract class defining a controller block.
+class AP_ControllerBlock {
+public:
+    ///
+    // Controller block constructor.
+    // This creates a controller block.
+    // @group The group containing the class parameters.
+    // @groupStart The start of the group. Used for chaining parameters.
+    // @groupEnd The end of the group.
+    AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart,
+                       uint8_t groupLength);
+
+    ///
+    // Get the end of the AP_Var group.
+    // This is used for chaining multiple AP_Var groups into one.
+    uint8_t getGroupEnd() {
+        return _groupEnd;
+    }
+protected:
+    AP_Var_group * _group; /// Contains all the parameters of the class.
+    uint8_t _groupStart; /// The start of the AP_Var group. Used for chaining parameters.
+    uint8_t _groupEnd; /// The end of the AP_Var group.
+};
+
+///
+// A low pass filter block.
+// This takes a signal and smooths it out. It 
+// cuts all frequencies higher than the frequency provided.
+class BlockLowPass: public AP_ControllerBlock {
+public:
+    ///
+    // The constructor.
+    // @group The group containing the class parameters.
+    // @groupStart The start of the group. Used for chaining parameters.
+    // @fCut The cut-off frequency in Hz for smoothing.
+    BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut,
+                 const prog_char_t * fCutLabel = NULL);
+
+    ///
+    // The update function.
+    // @input The input signal.
+    // @dt The timer interval.
+    // @return The output (smoothed) signal.
+    float update(const float & input, const float & dt);
+protected:
+    AP_Float _fCut; /// The cut-off frequency in Hz.
+    float _y; /// The internal state of the low pass filter.
+};
+
+///
+// This block saturates a signal.
+// If the signal is above max it is set to max.
+// If it is below -max it is set to -max. 
+class BlockSaturation: public AP_ControllerBlock {
+public:
+    ///
+    // Controller block constructor.
+    // This creates a controller block.
+    // @group The group containing the class parameters.
+    // @groupStart The start of the group. Used for chaining parameters.
+    // @yMax The maximum threshold.
+    BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax,
+                    const prog_char_t * yMaxLabel = NULL);
+    ///
+    // The update function.
+    // @input The input signal.
+    // @return The output (saturated) signal.
+    float update(const float & input);
+protected:
+    AP_Float _yMax; /// output saturation
+};
+
+///
+// This block calculates a derivative.
+class BlockDerivative {
+public:
+    /// The constructor.
+    BlockDerivative();
+
+    ///
+    // The update function.
+    // @input The input signal.
+    // @return The derivative.
+    float update(const float & input, const float & dt);
+protected:
+    float _lastInput; /// The last input to the block.
+    bool firstRun; /// Keeps track of first run inorder to decide if _lastInput should be used.
+};
+
+/// This block calculates an integral.
+class BlockIntegral {
+public:
+    /// The constructor.
+    BlockIntegral();
+    ///
+    // The update function.
+    // @input The input signal.
+    // @dt The timer interval.
+    // @return The output (integrated) signal.
+    float update(const float & input, const float & dt);
+protected:
+    float _i; /// integral
+};
+
+///
+// This is a proportional block with built-in gain.
+class BlockP: public AP_ControllerBlock {
+public:
+    BlockP(AP_Var_group * group, uint8_t groupStart, float kP,
+           const prog_char_t * kPLabel = NULL);
+    ///
+    // The update function.
+    // @input The input signal.
+    // @return The output signal (kP*input).
+    float update(const float & input);
+protected:
+    AP_Float _kP; /// proportional gain
+};
+
+///
+// This is a integral block with built-in gain.
+class BlockI: public AP_ControllerBlock {
+public:
+    BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax,
+           const prog_char_t * kILabel = NULL,
+           const prog_char_t * iMaxLabel = NULL);
+    float update(const float & input, const float & dt);
+protected:
+    AP_Float _kI;                       /// integral gain
+    BlockSaturation _blockSaturation;   /// integrator saturation
+    float _eI;                          /// internal integrator state
+};
+
+///
+// This is a derivative block with built-in gain.
+class BlockD: public AP_ControllerBlock {
+public:
+    BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut,
+           const prog_char_t * kDLabel = NULL,
+           const prog_char_t * dFCutLabel = NULL);
+    float update(const float & input, const float & dt);
+protected:
+    BlockLowPass _blockLowPass;         /// The low-pass filter block
+    AP_Float _kD;                       /// The derivative gain
+    float _eP;                          /// The previous state
+};
+
+///
+// This is a proportional, integral, derivative block with built-in gains.
+class BlockPID: public AP_ControllerBlock {
+public:
+    BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
+             float kD, float iMax, float yMax, uint8_t dFcut);
+    float update(const float & input, const float & dt);
+protected:
+    BlockP _blockP;                     /// The proportional block.
+    BlockI _blockI;                     /// The integral block.
+    BlockD _blockD;                     /// The derivative block.
+    BlockSaturation _blockSaturation;   /// The saturation block.
+};
+
+///
+// This is a proportional, integral block with built-in gains.
+class BlockPI: public AP_ControllerBlock {
+public:
+    BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
+            float iMax, float yMax);
+    float update(const float & input, const float & dt);
+protected:
+    BlockP _blockP;                     /// The proportional block.
+    BlockI _blockI;                     /// The derivative block.
+    BlockSaturation _blockSaturation;   /// The saturation block.
+};
+
+///
+// This is a proportional, derivative block with built-in gains.
+class BlockPD: public AP_ControllerBlock {
+public:
+    BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
+            float kD, float iMax, float yMax, uint8_t dFcut);
+    float update(const float & input, const float & dt);
+protected:
+    BlockP _blockP;                     /// The proportional block.
+    BlockD _blockD;                     /// The derivative block.
+    BlockSaturation _blockSaturation;   /// The saturation block.
+};
+
+/// PID with derivative feedback (ignores reference derivative)
+class BlockPIDDfb: public AP_ControllerBlock {
+public:
+    BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
+                float kD, float iMax, float yMax, float dFCut,
+                const prog_char_t * dFCutLabel = NULL,
+                const prog_char_t * dLabel = NULL);
+    float update(const float & input, const float & derivative,
+                 const float & dt);
+protected:
+    BlockP _blockP;                     /// The proportional block.
+    BlockI _blockI;                     /// The integral block.
+    BlockSaturation _blockSaturation;   /// The saturation block.
+    BlockLowPass _blockLowPass;         /// The low-pass filter block.
+    AP_Float _kD;                       /// derivative gain
+};
+
+} // apo
+
+#endif // AP_ControllerBlock_H
+// vim:ts=4:sw=4:expandtab
diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp
index 49eaf263a8..9b365d8384 100644
--- a/libraries/APO/AP_Guide.cpp
+++ b/libraries/APO/AP_Guide.cpp
@@ -18,8 +18,8 @@ AP_Guide::AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal)
     _navigator(navigator), _hal(hal), _command(AP_MavlinkCommand::home),
     _previousCommand(AP_MavlinkCommand::home),
     _headingCommand(0), _airSpeedCommand(0),
-    _groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0),
-    _pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST),
+    _groundSpeedCommand(0), _altitudeCommand(0),
+    _mode(MAV_NAV_LOST),
     _numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0),
     _nextCommandTimer(0) {
 }
@@ -42,6 +42,11 @@ float AP_Guide::getHeadingError() {
     return headingError;
 }
 
+float AP_Guide::getDistanceToNextWaypoint() {
+    return _command.distanceTo(_navigator->getLat_degInt(),
+        _navigator->getLon_degInt());
+}
+
 MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
                            AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
     AP_Guide(navigator, hal),
@@ -56,6 +61,16 @@ void MavlinkGuide::update() {
     handleCommand();
 }
 
+float MavlinkGuide::getPNError() {
+    return -_command.getPN(_navigator->getLat_degInt(), _navigator->getLon_degInt());
+}
+float MavlinkGuide::getPEError() {
+    return -_command.getPE(_navigator->getLat_degInt(), _navigator->getLon_degInt());
+}
+float MavlinkGuide::getPDError() {
+    return -_command.getPD(_navigator->getAlt_intM());
+}
+
 void MavlinkGuide::nextCommand() {
     // within 1 seconds, check if more than 5 calls to next command occur
     // if they do, go to home waypoint
@@ -141,12 +156,9 @@ void MavlinkGuide::handleCommand() {
             return;
         }
 
-        float distanceToNext = _command.distanceTo(
-                                   _navigator->getLat_degInt(), _navigator->getLon_degInt());
-
         // check if we are at waypoint or if command
         // radius is zero within tolerance
-        if (distanceToNext < _command.getRadius() | distanceToNext < 1e-5) {
+        if ( (getDistanceToNextWaypoint() < _command.getRadius()) | (getDistanceToNextWaypoint() < 1e-5) ) {
             _hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (distance)"));
             _hal->debug->printf_P(PSTR("waypoint reached (distance)"));
             nextCommand();
@@ -209,17 +221,10 @@ void MavlinkGuide::handleCommand() {
 
     _groundSpeedCommand = _velocityCommand;
 
-    // calculate pN,pE,pD from home and gps coordinates
-    _pNCmd = _command.getPN(_navigator->getLat_degInt(),
-                            _navigator->getLon_degInt());
-    _pECmd = _command.getPE(_navigator->getLat_degInt(),
-                            _navigator->getLon_degInt());
-    _pDCmd = _command.getPD(_navigator->getAlt_intM());
-
     // debug
-    _hal->debug->printf_P(
-        PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),
-        getNumberOfCommands(), getCurrentIndex(), getPreviousIndex());
+    //_hal->debug->printf_P(
+        //PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),
+        //getNumberOfCommands(), getCurrentIndex(), getPreviousIndex());
 }
 
 } // namespace apo
diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h
index 033e27ed45..fbb6b055f2 100644
--- a/libraries/APO/AP_Guide.h
+++ b/libraries/APO/AP_Guide.h
@@ -48,6 +48,11 @@ public:
     MAV_NAV getMode() const {
         return _mode;
     }
+
+    void setMode(MAV_NAV mode) {
+        _mode = mode;
+    }
+
     uint8_t getCurrentIndex() {
         return _cmdIndex;
     }
@@ -92,15 +97,12 @@ public:
     float getAltitudeCommand() {
         return _altitudeCommand;
     }
-    float getPNCmd() {
-        return _pNCmd;
-    }
-    float getPECmd() {
-        return _pECmd;
-    }
-    float getPDCmd() {
-        return _pDCmd;
-    }
+    float getDistanceToNextWaypoint();
+
+    virtual float getPNError() = 0;
+    virtual float getPEError() = 0;
+    virtual float getPDError() = 0;
+
     MAV_NAV getMode() {
         return _mode;
     }
@@ -116,9 +118,6 @@ protected:
     float _airSpeedCommand;
     float _groundSpeedCommand;
     float _altitudeCommand;
-    float _pNCmd;
-    float _pECmd;
-    float _pDCmd;
     MAV_NAV _mode;
     AP_Uint8 _numberOfCommands;
     AP_Uint8 _cmdIndex;
@@ -134,6 +133,9 @@ public:
     void nextCommand();
     void handleCommand();
     void updateCommand();
+    virtual float getPNError();
+    virtual float getPEError();
+    virtual float getPDError();
 
 private:
     AP_Var_group _group;
diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h
index 9b3b9e2810..acea7e6529 100644
--- a/libraries/APO/AP_HardwareAbstractionLayer.h
+++ b/libraries/APO/AP_HardwareAbstractionLayer.h
@@ -50,7 +50,7 @@ public:
         adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), rc(), gcs(),
         hil(), debug(), load(), lastHeartBeat(),
         _heartBeatTimeout(heartBeatTimeout), _mode(mode),
-        _board(board), _vehicle(vehicle), _state(MAV_STATE_UNINIT) {
+        _board(board), _vehicle(vehicle) {
 
         /*
          * Board specific hardware initialization
@@ -146,13 +146,6 @@ public:
     vehicle_t getVehicle() {
         return _vehicle;
     }
-    MAV_STATE getState() {
-        return _state;
-    }
-    void setState(MAV_STATE state) {
-        _state = state;
-    }
-
     bool heartBeatLost() {
         if (_heartBeatTimeout == 0)
             return false;
@@ -167,7 +160,6 @@ private:
     halMode_t _mode;
     board_t _board;
     vehicle_t _vehicle;
-    MAV_STATE _state;
 };
 
 } // namespace apo
diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp
index 9fd6cd5627..da2fdbf7b8 100644
--- a/libraries/APO/AP_MavlinkCommand.cpp
+++ b/libraries/APO/AP_MavlinkCommand.cpp
@@ -62,8 +62,8 @@ AP_MavlinkCommand::AP_MavlinkCommand(const mavlink_waypoint_t & cmd) :
     setZ(cmd.z);
     save();
 
-    // reload home if sent
-    if (cmd.seq == 0) home.load();
+    // reload home if sent, home must be a global waypoint
+    if ( (cmd.seq == 0) && (cmd.frame == MAV_FRAME_GLOBAL)) home.load();
 
     Serial.println("============================================================");
     Serial.println("storing new command from mavlink_waypoint_t");
diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp
index 70a2f3289c..8cd442e337 100644
--- a/libraries/APO/AP_Navigator.cpp
+++ b/libraries/APO/AP_Navigator.cpp
@@ -22,8 +22,9 @@ namespace apo {
 
 AP_Navigator::AP_Navigator(AP_HardwareAbstractionLayer * hal) :
     _hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0),
-    _pitchRate(0), _yaw(0), _yawRate(0), _airSpeed(0),
-    _groundSpeed(0), _vD(0), _lat_degInt(0),
+    _pitchRate(0), _yaw(0), _yawRate(0), 
+    _windSpeed(0), _windDirection(0),
+    _vN(0), _vE(0), _vD(0), _lat_degInt(0),
     _lon_degInt(0), _alt_intM(0) {
 }
 void AP_Navigator::calibrate() {
diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h
index 49db8c5982..437386c54b 100644
--- a/libraries/APO/AP_Navigator.h
+++ b/libraries/APO/AP_Navigator.h
@@ -45,7 +45,18 @@ public:
     void setPN(float _pN);
 
     float getAirSpeed() const {
-        return _airSpeed;
+        // neglects vertical wind
+        float vWN = getVN() + getWindSpeed()*cos(getWindDirection());
+        float vWE = getVE() + getWindSpeed()*sin(getWindDirection());
+        return sqrt(vWN*vWN+vWE+vWE+getVD()*getVD());
+    }
+
+    float getGroundSpeed() const {
+        return sqrt(getVN()*getVN()+getVE()*getVE());
+    } 
+
+    float getWindSpeed() const {
+        return _windSpeed;
     }
 
     int32_t getAlt_intM() const {
@@ -80,16 +91,16 @@ public:
         this->_lon_degInt = _lon * rad2DegInt;
     }
 
-    float getVD() const {
-        return _vD;
+    float getVN() const {
+        return _vN;
     }
 
     float getVE() const {
-        return sin(getYaw()) * getGroundSpeed();
+        return _vE;
     }
 
-    float getGroundSpeed() const {
-        return _groundSpeed;
+    float getVD() const {
+        return _vD;
     }
 
     int32_t getLat_degInt() const {
@@ -103,10 +114,6 @@ public:
         return _lon_degInt;
     }
 
-    float getVN() const {
-        return cos(getYaw()) * getGroundSpeed();
-    }
-
     float getPitch() const {
         return _pitch;
     }
@@ -131,20 +138,71 @@ public:
         return _yawRate;
     }
 
+    float getWindDirection() const {
+        return _windDirection;
+    }
+
+    float getCourseOverGround() const {
+        return atan2(getVE(),getVN());
+    }
+
+    float getSpeedOverGround() const {
+        return sqrt(getVN()*getVN()+getVE()*getVE());
+    }
+
+    float getXAccel() const {
+        return _xAccel;
+    }
+
+    float getYAccel() const {
+        return _yAccel;
+    }
+
+    float getZAccel() const {
+        return _zAccel;
+    }
+
     void setAirSpeed(float airSpeed) {
-        _airSpeed = airSpeed;
+        // assumes wind constant and rescale navigation speed
+        float vScale = (1 + airSpeed/getAirSpeed());
+        float vNorm = sqrt(getVN()*getVN()+getVE()*getVE()+getVD()*getVD());
+        _vN *= vScale/vNorm;
+        _vE *= vScale/vNorm;
+        _vD *= vScale/vNorm;
     }
 
     void setAlt_intM(int32_t alt_intM) {
         _alt_intM = alt_intM;
     }
 
+    void setVN(float vN) {
+        _vN = vN;
+    }
+
+    void setVE(float vE) {
+        _vE = vE;
+    }
+
     void setVD(float vD) {
         _vD = vD;
     }
 
+    void setXAccel(float xAccel) {
+        _xAccel = xAccel;
+    }
+
+    void setYAccel(float yAccel) {
+        _yAccel = yAccel;
+    }
+
+    void setZAccel(float zAccel) {
+        _zAccel = zAccel;
+    }
+
     void setGroundSpeed(float groundSpeed) {
-        _groundSpeed = groundSpeed;
+        float cog = getCourseOverGround();
+        _vN = cos(cog)*groundSpeed;
+        _vE = sin(cog)*groundSpeed;
     }
 
     void setLat_degInt(int32_t lat_degInt) {
@@ -180,13 +238,24 @@ public:
     void setYawRate(float yawRate) {
         _yawRate = yawRate;
     }
+
     void setTimeStamp(int32_t timeStamp) {
         _timeStamp = timeStamp;
     }
+
     int32_t getTimeStamp() const {
         return _timeStamp;
     }
 
+    void setWindDirection(float windDirection) {
+        _windDirection = windDirection;
+    }
+
+    void setWindSpeed(float windSpeed) {
+        _windSpeed = windSpeed;
+    }
+
+
 protected:
     AP_HardwareAbstractionLayer * _hal;
 private:
@@ -197,9 +266,15 @@ private:
     float _pitchRate; // rad/s
     float _yaw; // rad
     float _yawRate; // rad/s
-    float _airSpeed; // m/s
-    float _groundSpeed; // m/s
+    // wind assumed to be N-E plane
+    float _windSpeed; // m/s
+    float _windDirection; // m/s
+    float _vN;
+    float _vE;
     float _vD; // m/s
+    float _xAccel;
+    float _yAccel;
+    float _zAccel;
     int32_t _lat_degInt; // deg / 1e7
     int32_t _lon_degInt; // deg / 1e7
     int32_t _alt_intM; // meters / 1e3
diff --git a/libraries/AP_Common/AP_Vector.h b/libraries/AP_Common/AP_Vector.h
index 287056b2a1..a82018c388 100644
--- a/libraries/AP_Common/AP_Vector.h
+++ b/libraries/AP_Common/AP_Vector.h
@@ -19,6 +19,7 @@
 #ifndef Vector_H
 #define Vector_H
 
+#include "../FastSerial/BetterStream.h"
 #include <stdlib.h>
 #include <inttypes.h>
 #include <WProgram.h>
diff --git a/libraries/AP_Common/include/menu.h b/libraries/AP_Common/include/menu.h
index 1e4e3a0d89..2cff1b9aca 100644
--- a/libraries/AP_Common/include/menu.h
+++ b/libraries/AP_Common/include/menu.h
@@ -16,6 +16,8 @@
 #ifndef __AP_COMMON_MENU_H
 #define __AP_COMMON_MENU_H
 
+#include <inttypes.h>
+
 #define MENU_COMMANDLINE_MAX	32	///< maximum input line length
 #define MENU_ARGS_MAX			4	///< maximum number of arguments
 #define MENU_COMMAND_MAX		14	///< maximum size of a command name