diff --git a/.gitignore b/.gitignore
index 99b43f7898..ebd471e7e0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,6 @@ Tools/ArdupilotMegaPlanner/bin/Release/logs/
 ArduCopter/Debug/
 config.mk
 serialsent.raw
+CMakeFiles
+CMakeCache.txt
+cmake_install.cmake
diff --git a/ArduPlane/CMakeLists.txt b/ArduPlane/CMakeLists.txt
index 19a3b35a50..3b6d496b97 100644
--- a/ArduPlane/CMakeLists.txt
+++ b/ArduPlane/CMakeLists.txt
@@ -62,7 +62,7 @@ set(${FIRMWARE_NAME}_SKETCHES
 	#GCS_Standard.pde
 	#GCS_Xplane.pde
 	#heli.pde
-	HIL_Xplane.pde
+	#HIL_Xplane.pde
 	#leds.pde
 	Log.pde
 	#motors_hexa.pde
@@ -92,12 +92,12 @@ set(${FIRMWARE_NAME}_SRCS
 set(${FIRMWARE_NAME}_HDRS 	
 	APM_Config.h
 	APM_Config_mavlink_hil.h
-	APM_Config_xplane.h
+	#APM_Config_xplane.h
 	config.h
 	defines.h
 	GCS.h
-	HIL.h
-	Mavlink_Common.h
+	#HIL.h
+	#Mavlink_Common.h
 	Parameters.h
 	)  # Firmware sources
 
diff --git a/ArduPlane/Makefile b/ArduPlane/Makefile
index 34f8baffd9..0ea3e7a873 100644
--- a/ArduPlane/Makefile
+++ b/ArduPlane/Makefile
@@ -1,10 +1,510 @@
-#
-# Trivial makefile for building APM
-#
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
 
-#
-# Select 'mega' for the original APM, or 'mega2560' for the V2 APM.
-#
-BOARD	=	mega
+# Default target executed when no arguments are given to make.
+default_target: all
+.PHONY : default_target
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canoncical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane
+
+#=============================================================================
+# Targets provided globally by CMake.
+
+# Special rule for the target edit_cache
+edit_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..."
+	/usr/bin/cmake -i .
+.PHONY : edit_cache
+
+# Special rule for the target edit_cache
+edit_cache/fast: edit_cache
+.PHONY : edit_cache/fast
+
+# Special rule for the target rebuild_cache
+rebuild_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
+	/usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
+.PHONY : rebuild_cache
+
+# Special rule for the target rebuild_cache
+rebuild_cache/fast: rebuild_cache
+.PHONY : rebuild_cache/fast
+
+# The main all target
+all: cmake_check_build_system
+	$(CMAKE_COMMAND) -E cmake_progress_start /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane/CMakeFiles /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane/CMakeFiles/progress.marks
+	$(MAKE) -f CMakeFiles/Makefile2 all
+	$(CMAKE_COMMAND) -E cmake_progress_start /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane/CMakeFiles 0
+.PHONY : all
+
+# The main clean target
+clean:
+	$(MAKE) -f CMakeFiles/Makefile2 clean
+.PHONY : clean
+
+# The main clean target
+clean/fast: clean
+.PHONY : clean/fast
+
+# Prepare targets for installation.
+preinstall: all
+	$(MAKE) -f CMakeFiles/Makefile2 preinstall
+.PHONY : preinstall
+
+# Prepare targets for installation.
+preinstall/fast:
+	$(MAKE) -f CMakeFiles/Makefile2 preinstall
+.PHONY : preinstall/fast
+
+# clear depends
+depend:
+	$(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
+.PHONY : depend
+
+#=============================================================================
+# Target rules for targets named ArduPlane
+
+# Build rule for target.
+ArduPlane: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 ArduPlane
+.PHONY : ArduPlane
+
+# fast build rule for target.
+ArduPlane/fast:
+	$(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/build
+.PHONY : ArduPlane/fast
+
+#=============================================================================
+# Target rules for targets named ArduPlane-serial
+
+# Build rule for target.
+ArduPlane-serial: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 ArduPlane-serial
+.PHONY : ArduPlane-serial
+
+# fast build rule for target.
+ArduPlane-serial/fast:
+	$(MAKE) -f CMakeFiles/ArduPlane-serial.dir/build.make CMakeFiles/ArduPlane-serial.dir/build
+.PHONY : ArduPlane-serial/fast
+
+#=============================================================================
+# Target rules for targets named ArduPlane-upload
+
+# Build rule for target.
+ArduPlane-upload: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 ArduPlane-upload
+.PHONY : ArduPlane-upload
+
+# fast build rule for target.
+ArduPlane-upload/fast:
+	$(MAKE) -f CMakeFiles/ArduPlane-upload.dir/build.make CMakeFiles/ArduPlane-upload.dir/build
+.PHONY : ArduPlane-upload/fast
+
+#=============================================================================
+# Target rules for targets named upload
+
+# Build rule for target.
+upload: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 upload
+.PHONY : upload
+
+# fast build rule for target.
+upload/fast:
+	$(MAKE) -f CMakeFiles/upload.dir/build.make CMakeFiles/upload.dir/build
+.PHONY : upload/fast
+
+#=============================================================================
+# Target rules for targets named DataFlash
+
+# Build rule for target.
+DataFlash: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 DataFlash
+.PHONY : DataFlash
+
+# fast build rule for target.
+DataFlash/fast:
+	$(MAKE) -f libs/DataFlash/CMakeFiles/DataFlash.dir/build.make libs/DataFlash/CMakeFiles/DataFlash.dir/build
+.PHONY : DataFlash/fast
+
+#=============================================================================
+# Target rules for targets named mega_CORE
+
+# Build rule for target.
+mega_CORE: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 mega_CORE
+.PHONY : mega_CORE
+
+# fast build rule for target.
+mega_CORE/fast:
+	$(MAKE) -f libs/DataFlash/CMakeFiles/mega_CORE.dir/build.make libs/DataFlash/CMakeFiles/mega_CORE.dir/build
+.PHONY : mega_CORE/fast
+
+#=============================================================================
+# Target rules for targets named mega_SPI
+
+# Build rule for target.
+mega_SPI: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 mega_SPI
+.PHONY : mega_SPI
+
+# fast build rule for target.
+mega_SPI/fast:
+	$(MAKE) -f libs/DataFlash/CMakeFiles/mega_SPI.dir/build.make libs/DataFlash/CMakeFiles/mega_SPI.dir/build
+.PHONY : mega_SPI/fast
+
+#=============================================================================
+# Target rules for targets named AP_Math
+
+# Build rule for target.
+AP_Math: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 AP_Math
+.PHONY : AP_Math
+
+# fast build rule for target.
+AP_Math/fast:
+	$(MAKE) -f libs/AP_Math/CMakeFiles/AP_Math.dir/build.make libs/AP_Math/CMakeFiles/AP_Math.dir/build
+.PHONY : AP_Math/fast
+
+#=============================================================================
+# Target rules for targets named PID
+
+# Build rule for target.
+PID: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 PID
+.PHONY : PID
+
+# fast build rule for target.
+PID/fast:
+	$(MAKE) -f libs/PID/CMakeFiles/PID.dir/build.make libs/PID/CMakeFiles/PID.dir/build
+.PHONY : PID/fast
+
+#=============================================================================
+# Target rules for targets named AP_Common
+
+# Build rule for target.
+AP_Common: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 AP_Common
+.PHONY : AP_Common
+
+# fast build rule for target.
+AP_Common/fast:
+	$(MAKE) -f libs/AP_Common/CMakeFiles/AP_Common.dir/build.make libs/AP_Common/CMakeFiles/AP_Common.dir/build
+.PHONY : AP_Common/fast
+
+#=============================================================================
+# Target rules for targets named RC_Channel
+
+# Build rule for target.
+RC_Channel: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 RC_Channel
+.PHONY : RC_Channel
+
+# fast build rule for target.
+RC_Channel/fast:
+	$(MAKE) -f libs/RC_Channel/CMakeFiles/RC_Channel.dir/build.make libs/RC_Channel/CMakeFiles/RC_Channel.dir/build
+.PHONY : RC_Channel/fast
+
+#=============================================================================
+# Target rules for targets named AP_OpticalFlow
+
+# Build rule for target.
+AP_OpticalFlow: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 AP_OpticalFlow
+.PHONY : AP_OpticalFlow
+
+# fast build rule for target.
+AP_OpticalFlow/fast:
+	$(MAKE) -f libs/AP_OpticalFlow/CMakeFiles/AP_OpticalFlow.dir/build.make libs/AP_OpticalFlow/CMakeFiles/AP_OpticalFlow.dir/build
+.PHONY : AP_OpticalFlow/fast
+
+#=============================================================================
+# Target rules for targets named ModeFilter
+
+# Build rule for target.
+ModeFilter: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 ModeFilter
+.PHONY : ModeFilter
+
+# fast build rule for target.
+ModeFilter/fast:
+	$(MAKE) -f libs/ModeFilter/CMakeFiles/ModeFilter.dir/build.make libs/ModeFilter/CMakeFiles/ModeFilter.dir/build
+.PHONY : ModeFilter/fast
+
+#=============================================================================
+# Target rules for targets named memcheck
+
+# Build rule for target.
+memcheck: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 memcheck
+.PHONY : memcheck
+
+# fast build rule for target.
+memcheck/fast:
+	$(MAKE) -f libs/memcheck/CMakeFiles/memcheck.dir/build.make libs/memcheck/CMakeFiles/memcheck.dir/build
+.PHONY : memcheck/fast
+
+#=============================================================================
+# Target rules for targets named APM_PI
+
+# Build rule for target.
+APM_PI: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 APM_PI
+.PHONY : APM_PI
+
+# fast build rule for target.
+APM_PI/fast:
+	$(MAKE) -f libs/APM_PI/CMakeFiles/APM_PI.dir/build.make libs/APM_PI/CMakeFiles/APM_PI.dir/build
+.PHONY : APM_PI/fast
+
+#=============================================================================
+# Target rules for targets named AP_GPS
+
+# Build rule for target.
+AP_GPS: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 AP_GPS
+.PHONY : AP_GPS
+
+# fast build rule for target.
+AP_GPS/fast:
+	$(MAKE) -f libs/AP_GPS/CMakeFiles/AP_GPS.dir/build.make libs/AP_GPS/CMakeFiles/AP_GPS.dir/build
+.PHONY : AP_GPS/fast
+
+#=============================================================================
+# Target rules for targets named AP_DCM
+
+# Build rule for target.
+AP_DCM: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 AP_DCM
+.PHONY : AP_DCM
+
+# fast build rule for target.
+AP_DCM/fast:
+	$(MAKE) -f libs/AP_DCM/CMakeFiles/AP_DCM.dir/build.make libs/AP_DCM/CMakeFiles/AP_DCM.dir/build
+.PHONY : AP_DCM/fast
+
+#=============================================================================
+# Target rules for targets named AP_Compass
+
+# Build rule for target.
+AP_Compass: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 AP_Compass
+.PHONY : AP_Compass
+
+# fast build rule for target.
+AP_Compass/fast:
+	$(MAKE) -f libs/AP_Compass/CMakeFiles/AP_Compass.dir/build.make libs/AP_Compass/CMakeFiles/AP_Compass.dir/build
+.PHONY : AP_Compass/fast
+
+#=============================================================================
+# Target rules for targets named AP_ADC
+
+# Build rule for target.
+AP_ADC: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 AP_ADC
+.PHONY : AP_ADC
+
+# fast build rule for target.
+AP_ADC/fast:
+	$(MAKE) -f libs/AP_ADC/CMakeFiles/AP_ADC.dir/build.make libs/AP_ADC/CMakeFiles/AP_ADC.dir/build
+.PHONY : AP_ADC/fast
+
+#=============================================================================
+# Target rules for targets named AP_IMU
+
+# Build rule for target.
+AP_IMU: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 AP_IMU
+.PHONY : AP_IMU
+
+# fast build rule for target.
+AP_IMU/fast:
+	$(MAKE) -f libs/AP_IMU/CMakeFiles/AP_IMU.dir/build.make libs/AP_IMU/CMakeFiles/AP_IMU.dir/build
+.PHONY : AP_IMU/fast
+
+#=============================================================================
+# Target rules for targets named AP_RangeFinder
+
+# Build rule for target.
+AP_RangeFinder: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 AP_RangeFinder
+.PHONY : AP_RangeFinder
+
+# fast build rule for target.
+AP_RangeFinder/fast:
+	$(MAKE) -f libs/AP_RangeFinder/CMakeFiles/AP_RangeFinder.dir/build.make libs/AP_RangeFinder/CMakeFiles/AP_RangeFinder.dir/build
+.PHONY : AP_RangeFinder/fast
+
+#=============================================================================
+# Target rules for targets named APM_RC
+
+# Build rule for target.
+APM_RC: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 APM_RC
+.PHONY : APM_RC
+
+# fast build rule for target.
+APM_RC/fast:
+	$(MAKE) -f libs/APM_RC/CMakeFiles/APM_RC.dir/build.make libs/APM_RC/CMakeFiles/APM_RC.dir/build
+.PHONY : APM_RC/fast
+
+#=============================================================================
+# Target rules for targets named APM_BMP085
+
+# Build rule for target.
+APM_BMP085: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 APM_BMP085
+.PHONY : APM_BMP085
+
+# fast build rule for target.
+APM_BMP085/fast:
+	$(MAKE) -f libs/APM_BMP085/CMakeFiles/APM_BMP085.dir/build.make libs/APM_BMP085/CMakeFiles/APM_BMP085.dir/build
+.PHONY : APM_BMP085/fast
+
+#=============================================================================
+# Target rules for targets named mega_Wire
+
+# Build rule for target.
+mega_Wire: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 mega_Wire
+.PHONY : mega_Wire
+
+# fast build rule for target.
+mega_Wire/fast:
+	$(MAKE) -f libs/APM_BMP085/CMakeFiles/mega_Wire.dir/build.make libs/APM_BMP085/CMakeFiles/mega_Wire.dir/build
+.PHONY : mega_Wire/fast
+
+#=============================================================================
+# Target rules for targets named FastSerial
+
+# Build rule for target.
+FastSerial: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 FastSerial
+.PHONY : FastSerial
+
+# fast build rule for target.
+FastSerial/fast:
+	$(MAKE) -f libs/FastSerial/CMakeFiles/FastSerial.dir/build.make libs/FastSerial/CMakeFiles/FastSerial.dir/build
+.PHONY : FastSerial/fast
+
+#=============================================================================
+# Target rules for targets named GCS_MAVLink
+
+# Build rule for target.
+GCS_MAVLink: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 GCS_MAVLink
+.PHONY : GCS_MAVLink
+
+# fast build rule for target.
+GCS_MAVLink/fast:
+	$(MAKE) -f libs/GCS_MAVLink/CMakeFiles/GCS_MAVLink.dir/build.make libs/GCS_MAVLink/CMakeFiles/GCS_MAVLink.dir/build
+.PHONY : GCS_MAVLink/fast
+
+ArduPlane.obj: ArduPlane.cpp.obj
+.PHONY : ArduPlane.obj
+
+# target to build an object file
+ArduPlane.cpp.obj:
+	$(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/ArduPlane.cpp.obj
+.PHONY : ArduPlane.cpp.obj
+
+ArduPlane.i: ArduPlane.cpp.i
+.PHONY : ArduPlane.i
+
+# target to preprocess a source file
+ArduPlane.cpp.i:
+	$(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/ArduPlane.cpp.i
+.PHONY : ArduPlane.cpp.i
+
+ArduPlane.s: ArduPlane.cpp.s
+.PHONY : ArduPlane.s
+
+# target to generate assembly for a file
+ArduPlane.cpp.s:
+	$(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/ArduPlane.cpp.s
+.PHONY : ArduPlane.cpp.s
+
+# Help Target
+help:
+	@echo "The following are some of the valid targets for this Makefile:"
+	@echo "... all (the default if no target is provided)"
+	@echo "... clean"
+	@echo "... depend"
+	@echo "... ArduPlane"
+	@echo "... ArduPlane-serial"
+	@echo "... ArduPlane-upload"
+	@echo "... edit_cache"
+	@echo "... rebuild_cache"
+	@echo "... upload"
+	@echo "... DataFlash"
+	@echo "... mega_CORE"
+	@echo "... mega_SPI"
+	@echo "... AP_Math"
+	@echo "... PID"
+	@echo "... AP_Common"
+	@echo "... RC_Channel"
+	@echo "... AP_OpticalFlow"
+	@echo "... ModeFilter"
+	@echo "... memcheck"
+	@echo "... APM_PI"
+	@echo "... AP_GPS"
+	@echo "... AP_DCM"
+	@echo "... AP_Compass"
+	@echo "... AP_ADC"
+	@echo "... AP_IMU"
+	@echo "... AP_RangeFinder"
+	@echo "... APM_RC"
+	@echo "... APM_BMP085"
+	@echo "... mega_Wire"
+	@echo "... FastSerial"
+	@echo "... GCS_MAVLink"
+	@echo "... ArduPlane.obj"
+	@echo "... ArduPlane.i"
+	@echo "... ArduPlane.s"
+.PHONY : help
+
+
+
+#=============================================================================
+# Special targets to cleanup operation of make.
+
+# Special rule to run CMake to check the build system integrity.
+# No rule that depends on this can have commands that come from listfiles
+# because they might be regenerated.
+cmake_check_build_system:
+	$(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
+.PHONY : cmake_check_build_system
 
-include ../libraries/AP_Common/Arduino.mk
diff --git a/apo/CMakeLists.txt b/apo/CMakeLists.txt
index 95c4c1289c..f08d48c262 100644
--- a/apo/CMakeLists.txt
+++ b/apo/CMakeLists.txt
@@ -7,24 +7,18 @@
 #====================================================================#
 #  Settings                                                          #
 #====================================================================#
-set(FIRMWARE_NAME ardupilotone)
+set(FIRMWARE_NAME apo)
 
 set(${FIRMWARE_NAME}_BOARD ${BOARD})               # Arduino Target board
 
 set(${FIRMWARE_NAME}_SKETCHES
-	ardupilotone.pde	
+	apo.pde	
 	)  # Firmware sketches
 	
 set(${FIRMWARE_NAME}_SRCS 
 	)  # Firmware sources
 		
 set(${FIRMWARE_NAME}_HDRS 
-        BoatGeneric.h
-        CarStampede.h
-        #CNIRover.h
-        #CNIBoat.h
-        ControllerBoat.h
-        ControllerCar.h
         ControllerPlane.h
         ControllerQuad.h
         PlaneEasystar.h
diff --git a/apo/apo.pde b/apo/apo.pde
index 7e79ec1a9e..233b9513ee 100644
--- a/apo/apo.pde
+++ b/apo/apo.pde
@@ -1,12 +1,3 @@
-/*
- * ardupilotone
- *
- *  Created on: Apr 30, 2011
- *      Author: jgoppert
- */
-
-#define ENABLE_FASTSERIAL_DEBUG
-
 // Libraries
 #include <FastSerial.h>
 #include <AP_Common.h>
@@ -23,162 +14,8 @@
 #include <ModeFilter.h>
 #include <APO.h>
 
-FastSerialPort0(Serial);
-FastSerialPort1(Serial1);
-FastSerialPort2(Serial2);
-FastSerialPort3(Serial3);
-
 // Vehicle Configuration
 #include "PlaneEasystar.h"
 
-/*
- * Required Global Declarations
- */
-
-static apo::AP_Autopilot * autoPilot;
-
-void setup() {
-
-	using namespace apo;
-	
-	AP_Var::load_all();
-
-	/*
-	 * Communications
-	 */
-	Serial.begin(DEBUG_BAUD, 128, 128); // debug
-	if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs
-	else Serial3.begin(TELEM_BAUD, 128, 128); // gcs
-
-	// hardware abstraction layer
-	AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer(
-			halMode, board, vehicle, heartBeatTimeout);
-	
-	// debug serial
-	hal->debug = &Serial;
-	hal->debug->println_P(PSTR("initializing debug line"));
-
-	/*
-	 * Initialize Comm Channels
-	 */
-	hal->debug->println_P(PSTR("initializing comm channels"));
-	if (hal->getMode() == MODE_LIVE) {
-		Serial1.begin(GPS_BAUD, 128, 16); // gps
-	} else { // hil
-		Serial1.begin(HIL_BAUD, 128, 128);
-	}
-
-	/*
-	 * Sensor initialization
-	 */
-	if (hal->getMode() == MODE_LIVE) {
-		hal->debug->println_P(PSTR("initializing adc"));
-		hal->adc = new ADC_CLASS;
-		hal->adc->Init();
-
-		if (gpsEnabled) {
-			hal->debug->println_P(PSTR("initializing gps"));
-			AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
-			hal->gps = &gpsDriver;
-			hal->gps->init();
-		}
-
-		if (baroEnabled) {
-			hal->debug->println_P(PSTR("initializing baro"));
-			hal->baro = new BARO_CLASS;
-			hal->baro->Init();
-		}
-
-		if (compassEnabled) {
-			hal->debug->println_P(PSTR("initializing compass"));
-			hal->compass = new COMPASS_CLASS;
-			hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD);
-			hal->compass->init();
-		}
-
-		/**
-		 * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not
-		 * initialize them and NULL will be assigned to those corresponding pointers.
-		 * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code
-		 * will not be executed by the navigator.
-		 * The coordinate system is assigned by the right hand rule with the thumb pointing down.
-		 * In set_orientation, it is defind as (front/back,left/right,down,up)
-		 */
-
-		if (rangeFinderFrontEnabled) {
-			hal->debug->println_P(PSTR("initializing front range finder"));
-			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
-			rangeFinder->set_analog_port(1);
-			rangeFinder->set_orientation(1, 0, 0);
-			hal->rangeFinders.push_back(rangeFinder);
-		}
-
-		if (rangeFinderBackEnabled) {
-			hal->debug->println_P(PSTR("initializing back range finder"));
-			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
-			rangeFinder->set_analog_port(2);
-			rangeFinder->set_orientation(-1, 0, 0);
-			hal->rangeFinders.push_back(rangeFinder);
-		}
-
-		if (rangeFinderLeftEnabled) {
-			hal->debug->println_P(PSTR("initializing left range finder"));
-			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
-			rangeFinder->set_analog_port(3);
-			rangeFinder->set_orientation(0, -1, 0);
-			hal->rangeFinders.push_back(rangeFinder);
-		}
-
-		if (rangeFinderRightEnabled) {
-			hal->debug->println_P(PSTR("initializing right range finder"));
-			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
-			rangeFinder->set_analog_port(4);
-			rangeFinder->set_orientation(0, 1, 0);
-			hal->rangeFinders.push_back(rangeFinder);
-		}
-
-		if (rangeFinderUpEnabled) {
-			hal->debug->println_P(PSTR("initializing up range finder"));
-			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
-			rangeFinder->set_analog_port(5);
-			rangeFinder->set_orientation(0, 0, -1);
-			hal->rangeFinders.push_back(rangeFinder);
-		}
-
-		if (rangeFinderDownEnabled) {
-			hal->debug->println_P(PSTR("initializing down range finder"));
-			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
-			rangeFinder->set_analog_port(6);
-			rangeFinder->set_orientation(0, 0, 1);
-			hal->rangeFinders.push_back(rangeFinder);
-		}
-
-	}
-
-	/*
-	 * Select guidance, navigation, control algorithms
-	 */
-	AP_Navigator * navigator = new NAVIGATOR_CLASS(hal);
-	AP_Guide * guide = new GUIDE_CLASS(navigator, hal);
-	AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal);
-
-	/*
-	 * CommLinks
-	 */
-	if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal);
-	else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal);
-	
-	hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
-
-	/*
-	 * Start the autopilot
-	 */
-	hal->debug->printf_P(PSTR("initializing arduplane\n"));
-	hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
-	autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,
-			loop0Rate, loop1Rate, loop2Rate, loop3Rate);
-}
-
-void loop() {
-	autoPilot->update();
-}
+// ArduPilotOne Default Setup
+#include "APO_DefaultSetup.h"