Merge branch 'master' of https://code.google.com/p/ardupilot-mega

.gitignore

@@ -3,3 +3,7 @@ Tools/ArdupilotMegaPlanner/bin/Release/logs/
 ArduCopter/Debug/
 config.mk
 serialsent.raw
+CMakeFiles
+CMakeCache.txt
+cmake_install.cmake
+build

ArduBoat/ArduBoat.pde

@@ -0,0 +1,21 @@
+// Libraries
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <APM_RC.h>
+#include <AP_RangeFinder.h>
+#include <GCS_MAVLink.h>
+#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>
+#include <ModeFilter.h>
+#include <APO.h>
+
+// Vehicle Configuration
+#include "BoatGeneric.h"
+
+// ArduPilotOne Default Setup
+#include "APO_DefaultSetup.h"

ArduBoat/BoatGeneric.h

@@ -0,0 +1,68 @@
+/*
+ * BoatGeneric.h
+ *
+ *  Created on: May 1, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef BOATGENERIC_H_
+#define BOATGENERIC_H_
+
+// vehicle options
+static const apo::vehicle_t vehicle = apo::VEHICLE_BOAT;
+static const apo::halMode_t halMode = apo::MODE_LIVE;
+static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560;
+static const uint8_t heartBeatTimeout = 3;
+
+// algorithm selection
+#define CONTROLLER_CLASS ControllerBoat
+#define GUIDE_CLASS MavlinkGuide
+#define NAVIGATOR_CLASS DcmNavigator
+#define COMMLINK_CLASS MavlinkComm
+
+// hardware selection
+#define ADC_CLASS AP_ADC_ADS7844
+#define COMPASS_CLASS AP_Compass_HMC5843
+#define BARO_CLASS APM_BMP085_Class
+#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
+#define DEBUG_BAUD 57600
+#define TELEM_BAUD 57600
+#define GPS_BAUD 38400
+#define HIL_BAUD 57600
+
+// optional sensors
+static bool gpsEnabled = false;
+static bool baroEnabled = true;
+static bool compassEnabled = true;
+
+static bool rangeFinderFrontEnabled = true;
+static bool rangeFinderBackEnabled = true;
+static bool rangeFinderLeftEnabled = true;
+static bool rangeFinderRightEnabled = true;
+static bool rangeFinderUpEnabled = true;
+static bool rangeFinderDownEnabled = true;
+
+// loop rates
+static const float loop0Rate = 150;
+static const float loop1Rate = 100;
+static const float loop2Rate = 10;
+static const float loop3Rate = 1;
+static const float loop4Rate = 0.1;
+
+// gains
+const float steeringP = 1.0;
+const float steeringI = 0.0;
+const float steeringD = 0.0;
+const float steeringIMax = 0.0;
+const float steeringYMax = 3.0;
+
+const float throttleP = 0.0;
+const float throttleI = 0.0;
+const float throttleD = 0.0;
+const float throttleIMax = 0.0;
+const float throttleYMax = 0.0;
+const float throttleDFCut = 3.0;
+
+#include "ControllerBoat.h"
+
+#endif /* BOATGENERIC_H_ */

ArduBoat/CMakeLists.txt

@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 2.6)
+set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../")
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain
+set(CMAKE_MODULE_PATH    ${CMAKE_SOURCE_DIR}/cmake/modules)  # CMake module search path
+
+string(REGEX REPLACE ".*/" "" PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR})
+project(${PROJECT_NAME} C CXX)
+
+set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde)
+
+find_package(Arduino 22 REQUIRED)
+
+if (NOT DEFINED BOARD)
+	message(STATUS "board not defined, assuming mega, use cmake -DBOARD=mega2560 , etc. to specify")
+	set(BOARD "mega")
+endif()
+message(STATUS "Board configured as: ${BOARD}")
+
+# need to configure based on host operating system
+set(${PROJECT_NAME}_PORT COM2)
+set(${PROJECT_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X )
+	
+include_directories(
+${ARDUINO_LIBRARIES_PATH}/Wire
+${CMAKE_SOURCE_DIR}/libraries/APO
+${CMAKE_SOURCE_DIR}/libraries/AP_Common
+${CMAKE_SOURCE_DIR}/libraries/FastSerial
+${CMAKE_SOURCE_DIR}/libraries/ModeFilter
+${CMAKE_SOURCE_DIR}/libraries/AP_Compass
+${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder
+${CMAKE_SOURCE_DIR}/libraries/AP_GPS
+${CMAKE_SOURCE_DIR}/libraries/AP_IMU
+${CMAKE_SOURCE_DIR}/libraries/AP_ADC
+${CMAKE_SOURCE_DIR}/libraries/AP_DCM
+${CMAKE_SOURCE_DIR}/libraries/APM_RC
+${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink
+${CMAKE_SOURCE_DIR}/libraries/APM_BMP085
+)
+
+add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs")
+
+set(${PROJECT_NAME}_BOARD ${BOARD})
+file(GLOB ${PROJECT_NAME}_SKETCHES *.pde)
+file(GLOB ${PROJECT_NAME}_SRCS *.cpp)
+file(GLOB ${PROJECT_NAME}_HDRS *.h)
+set(${PROJECT_NAME}_LIBS
+	c
+    m
+    APO
+    FastSerial
+    AP_Common
+    GCS_MAVLink
+    AP_GPS
+    APM_RC
+    AP_DCM
+    AP_ADC
+    AP_Compass
+    AP_IMU
+    AP_RangeFinder
+    APM_BMP085
+	ModeFilter
+)
+
+generate_arduino_firmware(${PROJECT_NAME})
+
+install(FILES
+	${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.hex 
+	DESTINATION bin
+	)

ArduBoat/ControllerBoat.h

@@ -0,0 +1,125 @@
+/*
+ * ControllerBoat.h
+ *
+ *  Created on: Jun 30, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef CONTROLLERBOAT_H_
+#define CONTROLLERBOAT_H_
+
+#include "../APO/AP_Controller.h"
+
+namespace apo {
+
+class ControllerBoat: public AP_Controller {
+private:
+	AP_Var_group _group;
+	AP_Uint8 _mode;
+	enum {
+		k_chMode = k_radioChannelsStart, k_chStr, k_chThr
+	};
+	enum {
+		k_pidStr = k_controllersStart, k_pidThr
+	};
+	enum {
+		CH_MODE = 0, CH_STR, CH_THR
+	};
+	BlockPIDDfb pidStr;
+	BlockPID pidThr;
+public:
+	ControllerBoat(AP_Navigator * nav, AP_Guide * guide,
+			AP_HardwareAbstractionLayer * hal) :
+				AP_Controller(nav, guide, hal),
+				_group(k_cntrl, PSTR("CNTRL_")),
+				_mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")),
+				pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
+						steeringI, steeringD, steeringIMax, steeringYMax),
+				pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP,
+						throttleI, throttleD, throttleIMax, throttleYMax,
+						throttleDFCut) {
+		_hal->debug->println_P(PSTR("initializing boat controller"));
+
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100,
+						1500, 1900));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1540,
+						1900));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500,
+						1900));
+	}
+	virtual MAV_MODE getMode() {
+		return (MAV_MODE) _mode.get();
+	}
+	virtual void update(const float & dt) {
+
+		// check for heartbeat
+		if (_hal->heartBeatLost()) {
+			_mode = MAV_MODE_FAILSAFE;
+			setAllRadioChannelsToNeutral();
+			_hal->setState(MAV_STATE_EMERGENCY);
+			_hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n"));
+			return;
+		// if throttle less than 5% cut motor power
+		} else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) {
+			_mode = MAV_MODE_LOCKED;
+			setAllRadioChannelsToNeutral();
+			_hal->setState(MAV_STATE_STANDBY);
+			return;
+		// if in live mode then set state to active
+		} else if (_hal->getMode() == MODE_LIVE) {
+			_hal->setState(MAV_STATE_ACTIVE);
+		// if in hardware in the loop (control) mode, set to hilsim
+		} else if (_hal->getMode() == MODE_HIL_CNTL) {
+			_hal->setState(MAV_STATE_HILSIM);
+		}
+
+		// read switch to set mode
+		if (_hal->rc[CH_MODE]->getRadioPosition() > 0) {
+			_mode = MAV_MODE_MANUAL;
+		} else {
+			_mode = MAV_MODE_AUTO;
+		}
+
+		// manual mode
+		switch (_mode) {
+
+		case MAV_MODE_MANUAL: {
+			setAllRadioChannelsManually();
+			//_hal->debug->println("manual");
+			break;
+		}
+		case MAV_MODE_AUTO: {
+			float headingError = _guide->getHeadingCommand()
+					- _nav->getYaw();
+			if (headingError > 180 * deg2Rad)
+				headingError -= 360 * deg2Rad;
+			if (headingError < -180 * deg2Rad)
+				headingError += 360 * deg2Rad;
+			_hal->rc[CH_STR]->setPosition(
+					pidStr.update(headingError, _nav->getYawRate(), dt));
+			_hal->rc[CH_THR]->setPosition(
+					pidThr.update(
+							_guide->getGroundSpeedCommand()
+									- _nav->getGroundSpeed(), dt));
+			//_hal->debug->println("automode");
+			break;
+		}
+
+		default: {
+			setAllRadioChannelsToNeutral();
+			_mode = MAV_MODE_FAILSAFE;
+			_hal->setState(MAV_STATE_EMERGENCY);
+			_hal->debug->printf_P(PSTR("unknown controller mode\n"));
+			break;
+		}
+
+		}
+	}
+};
+
+} // namespace apo
+
+#endif /* CONTROLLERBOAT_H_ */

ArduBoat/Makefile

@@ -0,0 +1 @@
+include ../libraries/AP_Common/Arduino.mk

ArduCopter/.gitignore

@@ -0,0 +1 @@
+arducopter.cpp

ArduCopter/CMakeLists.txt

@@ -42,7 +42,7 @@ PRINT_BOARD_SETTINGS(mega2560)
 #====================================================================#
 set(FIRMWARE_NAME arducopter)
 
-set(${FIRMWARE_NAME}_BOARD mega2560)               # Arduino Target board
+set(${FIRMWARE_NAME}_BOARD ${BOARD})               # Arduino Target board
 
 set(${FIRMWARE_NAME}_SKETCHES
     ArduCopter.pde
@@ -126,8 +126,6 @@ set(${FIRMWARE_NAME}_LIBS
         c
         m
 )
-SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX)
-
 	
 #${CONSOLE_PORT}
 set(${FIRMWARE_NAME}_PORT COM2)            # Serial upload port

ArduPlane/.gitignore

@@ -0,0 +1 @@
+ArduPlane.cpp

ArduPlane/CMakeLists.txt

@@ -42,7 +42,7 @@ PRINT_BOARD_SETTINGS(mega2560)
 #====================================================================#
 set(FIRMWARE_NAME ArduPlane)
 
-set(${FIRMWARE_NAME}_BOARD mega2560)               # Arduino Target board
+set(${FIRMWARE_NAME}_BOARD ${BOARD})               # Arduino Target board
 
 set(${FIRMWARE_NAME}_SKETCHES
     ArduPlane.pde
@@ -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
+	#HIL.h
-	Mavlink_Common.h
+	#Mavlink_Common.h
 	Parameters.h
 	)  # Firmware sources
 
@@ -126,8 +126,6 @@ set(${FIRMWARE_NAME}_LIBS
         c
         m
 )
-SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX)
-
 	
 #${CONSOLE_PORT}
 set(${FIRMWARE_NAME}_PORT COM2)            # Serial upload port

ArduRover/ArduRover.pde

@@ -0,0 +1,21 @@
+// Libraries
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <APM_RC.h>
+#include <AP_RangeFinder.h>
+#include <GCS_MAVLink.h>
+#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>
+#include <ModeFilter.h>
+#include <APO.h>
+
+// Vehicle Configuration
+#include "CarStampede.h"
+
+// ArduPilotOne Default Setup
+#include "APO_DefaultSetup.h"

ArduRover/CMakeLists.txt

@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 2.6)
+set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../")
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain
+set(CMAKE_MODULE_PATH    ${CMAKE_SOURCE_DIR}/cmake/modules)  # CMake module search path
+
+string(REGEX REPLACE ".*/" "" PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR})
+project(${PROJECT_NAME} C CXX)
+
+set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde)
+
+find_package(Arduino 22 REQUIRED)
+
+if (NOT DEFINED BOARD)
+	message(STATUS "board not defined, assuming mega, use cmake -DBOARD=mega2560 , etc. to specify")
+	set(BOARD "mega")
+endif()
+message(STATUS "Board configured as: ${BOARD}")
+
+# need to configure based on host operating system
+set(${PROJECT_NAME}_PORT COM2)
+set(${PROJECT_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X )
+	
+include_directories(
+${ARDUINO_LIBRARIES_PATH}/Wire
+${CMAKE_SOURCE_DIR}/libraries/APO
+${CMAKE_SOURCE_DIR}/libraries/AP_Common
+${CMAKE_SOURCE_DIR}/libraries/FastSerial
+${CMAKE_SOURCE_DIR}/libraries/ModeFilter
+${CMAKE_SOURCE_DIR}/libraries/AP_Compass
+${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder
+${CMAKE_SOURCE_DIR}/libraries/AP_GPS
+${CMAKE_SOURCE_DIR}/libraries/AP_IMU
+${CMAKE_SOURCE_DIR}/libraries/AP_ADC
+${CMAKE_SOURCE_DIR}/libraries/AP_DCM
+${CMAKE_SOURCE_DIR}/libraries/APM_RC
+${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink
+${CMAKE_SOURCE_DIR}/libraries/APM_BMP085
+)
+
+add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs")
+
+set(${PROJECT_NAME}_BOARD ${BOARD})
+file(GLOB ${PROJECT_NAME}_SKETCHES *.pde)
+file(GLOB ${PROJECT_NAME}_SRCS *.cpp)
+file(GLOB ${PROJECT_NAME}_HDRS *.h)
+set(${PROJECT_NAME}_LIBS
+	c
+    m
+    APO
+    FastSerial
+    AP_Common
+    GCS_MAVLink
+    AP_GPS
+    APM_RC
+    AP_DCM
+    AP_ADC
+    AP_Compass
+    AP_IMU
+    AP_RangeFinder
+    APM_BMP085
+	ModeFilter
+)
+
+generate_arduino_firmware(${PROJECT_NAME})
+
+install(FILES
+	${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.hex 
+	DESTINATION bin
+	)

ArduRover/CarStampede.h

@@ -0,0 +1,69 @@
+/*
+ * CarStampede.h
+ *
+ *  Created on: May 1, 2011
+ *      Author: jgoppert
+ *
+ */
+
+#ifndef CARSTAMPEDE_H_
+#define CARSTAMPEDE_H_
+
+// vehicle options
+static const apo::vehicle_t vehicle = apo::VEHICLE_CAR;
+static const apo::halMode_t halMode = apo::MODE_LIVE;
+static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560;
+static const uint8_t heartBeatTimeout = 3;
+
+// algorithm selection
+#define CONTROLLER_CLASS ControllerCar
+#define GUIDE_CLASS MavlinkGuide
+#define NAVIGATOR_CLASS DcmNavigator
+#define COMMLINK_CLASS MavlinkComm
+
+// hardware selection
+#define ADC_CLASS AP_ADC_ADS7844
+#define COMPASS_CLASS AP_Compass_HMC5843
+#define BARO_CLASS APM_BMP085_Class
+#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
+#define DEBUG_BAUD 57600
+#define TELEM_BAUD 57600
+#define GPS_BAUD 38400
+#define HIL_BAUD 57600
+
+// optional sensors
+static bool gpsEnabled = false;
+static bool baroEnabled = true;
+static bool compassEnabled = true;
+
+static bool rangeFinderFrontEnabled = true;
+static bool rangeFinderBackEnabled = true;
+static bool rangeFinderLeftEnabled = true;
+static bool rangeFinderRightEnabled = true;
+static bool rangeFinderUpEnabled = true;
+static bool rangeFinderDownEnabled = true;
+
+// loop rates
+static const float loop0Rate = 150;
+static const float loop1Rate = 100;
+static const float loop2Rate = 10;
+static const float loop3Rate = 1;
+static const float loop4Rate = 0.1;
+
+// gains
+static const float steeringP = 1.0;
+static const float steeringI = 0.0;
+static const float steeringD = 0.0;
+static const float steeringIMax = 0.0;
+static const float steeringYMax = 3.0;
+
+static const float throttleP = 0.0;
+static const float throttleI = 0.0;
+static const float throttleD = 0.0;
+static const float throttleIMax = 0.0;
+static const float throttleYMax = 0.0;
+static const float throttleDFCut = 3.0;
+
+#include "ControllerCar.h"
+
+#endif /* CARSTAMPEDE_H_ */

ArduRover/ControllerCar.h

@@ -0,0 +1,125 @@
+/*
+ * ControllerCar.h
+ *
+ *  Created on: Jun 30, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef CONTROLLERCAR_H_
+#define CONTROLLERCAR_H_
+
+#include "../APO/AP_Controller.h"
+
+namespace apo {
+
+class ControllerCar: public AP_Controller {
+private:
+	AP_Var_group _group;
+	AP_Uint8 _mode;
+	enum {
+		k_chMode = k_radioChannelsStart, k_chStr, k_chThr
+	};
+	enum {
+		k_pidStr = k_controllersStart, k_pidThr
+	};
+	enum {
+		CH_MODE = 0, CH_STR, CH_THR
+	};
+	BlockPIDDfb pidStr;
+	BlockPID pidThr;
+public:
+	ControllerCar(AP_Navigator * nav, AP_Guide * guide,
+			AP_HardwareAbstractionLayer * hal) :
+				AP_Controller(nav, guide, hal),
+				_group(k_cntrl, PSTR("CNTRL_")),
+				_mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")),
+				pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
+						steeringI, steeringD, steeringIMax, steeringYMax),
+				pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP,
+						throttleI, throttleD, throttleIMax, throttleYMax,
+						throttleDFCut) {
+		_hal->debug->println_P(PSTR("initializing car controller"));
+
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100,
+						1500, 1900));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1540,
+						1900));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500,
+						1900));
+	}
+	virtual MAV_MODE getMode() {
+		return (MAV_MODE) _mode.get();
+	}
+	virtual void update(const float & dt) {
+
+		// check for heartbeat
+		if (_hal->heartBeatLost()) {
+			_mode = MAV_MODE_FAILSAFE;
+			setAllRadioChannelsToNeutral();
+			_hal->setState(MAV_STATE_EMERGENCY);
+			_hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n"));
+			return;
+		// if throttle less than 5% cut motor power
+		} else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) {
+			_mode = MAV_MODE_LOCKED;
+			setAllRadioChannelsToNeutral();
+			_hal->setState(MAV_STATE_STANDBY);
+			return;
+		// if in live mode then set state to active
+		} else if (_hal->getMode() == MODE_LIVE) {
+			_hal->setState(MAV_STATE_ACTIVE);
+		// if in hardware in the loop (control) mode, set to hilsim
+		} else if (_hal->getMode() == MODE_HIL_CNTL) {
+			_hal->setState(MAV_STATE_HILSIM);
+		}
+		
+		// read switch to set mode
+		if (_hal->rc[CH_MODE]->getRadioPosition() > 0) {
+			_mode = MAV_MODE_MANUAL;
+		} else {
+			_mode = MAV_MODE_AUTO;
+		}
+
+		// manual mode
+		switch (_mode) {
+
+		case MAV_MODE_MANUAL: {
+			setAllRadioChannelsManually();
+			//_hal->debug->println("manual");
+			break;
+		}
+		case MAV_MODE_AUTO: {
+			float headingError = _guide->getHeadingCommand()
+					- _nav->getYaw();
+			if (headingError > 180 * deg2Rad)
+				headingError -= 360 * deg2Rad;
+			if (headingError < -180 * deg2Rad)
+				headingError += 360 * deg2Rad;
+			_hal->rc[CH_STR]->setPosition(
+					pidStr.update(headingError, _nav->getYawRate(), dt));
+			_hal->rc[CH_THR]->setPosition(
+					pidThr.update(
+							_guide->getGroundSpeedCommand()
+									- _nav->getGroundSpeed(), dt));
+			//_hal->debug->println("automode");
+			break;
+		}
+
+		default: {
+			setAllRadioChannelsToNeutral();
+			_mode = MAV_MODE_FAILSAFE;
+			_hal->setState(MAV_STATE_EMERGENCY);
+			_hal->debug->printf_P(PSTR("unknown controller mode\n"));
+			break;
+		}
+
+		}
+	}
+};
+
+} // namespace apo
+
+#endif /* CONTROLLERCAR_H_ */

ArduRover/ControllerTank.h

@@ -0,0 +1,133 @@
+/*
+ * ControllerTank.h
+ *
+ *  Created on: Jun 30, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef CONTROLLERTANK_H_
+#define CONTROLLERTANK_H_
+
+#include "../APO/AP_Controller.h"
+
+namespace apo {
+
+class ControllerTank: public AP_Controller {
+private:
+	AP_Var_group _group;
+	AP_Uint8 _mode;
+	enum {
+		k_chMode = k_radioChannelsStart, k_chLeft, k_chRight, k_chStr, k_chThr
+	};
+	enum {
+		k_pidStr = k_controllersStart, k_pidThr
+	};
+	enum {
+		CH_MODE = 0, CH_LEFT, CH_RIGHT, CH_STR, CH_THR
+	};
+	BlockPIDDfb pidStr;
+	BlockPID pidThr;
+public:
+	ControllerTank(AP_Navigator * nav, AP_Guide * guide,
+			AP_HardwareAbstractionLayer * hal) :
+				AP_Controller(nav, guide, hal),
+				_group(k_cntrl, PSTR("CNTRL_")),
+				_mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")),
+				pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
+						steeringI, steeringD, steeringIMax, steeringYMax),
+				pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP,
+						throttleI, throttleD, throttleIMax, throttleYMax,
+						throttleDFCut) {
+		_hal->debug->println_P(PSTR("initializing tank controller"));
+
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100,
+						1500, 1900, RC_MODE_IN));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, 1500,
+						1900, RC_MODE_OUT));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, 1500,
+						1900, RC_MODE_OUT));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1500,
+						1900, RC_MODE_IN));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500,
+						1900, RC_MODE_IN));
+	}
+	virtual MAV_MODE getMode() {
+		return (MAV_MODE) _mode.get();
+	}
+	void mix(float headingOutput, float throttleOutput) {
+		_hal->rc[CH_LEFT]->setPosition(throttleOutput + headingOutput);
+		_hal->rc[CH_RIGHT]->setPosition(throttleOutput - headingOutput);
+	}
+	virtual void update(const float & dt) {
+
+		// check for heartbeat
+		if (_hal->heartBeatLost()) {
+			_mode = MAV_MODE_FAILSAFE;
+			setAllRadioChannelsToNeutral();
+			_hal->setState(MAV_STATE_EMERGENCY);
+			_hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n"));
+			return;
+		// if throttle less than 5% cut motor power
+		} else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) {
+			_mode = MAV_MODE_LOCKED;
+			setAllRadioChannelsToNeutral();
+			_hal->setState(MAV_STATE_STANDBY);
+			return;
+		// if in live mode then set state to active
+		} else if (_hal->getMode() == MODE_LIVE) {
+			_hal->setState(MAV_STATE_ACTIVE);
+		// if in hardware in the loop (control) mode, set to hilsim
+		} else if (_hal->getMode() == MODE_HIL_CNTL) {
+			_hal->setState(MAV_STATE_HILSIM);
+		}
+		
+		// read switch to set mode
+		if (_hal->rc[CH_MODE]->getRadioPosition() > 0) {
+			_mode = MAV_MODE_MANUAL;
+		} else {
+			_mode = MAV_MODE_AUTO;
+		}
+
+		// manual mode
+		switch (_mode) {
+
+		case MAV_MODE_MANUAL: {
+			setAllRadioChannelsManually();
+			mix(_hal->rc[CH_STR]->getPosition(),
+					_hal->rc[CH_THR]->getPosition());
+			break;
+		}
+		case MAV_MODE_AUTO: {
+			float headingError = _guide->getHeadingCommand()
+					- _nav->getYaw();
+			if (headingError > 180 * deg2Rad)
+				headingError -= 360 * deg2Rad;
+			if (headingError < -180 * deg2Rad)
+				headingError += 360 * deg2Rad;
+			mix(pidStr.update(headingError, _nav->getYawRate(), dt),
+				pidThr.update(_guide->getGroundSpeedCommand()
+					- _nav->getGroundSpeed(), dt));
+			//_hal->debug->println("automode");
+			break;
+		}
+
+		default: {
+			setAllRadioChannelsToNeutral();
+			_mode = MAV_MODE_FAILSAFE;
+			_hal->setState(MAV_STATE_EMERGENCY);
+			_hal->debug->printf_P(PSTR("unknown controller mode\n"));
+			break;
+		}
+
+		}
+	}
+};
+
+} // namespace apo
+
+#endif /* CONTROLLERTANK_H_ */

ArduRover/Makefile

@@ -0,0 +1 @@
+include ../libraries/AP_Common/Arduino.mk

ArduRover/TankGeneric.h

@@ -0,0 +1,68 @@
+/*
+ * TankGeneric.h
+ *
+ *  Created on: Sep 26, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef TANKGENERIC_H_
+#define TANKGENERIC_H_
+
+// vehicle options
+static const apo::vehicle_t vehicle = apo::VEHICLE_TANK;
+static const apo::halMode_t halMode = apo::MODE_LIVE;
+static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560;
+static const uint8_t heartBeatTimeout = 3;
+
+// algorithm selection
+#define CONTROLLER_CLASS ControllerTank
+#define GUIDE_CLASS MavlinkGuide
+#define NAVIGATOR_CLASS DcmNavigator
+#define COMMLINK_CLASS MavlinkComm
+
+// hardware selection
+#define ADC_CLASS AP_ADC_ADS7844
+#define COMPASS_CLASS AP_Compass_HMC5843
+#define BARO_CLASS APM_BMP085_Class
+#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
+#define DEBUG_BAUD 57600
+#define TELEM_BAUD 57600
+#define GPS_BAUD 38400
+#define HIL_BAUD 57600
+
+// optional sensors
+static bool gpsEnabled = false;
+static bool baroEnabled = true;
+static bool compassEnabled = true;
+
+static bool rangeFinderFrontEnabled = true;
+static bool rangeFinderBackEnabled = true;
+static bool rangeFinderLeftEnabled = true;
+static bool rangeFinderRightEnabled = true;
+static bool rangeFinderUpEnabled = true;
+static bool rangeFinderDownEnabled = true;
+
+// loop rates
+static const float loop0Rate = 150;
+static const float loop1Rate = 100;
+static const float loop2Rate = 10;
+static const float loop3Rate = 1;
+static const float loop4Rate = 0.1;
+
+// gains
+const float steeringP = 1.0;
+const float steeringI = 0.0;
+const float steeringD = 0.0;
+const float steeringIMax = 0.0;
+const float steeringYMax = 3.0;
+
+const float throttleP = 0.0;
+const float throttleI = 0.0;
+const float throttleD = 0.0;
+const float throttleIMax = 0.0;
+const float throttleYMax = 0.0;
+const float throttleDFCut = 3.0;
+
+#include "ControllerTank.h"
+
+#endif /* TANKGENERIC_H_ */

apo/.gitignore

@@ -0,0 +1 @@
+apo.cpp

apo/CMakeLists.txt

@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 2.6)
+set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../")
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain
+set(CMAKE_MODULE_PATH    ${CMAKE_SOURCE_DIR}/cmake/modules)  # CMake module search path
+
+string(REGEX REPLACE ".*/" "" PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR})
+project(${PROJECT_NAME} C CXX)
+
+set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde)
+
+find_package(Arduino 22 REQUIRED)
+
+if (NOT DEFINED BOARD)
+	message(STATUS "board not defined, assuming mega, use cmake -DBOARD=mega2560 , etc. to specify")
+	set(BOARD "mega")
+endif()
+message(STATUS "Board configured as: ${BOARD}")
+
+# need to configure based on host operating system
+set(${PROJECT_NAME}_PORT COM2)
+set(${PROJECT_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X )
+	
+include_directories(
+${ARDUINO_LIBRARIES_PATH}/Wire
+${CMAKE_SOURCE_DIR}/libraries/APO
+${CMAKE_SOURCE_DIR}/libraries/AP_Common
+${CMAKE_SOURCE_DIR}/libraries/FastSerial
+${CMAKE_SOURCE_DIR}/libraries/ModeFilter
+${CMAKE_SOURCE_DIR}/libraries/AP_Compass
+${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder
+${CMAKE_SOURCE_DIR}/libraries/AP_GPS
+${CMAKE_SOURCE_DIR}/libraries/AP_IMU
+${CMAKE_SOURCE_DIR}/libraries/AP_ADC
+${CMAKE_SOURCE_DIR}/libraries/AP_DCM
+${CMAKE_SOURCE_DIR}/libraries/APM_RC
+${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink
+${CMAKE_SOURCE_DIR}/libraries/APM_BMP085
+)
+
+add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs")
+
+set(${PROJECT_NAME}_BOARD ${BOARD})
+file(GLOB ${PROJECT_NAME}_SKETCHES *.pde)
+file(GLOB ${PROJECT_NAME}_SRCS *.cpp)
+file(GLOB ${PROJECT_NAME}_HDRS *.h)
+set(${PROJECT_NAME}_LIBS
+	c
+    m
+    APO
+    FastSerial
+    AP_Common
+    GCS_MAVLink
+    AP_GPS
+    APM_RC
+    AP_DCM
+    AP_ADC
+    AP_Compass
+    AP_IMU
+    AP_RangeFinder
+    APM_BMP085
+	ModeFilter
+)
+
+generate_arduino_firmware(${PROJECT_NAME})
+
+install(FILES
+	${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.hex 
+	DESTINATION bin
+	)

apo/ControllerPlane.h

@@ -0,0 +1,215 @@
+/*
+ * ControllerPlane.h
+ *
+ *  Created on: Jun 30, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef CONTROLLERPLANE_H_
+#define CONTROLLERPLANE_H_
+
+#include "../APO/AP_Controller.h"
+
+namespace apo {
+
+class ControllerPlane: public AP_Controller {
+private:
+	AP_Var_group _group;
+	AP_Var_group _trimGroup;
+	AP_Uint8 _mode;
+	AP_Uint8 _rdrAilMix;
+	bool _needsTrim;
+	AP_Float _ailTrim;
+	AP_Float _elvTrim;
+	AP_Float _rdrTrim;
+	AP_Float _thrTrim;
+	enum {
+		ch_mode = 0, ch_roll, ch_pitch, ch_thr, ch_yaw
+	};
+	enum {
+		k_chMode = k_radioChannelsStart,
+		k_chRoll,
+		k_chPitch,
+		k_chYaw,
+		k_chThr,
+
+		k_pidBnkRll = k_controllersStart,
+		k_pidSpdPit,
+		k_pidPitPit,
+		k_pidYwrYaw,
+		k_pidHdgBnk,
+		k_pidAltThr,
+
+		k_trim = k_customStart
+	};
+	BlockPID pidBnkRll; // bank error to roll servo deflection
+	BlockPID pidSpdPit; // speed error to pitch command
+	BlockPID pidPitPit; // pitch error to pitch servo deflection
+	BlockPID pidYwrYaw; // yaw rate error to yaw servo deflection
+	BlockPID pidHdgBnk; // heading error to bank command
+	BlockPID pidAltThr; // altitude error to throttle deflection
+	bool requireRadio;
+
+public:
+	ControllerPlane(AP_Navigator * nav, AP_Guide * guide,
+			AP_HardwareAbstractionLayer * hal) :
+				AP_Controller(nav, guide, hal),
+				_group(k_cntrl, PSTR("cntrl_")),
+				_trimGroup(k_trim, PSTR("trim_")),
+				_mode(&_group, 1, MAV_MODE_UNINIT, PSTR("mode")),
+				_rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")),
+				_needsTrim(false),
+				_ailTrim(&_trimGroup, 1, ailTrim, PSTR("ail")),
+				_elvTrim(&_trimGroup, 2, elvTrim, PSTR("elv")),
+				_rdrTrim(&_trimGroup, 3, rdrTrim, PSTR("rdr")),
+				_thrTrim(&_trimGroup, 4, thrTrim, PSTR("thr")),
+				pidBnkRll(new AP_Var_group(k_pidBnkRll, PSTR("bnkRll_")), 1,
+						pidBnkRllP, pidBnkRllI, pidBnkRllD, pidBnkRllAwu,
+						pidBnkRllLim, pidBnkRllDFCut),
+				pidPitPit(new AP_Var_group(k_pidPitPit, PSTR("pitPit_")), 1,
+						pidPitPitP, pidPitPitI, pidPitPitD, pidPitPitAwu,
+						pidPitPitLim, pidPitPitDFCut),
+				pidSpdPit(new AP_Var_group(k_pidSpdPit, PSTR("spdPit_")), 1,
+						pidSpdPitP, pidSpdPitI, pidSpdPitD, pidSpdPitAwu,
+						pidSpdPitLim, pidSpdPitDFCut),
+				pidYwrYaw(new AP_Var_group(k_pidYwrYaw, PSTR("ywrYaw_")), 1,
+						pidYwrYawP, pidYwrYawI, pidYwrYawD, pidYwrYawAwu,
+						pidYwrYawLim, pidYwrYawDFCut),
+				pidHdgBnk(new AP_Var_group(k_pidHdgBnk, PSTR("hdgBnk_")), 1,
+						pidHdgBnkP, pidHdgBnkI, pidHdgBnkD, pidHdgBnkAwu,
+						pidHdgBnkLim, pidHdgBnkDFCut),
+				pidAltThr(new AP_Var_group(k_pidAltThr, PSTR("altThr_")), 1,
+						pidAltThrP, pidAltThrI, pidAltThrD, pidAltThrAwu,
+						pidAltThrLim, pidAltThrDFCut),
+				requireRadio(false) {
+
+		_hal->debug->println_P(PSTR("initializing car controller"));
+
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100,
+						1500, 1900, RC_MODE_IN));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chRoll, PSTR("roll_"), APM_RC, 0, 1200,
+						1500, 1800, RC_MODE_INOUT));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chPitch, PSTR("pitch_"), APM_RC, 1, 1200,
+						1500, 1800, RC_MODE_INOUT));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chThr, PSTR("thr_"), APM_RC, 2, 1100, 1100,
+						1900, RC_MODE_INOUT));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chYaw, PSTR("yaw_"), APM_RC, 3, 1200, 1500,
+						1800, RC_MODE_INOUT));
+	}
+	virtual MAV_MODE getMode() {
+		return (MAV_MODE) _mode.get();
+	}
+	virtual void update(const float & dt) {
+
+		// check for heartbeat
+		if (_hal->heartBeatLost()) {
+			_mode = MAV_MODE_FAILSAFE;
+			setAllRadioChannelsToNeutral();
+			_hal->setState(MAV_STATE_EMERGENCY);
+			_hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n"));
+			return;
+		// if the value of the throttle is less than 5% cut motor power
+		} else if (requireRadio && _hal->rc[ch_thr]->getRadioPosition() < 0.05) {
+			_mode = MAV_MODE_LOCKED;
+			setAllRadioChannelsToNeutral();
+			_hal->setState(MAV_STATE_STANDBY);
+			return;
+		// if in live mode then set state to active
+		} else if (_hal->getMode() == MODE_LIVE) {
+			_hal->setState(MAV_STATE_ACTIVE);
+		// if in hardware in the loop (control) mode, set to hilsim
+		} else if (_hal->getMode() == MODE_HIL_CNTL) {
+			_hal->setState(MAV_STATE_HILSIM);
+		}
+
+		// read switch to set mode
+		if (_hal->rc[ch_mode]->getRadioPosition() > 0) {
+			_mode = MAV_MODE_MANUAL;
+		} else {
+			_mode = MAV_MODE_AUTO;
+		}
+
+		// manual mode
+		switch (_mode) {
+
+		case MAV_MODE_MANUAL: {
+			setAllRadioChannelsManually();
+
+			// force auto to read new manual trim
+			if (_needsTrim == false)
+				_needsTrim = true;
+			//_hal->debug->println("manual");
+			break;
+		}
+		case MAV_MODE_AUTO: {
+			float headingError = _guide->getHeadingCommand()
+					- _nav->getYaw();
+			if (headingError > 180 * deg2Rad)
+				headingError -= 360 * deg2Rad;
+			if (headingError < -180 * deg2Rad)
+				headingError += 360 * deg2Rad;
+
+			float aileron = pidBnkRll.update(
+					pidHdgBnk.update(headingError, dt) - _nav->getRoll(), dt);
+			float elevator = pidPitPit.update(
+					-pidSpdPit.update(
+							_guide->getAirSpeedCommand() - _nav->getAirSpeed(),
+							dt) - _nav->getPitch(), dt);
+			float rudder = pidYwrYaw.update(-_nav->getYawRate(), dt);
+			// desired yaw rate is zero, needs washout
+			float throttle = pidAltThr.update(
+					_guide->getAltitudeCommand() - _nav->getAlt(), dt);
+
+			// if needs trim
+			if (_needsTrim) {
+				// need to subtract current controller deflections so control
+				// surfaces are actually at the same position as manual flight
+				_ailTrim = _hal->rc[ch_roll]->getRadioPosition() - aileron;
+				_elvTrim = _hal->rc[ch_pitch]->getRadioPosition() - elevator;
+				_rdrTrim = _hal->rc[ch_yaw]->getRadioPosition() - rudder;
+				_thrTrim = _hal->rc[ch_thr]->getRadioPosition() - throttle;
+				_needsTrim = false;
+			}
+
+			// actuator mixing/ output
+			_hal->rc[ch_roll]->setPosition(
+					aileron + _rdrAilMix * rudder + _ailTrim);
+			_hal->rc[ch_yaw]->setPosition(rudder + _rdrTrim);
+			_hal->rc[ch_pitch]->setPosition(elevator + _elvTrim);
+			_hal->rc[ch_thr]->setPosition(throttle + _thrTrim);
+
+			//_hal->debug->println("automode");
+			
+
+			// heading debug
+//			Serial.print("heading command: "); Serial.println(_guide->getHeadingCommand());
+//			Serial.print("heading: "); Serial.println(_nav->getYaw());
+//			Serial.print("heading error: "); Serial.println(headingError);
+
+			// alt debug
+//			Serial.print("alt command: "); Serial.println(_guide->getAltitudeCommand());
+//			Serial.print("alt: "); Serial.println(_nav->getAlt());
+//			Serial.print("alt error: "); Serial.println(_guide->getAltitudeCommand() - _nav->getAlt());
+			break;
+		}
+
+		default: {
+			setAllRadioChannelsToNeutral();
+			_mode = MAV_MODE_FAILSAFE;
+			_hal->setState(MAV_STATE_EMERGENCY);
+			_hal->debug->printf_P(PSTR("unknown controller mode\n"));
+			break;
+		}
+
+		}
+	}
+};
+
+} // namespace apo
+
+#endif /* CONTROLLERPLANE_H_ */

apo/ControllerQuad.h

@@ -0,0 +1,250 @@
+/*
+ * ControllerQuad.h
+ *
+ *  Created on: Jun 30, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef CONTROLLERQUAD_H_
+#define CONTROLLERQUAD_H_
+
+#include "../APO/AP_Controller.h"
+
+namespace apo {
+
+class ControllerQuad: public AP_Controller {
+public:
+
+	/**
+	 * note that these are not the controller radio channel numbers, they are just
+	 * unique keys so they can be reaccessed from the hal rc vector
+	 */
+	enum {
+		CH_MODE = 0, // note scicoslab channels set mode, left, right, front, back order
+		CH_LEFT, // this enum must match this
+		CH_RIGHT,
+		CH_FRONT,
+		CH_BACK,
+		CH_ROLL,
+		CH_PITCH,
+		CH_YAW,
+		CH_THRUST
+	};
+
+	enum {
+		k_chMode = k_radioChannelsStart,
+		k_chLeft,
+		k_chRight,
+		k_chFront,
+		k_chBack,
+		k_chRoll,
+		k_chPitch,
+		k_chYaw,
+		k_chThr
+	};
+
+	enum {
+		k_pidGroundSpeed2Throttle = k_controllersStart,
+		k_pidStr,
+		k_pidPN,
+		k_pidPE,
+		k_pidPD,
+		k_pidRoll,
+		k_pidPitch,
+		k_pidYawRate,
+		k_pidYaw,
+	};
+
+	ControllerQuad(AP_Navigator * nav, AP_Guide * guide,
+			AP_HardwareAbstractionLayer * hal) :
+				AP_Controller(nav, guide, hal),
+				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),
+				pidPitch(new AP_Var_group(k_pidPitch, PSTR("PITCH_")), 1,
+						PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU,
+						PID_ATT_LIM),
+				pidYaw(new AP_Var_group(k_pidYaw, PSTR("YAW_")), 1,
+						PID_YAWPOS_P, PID_YAWPOS_I, PID_YAWPOS_D,
+						PID_YAWPOS_AWU, PID_YAWPOS_LIM),
+				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),
+				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),
+				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) {
+		/*
+		 * allocate radio channels
+		 * the order of the channels has to match the enumeration above
+		 */
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100,
+						1500, 1900, RC_MODE_IN));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100,
+						1100, 1900, RC_MODE_OUT));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100,
+						1100, 1900, RC_MODE_OUT));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100,
+						1100, 1900, RC_MODE_OUT));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100,
+						1100, 1900, RC_MODE_OUT));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100,
+						1500, 1900, RC_MODE_IN));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100,
+						1500, 1900, RC_MODE_IN));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 2, 1100, 1500,
+						1900, RC_MODE_IN));
+		_hal->rc.push_back(
+				new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 3, 1100,
+						1100, 1900, RC_MODE_IN));
+	}
+
+	virtual void update(const float & dt) {
+
+		// check for heartbeat
+		if (_hal->heartBeatLost()) {
+			_mode = MAV_MODE_FAILSAFE;
+			setAllRadioChannelsToNeutral();
+			_hal->setState(MAV_STATE_EMERGENCY);
+			_hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n"));
+			return;
+		// if throttle less than 5% cut motor power
+		} else if (_hal->rc[CH_THRUST]->getRadioPosition() < 0.05) {
+			_mode = MAV_MODE_LOCKED;
+			setAllRadioChannelsToNeutral();
+			_hal->setState(MAV_STATE_STANDBY);
+			return;
+		// if in live mode then set state to active
+		} else if (_hal->getMode() == MODE_LIVE) {
+			_hal->setState(MAV_STATE_ACTIVE);
+		// if in hardware in the loop (control) mode, set to hilsim
+		} else if (_hal->getMode() == MODE_HIL_CNTL) {
+			_hal->setState(MAV_STATE_HILSIM);
+		}
+
+		// manual mode
+		float mixRemoteWeight = 0;
+		if (_hal->rc[CH_MODE]->getRadioPosition() > 0) {
+			mixRemoteWeight = 1;
+			_mode = MAV_MODE_MANUAL;
+		} else {
+			_mode = MAV_MODE_AUTO;
+		}
+
+		// commands for inner loop
+		float cmdRoll = 0;
+		float cmdPitch = 0;
+		float cmdYawRate = 0;
+		float thrustMix = 0;
+
+		switch(_mode) {
+
+		case MAV_MODE_MANUAL: {
+			setAllRadioChannelsManually();
+			// "mix manual"
+			cmdRoll = 0.5 * _hal->rc[CH_ROLL]->getPosition()
+					* mixRemoteWeight;
+			cmdPitch = 0.5 * _hal->rc[CH_PITCH]->getPosition()
+					* mixRemoteWeight;
+			cmdYawRate = 0.5 * _hal->rc[CH_YAW]->getPosition()
+					* mixRemoteWeight;
+			thrustMix = _hal->rc[CH_THRUST]->getPosition() * mixRemoteWeight;
+			break;
+		}
+
+		case MAV_MODE_AUTO: {
+
+			// XXX kills all commands, 
+			// auto not currently implemented
+
+			// position loop
+			/*
+			 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);
+
+			 // "transform-to-body"
+			 {
+			 float trigSin = sin(-yaw);
+			 float trigCos = cos(-yaw);
+			 _cmdPitch = _cmdEastTilt * trigCos
+			 - _cmdNorthTilt * trigSin;
+			 _cmdRoll = -_cmdEastTilt * trigSin
+			 + _cmdNorthTilt * trigCos;
+			 // note that the north tilt is negative of the pitch
+			 }
+
+			 //thrustMix += THRUST_HOVER_OFFSET;
+
+			 // "thrust-trim-adjust"
+			 if (fabs(_cmdRoll) > 0.5) {
+			 _thrustMix *= 1.13949393;
+			 } else {
+			 _thrustMix /= cos(_cmdRoll);
+			 }
+			 if (fabs(_cmdPitch) > 0.5) {
+			 _thrustMix *= 1.13949393;
+			 } else {
+			 _thrustMix /= cos(_cmdPitch);
+			 }
+			 */
+		}
+
+		}
+
+		// attitude loop
+		// XXX negative sign added to nav roll, not sure why this is necessary
+		// XXX negative sign added to nav roll rate, not sure why this is necessary
+		float rollMix = pidRoll.update(cmdRoll + _nav->getRoll(),
+				-_nav->getRollRate(), dt);
+		// XXX negative sign added to cmdPitch, not sure why this is necessary
+		float pitchMix = pidPitch.update(-cmdPitch - _nav->getPitch(),
+				_nav->getPitchRate(), dt);
+		// XXX negative sign added to cmdYawRate, not sure why this is necessary
+		float yawMix = pidYawRate.update(-cmdYawRate - _nav->getYawRate(), dt);
+
+		_hal->rc[CH_LEFT]->setPosition(thrustMix + rollMix + yawMix);
+		_hal->rc[CH_RIGHT]->setPosition(thrustMix - rollMix + yawMix);
+		_hal->rc[CH_FRONT]->setPosition(thrustMix + pitchMix - yawMix);
+		_hal->rc[CH_BACK]->setPosition(thrustMix - pitchMix - yawMix);
+
+		//		_hal->debug->printf("L: %f\t R: %f\t F: %f\t B: %f\n",
+		//				_hal->rc[CH_LEFT]->getPosition(),
+		//				_hal->rc[CH_RIGHT]->getPosition(),
+		//				_hal->rc[CH_FRONT]->getPosition(),
+		//				_hal->rc[CH_BACK]->getPosition());
+
+		_hal->debug->printf(
+				"rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n",
+				rollMix, pitchMix, yawMix, thrustMix);
+
+		//			_hal->debug->printf("roll pwm: %d\t pitch pwm: %d\t yaw pwm: %d\t thrust pwm: %d\n",
+		//					_hal->rc[CH_ROLL]->readRadio(),
+		//					_hal->rc[CH_PITCH]->readRadio(),
+		//					_hal->rc[CH_YAW]->readRadio(),
+		//					_hal->rc[CH_THRUST]->readRadio());
+	}
+	virtual MAV_MODE getMode() {
+		return (MAV_MODE) _mode.get();
+	}
+private:
+	AP_Uint8 _mode;
+	BlockPIDDfb pidRoll, pidPitch, pidYaw;
+	BlockPID pidYawRate;
+	BlockPIDDfb pidPN, pidPE, pidPD;
+
+};
+
+} // namespace apo
+
+#endif /* CONTROLLERQUAD_H_ */

apo/Makefile

@@ -0,0 +1 @@
+include ../libraries/AP_Common/Arduino.mk

apo/PlaneEasystar.h

@@ -0,0 +1,113 @@
+/*
+ * PlaneEasystar.h
+ *
+ *  Created on: May 1, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef PLANEEASYSTAR_H_
+#define PLANEEASYSTAR_H_
+
+
+// vehicle options
+static const apo::vehicle_t vehicle = apo::VEHICLE_PLANE;
+//static const apo::halMode_t halMode = apo::MODE_LIVE; // live mode, actual flight
+static const apo::halMode_t halMode = apo::MODE_HIL_CNTL; // hardware in the loop, control level
+static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
+static const uint8_t heartBeatTimeout = 3;
+
+// algorithm selection
+#define CONTROLLER_CLASS ControllerPlane
+#define GUIDE_CLASS MavlinkGuide
+#define NAVIGATOR_CLASS DcmNavigator
+#define COMMLINK_CLASS MavlinkComm
+
+// hardware selection
+#define ADC_CLASS AP_ADC_ADS7844
+#define COMPASS_CLASS AP_Compass_HMC5843
+#define BARO_CLASS APM_BMP085_Class
+#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
+#define DEBUG_BAUD 57600
+#define TELEM_BAUD 57600
+#define GPS_BAUD 38400
+#define HIL_BAUD 57600
+
+// optional sensors
+static bool gpsEnabled = false;
+static bool baroEnabled = true;
+static bool compassEnabled = true;
+
+static bool rangeFinderFrontEnabled = true;
+static bool rangeFinderBackEnabled = true;
+static bool rangeFinderLeftEnabled = true;
+static bool rangeFinderRightEnabled = true;
+static bool rangeFinderUpEnabled = true;
+static bool rangeFinderDownEnabled = true;
+
+// loop rates
+static const float loop0Rate = 150;
+static const float loop1Rate = 100;
+static const float loop2Rate = 10;
+static const float loop3Rate = 1;
+static const float loop4Rate = 0.1;
+
+// gains
+static const float rdrAilMix = 1.0; // since there are no ailerons
+
+// bank error to roll servo
+static const float pidBnkRllP = -0.5;
+static const float pidBnkRllI = 0.0;
+static const float pidBnkRllD = 0.0;
+static const float pidBnkRllAwu = 0.0;
+static const float pidBnkRllLim = 1.0;
+static const float pidBnkRllDFCut = 0.0;
+
+// pitch error to pitch servo
+static const float pidPitPitP = -1;
+static const float pidPitPitI = 0.0;
+static const float pidPitPitD = 0.0;
+static const float pidPitPitAwu = 0.0;
+static const float pidPitPitLim = 1.0;
+static const float pidPitPitDFCut = 0.0;
+
+// speed error to pitch command
+static const float pidSpdPitP = 0.1;
+static const float pidSpdPitI = 0.0;
+static const float pidSpdPitD = 0.0;
+static const float pidSpdPitAwu = 0.0;
+static const float pidSpdPitLim = 1.0;
+static const float pidSpdPitDFCut = 0.0;
+
+// yaw rate error to yaw servo
+static const float pidYwrYawP = -0.2;
+static const float pidYwrYawI = 0.0;
+static const float pidYwrYawD = 0.0;
+static const float pidYwrYawAwu = 0.0;
+static const float pidYwrYawLim = 1.0;
+static const float pidYwrYawDFCut = 0.0;
+
+// heading error to bank angle command
+static const float pidHdgBnkP = 1.0;
+static const float pidHdgBnkI = 0.0;
+static const float pidHdgBnkD = 0.0;
+static const float pidHdgBnkAwu = 0.0;
+static const float pidHdgBnkLim = 0.5;
+static const float pidHdgBnkDFCut = 0.0;
+
+// altitude error to throttle command
+static const float pidAltThrP = .01;
+static const float pidAltThrI = 0.0;
+static const float pidAltThrD = 0.0;
+static const float pidAltThrAwu = 0.0;
+static const float pidAltThrLim = 1;
+static const float pidAltThrDFCut = 0.0;
+
+// trim control positions (-1,1)
+static const float ailTrim = 0.0;
+static const float elvTrim = 0.0;
+static const float rdrTrim = 0.0;
+static const float thrTrim = 0.5;
+
+#include "ControllerPlane.h"
+
+#endif /* PLANEEASYSTAR_H_ */

apo/QuadArducopter.h

@@ -0,0 +1,98 @@
+/*
+ * QuadArducopter.h
+ *
+ *  Created on: May 1, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef QUADARDUCOPTER_H_
+#define QUADARDUCOPTER_H_
+
+// vehicle options
+static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
+static const apo::halMode_t halMode = apo::MODE_LIVE;
+static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560;
+static const uint8_t heartBeatTimeout = 3;
+
+// algorithm selection
+#define CONTROLLER_CLASS ControllerQuad
+#define GUIDE_CLASS MavlinkGuide
+#define NAVIGATOR_CLASS DcmNavigator
+#define COMMLINK_CLASS MavlinkComm
+
+// hardware selection
+#define ADC_CLASS AP_ADC_ADS7844
+#define COMPASS_CLASS AP_Compass_HMC5843
+#define BARO_CLASS APM_BMP085_Class
+#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
+#define DEBUG_BAUD 57600
+#define TELEM_BAUD 57600
+#define GPS_BAUD 38400
+#define HIL_BAUD 57600
+
+// optional sensors
+static bool gpsEnabled = false;
+static bool baroEnabled = true;
+static bool compassEnabled = true;
+
+static bool rangeFinderFrontEnabled = true;
+static bool rangeFinderBackEnabled = true;
+static bool rangeFinderLeftEnabled = true;
+static bool rangeFinderRightEnabled = true;
+static bool rangeFinderUpEnabled = true;
+static bool rangeFinderDownEnabled = true;
+
+// loop rates
+static const float loop0Rate = 150;
+static const float loop1Rate = 100;
+static const float loop2Rate = 10;
+static const float loop3Rate = 1;
+static const float loop4Rate = 0.1;
+
+//motor parameters
+static const float MOTOR_MAX = 1;
+static const float MOTOR_MIN = 0.1;
+
+// position control loop
+static const float PID_POS_INTERVAL = 1 / 100; // 5 hz
+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_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_AWU = 0;
+
+// attitude control loop
+static const float PID_ATT_INTERVAL = 1 / 100; // 100 hz
+static const float PID_ATT_P = 0.1; // 0.1
+static const float PID_ATT_I = 0; // 0.0
+static const float PID_ATT_D = 0.1; // 0.1
+static const float PID_ATT_LIM = 1; // 0.01 // 10 % #define MOTORs
+static const float PID_ATT_AWU = 0; // 0.0
+static const float PID_YAWPOS_P = 0;
+static const float PID_YAWPOS_I = 0;
+static const float PID_YAWPOS_D = 0;
+static const float PID_YAWPOS_LIM = 0; // 1 rad/s
+static const float PID_YAWPOS_AWU = 0; // 1 rad/s
+static const float PID_YAWSPEED_P = .2;
+static const float PID_YAWSPEED_I = 0;
+static const float PID_YAWSPEED_D = 0;
+static const float PID_YAWSPEED_LIM = .3; // 0.01 // 10 % MOTORs
+static const float PID_YAWSPEED_AWU = 0.0;
+static const float PID_YAWSPEED_DFCUT = 3.0; // 3 Radians, about 1 Hz
+
+// mixing
+static const float MIX_REMOTE_WEIGHT = 1;
+static const float MIX_POSITION_WEIGHT = 1;
+static const float MIX_POSITION_Z_WEIGHT = 1;
+static const float MIX_POSITION_YAW_WEIGHT = 1;
+
+static const float THRUST_HOVER_OFFSET = 0.475;
+
+#include "ControllerQuad.h"
+
+#endif /* QUADARDUCOPTER_H_ */

apo/QuadMikrokopter.h

@@ -0,0 +1,98 @@
+/*
+ * QuadMikrokopter
+ *
+ *  Created on: May 1, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef QUADMIKROKOPTER_H_
+#define QUADMIKROKOPTER_H_
+
+// vehicle options
+static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
+static const apo::halMode_t halMode = apo::MODE_LIVE;
+static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560;
+static const uint8_t heartBeatTimeout = 3;
+
+// algorithm selection
+#define CONTROLLER_CLASS ControllerQuad
+#define GUIDE_CLASS MavlinkGuide
+#define NAVIGATOR_CLASS DcmNavigator
+#define COMMLINK_CLASS MavlinkComm
+
+// hardware selection
+#define ADC_CLASS AP_ADC_ADS7844
+#define COMPASS_CLASS AP_Compass_HMC5843
+#define BARO_CLASS APM_BMP085_Class
+#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
+#define DEBUG_BAUD 57600
+#define TELEM_BAUD 57600
+#define GPS_BAUD 38400
+#define HIL_BAUD 57600
+
+// optional sensors
+static bool gpsEnabled = false;
+static bool baroEnabled = true;
+static bool compassEnabled = true;
+
+static bool rangeFinderFrontEnabled = true;
+static bool rangeFinderBackEnabled = true;
+static bool rangeFinderLeftEnabled = true;
+static bool rangeFinderRightEnabled = true;
+static bool rangeFinderUpEnabled = true;
+static bool rangeFinderDownEnabled = true;
+
+// loop rates
+static const float loop0Rate = 150;
+static const float loop1Rate = 100;
+static const float loop2Rate = 10;
+static const float loop3Rate = 1;
+static const float loop4Rate = 0.1;
+
+//motor parameters
+static const float MOTOR_MAX = 1;
+static const float MOTOR_MIN = 0.1;
+
+// position control loop
+static const float PID_POS_INTERVAL = 1 / 100; // 5 hz
+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_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_AWU = 0;
+
+// attitude control loop
+static const float PID_ATT_INTERVAL = 1 / 100; // 100 hz
+static const float PID_ATT_P = 0.1; // 0.1
+static const float PID_ATT_I = 0; // 0.0
+static const float PID_ATT_D = 0.1; // 0.1
+static const float PID_ATT_LIM = 1; // 0.01 // 10 % #define MOTORs
+static const float PID_ATT_AWU = 0; // 0.0
+static const float PID_YAWPOS_P = 0;
+static const float PID_YAWPOS_I = 0;
+static const float PID_YAWPOS_D = 0;
+static const float PID_YAWPOS_LIM = 0; // 1 rad/s
+static const float PID_YAWPOS_AWU = 0; // 1 rad/s
+static const float PID_YAWSPEED_P = .2;
+static const float PID_YAWSPEED_I = 0;
+static const float PID_YAWSPEED_D = 0;
+static const float PID_YAWSPEED_LIM = .3; // 0.01 // 10 % MOTORs
+static const float PID_YAWSPEED_AWU = 0.0;
+static const float PID_YAWSPEED_DFCUT = 3.0; // 3 Radians, about 1 Hz
+
+// mixing
+static const float MIX_REMOTE_WEIGHT = 1;
+static const float MIX_POSITION_WEIGHT = 1;
+static const float MIX_POSITION_Z_WEIGHT = 1;
+static const float MIX_POSITION_YAW_WEIGHT = 1;
+
+static const float THRUST_HOVER_OFFSET = 0.475;
+
+#include "ControllerQuad.h"
+
+#endif /* QUADMIKROKOPTER_H_ */

apo/apo.pde

@@ -0,0 +1,22 @@
+// Libraries
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <APM_RC.h>
+#include <AP_RangeFinder.h>
+#include <GCS_MAVLink.h>
+#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>
+#include <ModeFilter.h>
+#include <APO.h>
+#include <WProgram.h>
+
+// Vehicle Configuration
+#include "PlaneEasystar.h"
+
+// ArduPilotOne Default Setup
+#include "APO_DefaultSetup.h"

cmake/arkcmake/.gitignore

@@ -0,0 +1,4 @@
+*.swp
+*.pyc
+.DS_Store
+arkcmake

cmake/arkcmake/COPYING

@@ -0,0 +1,674 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+  The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works.  By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users.  We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors.  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+  To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights.  Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received.  You must make sure that they, too, receive
+or can get the source code.  And you must show them these terms so they
+know their rights.
+
+  Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+  For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software.  For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+  Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so.  This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software.  The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable.  Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products.  If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+  Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary.  To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                       TERMS AND CONDITIONS
+
+  0. Definitions.
+
+  "This License" refers to version 3 of the GNU General Public License.
+
+  "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+  "The Program" refers to any copyrightable work licensed under this
+License.  Each licensee is addressed as "you".  "Licensees" and
+"recipients" may be individuals or organizations.
+
+  To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy.  The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+  A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+  To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy.  Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+  To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies.  Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+  An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License.  If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+  1. Source Code.
+
+  The "source code" for a work means the preferred form of the work
+for making modifications to it.  "Object code" means any non-source
+form of a work.
+
+  A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+  The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form.  A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+  The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities.  However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work.  For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+  The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+  The Corresponding Source for a work in source code form is that
+same work.
+
+  2. Basic Permissions.
+
+  All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met.  This License explicitly affirms your unlimited
+permission to run the unmodified Program.  The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work.  This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+  You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force.  You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright.  Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+  Conveying under any other circumstances is permitted solely under
+the conditions stated below.  Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+  3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+  No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+  When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+  4. Conveying Verbatim Copies.
+
+  You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+  You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+  5. Conveying Modified Source Versions.
+
+  You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+    a) The work must carry prominent notices stating that you modified
+    it, and giving a relevant date.
+
+    b) The work must carry prominent notices stating that it is
+    released under this License and any conditions added under section
+    7.  This requirement modifies the requirement in section 4 to
+    "keep intact all notices".
+
+    c) You must license the entire work, as a whole, under this
+    License to anyone who comes into possession of a copy.  This
+    License will therefore apply, along with any applicable section 7
+    additional terms, to the whole of the work, and all its parts,
+    regardless of how they are packaged.  This License gives no
+    permission to license the work in any other way, but it does not
+    invalidate such permission if you have separately received it.
+
+    d) If the work has interactive user interfaces, each must display
+    Appropriate Legal Notices; however, if the Program has interactive
+    interfaces that do not display Appropriate Legal Notices, your
+    work need not make them do so.
+
+  A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit.  Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+  6. Conveying Non-Source Forms.
+
+  You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+    a) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by the
+    Corresponding Source fixed on a durable physical medium
+    customarily used for software interchange.
+
+    b) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by a
+    written offer, valid for at least three years and valid for as
+    long as you offer spare parts or customer support for that product
+    model, to give anyone who possesses the object code either (1) a
+    copy of the Corresponding Source for all the software in the
+    product that is covered by this License, on a durable physical
+    medium customarily used for software interchange, for a price no
+    more than your reasonable cost of physically performing this
+    conveying of source, or (2) access to copy the
+    Corresponding Source from a network server at no charge.
+
+    c) Convey individual copies of the object code with a copy of the
+    written offer to provide the Corresponding Source.  This
+    alternative is allowed only occasionally and noncommercially, and
+    only if you received the object code with such an offer, in accord
+    with subsection 6b.
+
+    d) Convey the object code by offering access from a designated
+    place (gratis or for a charge), and offer equivalent access to the
+    Corresponding Source in the same way through the same place at no
+    further charge.  You need not require recipients to copy the
+    Corresponding Source along with the object code.  If the place to
+    copy the object code is a network server, the Corresponding Source
+    may be on a different server (operated by you or a third party)
+    that supports equivalent copying facilities, provided you maintain
+    clear directions next to the object code saying where to find the
+    Corresponding Source.  Regardless of what server hosts the
+    Corresponding Source, you remain obligated to ensure that it is
+    available for as long as needed to satisfy these requirements.
+
+    e) Convey the object code using peer-to-peer transmission, provided
+    you inform other peers where the object code and Corresponding
+    Source of the work are being offered to the general public at no
+    charge under subsection 6d.
+
+  A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+  A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling.  In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage.  For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product.  A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+  "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source.  The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+  If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information.  But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+  The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed.  Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+  Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+  7. Additional Terms.
+
+  "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law.  If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+  When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it.  (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.)  You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+  Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+    a) Disclaiming warranty or limiting liability differently from the
+    terms of sections 15 and 16 of this License; or
+
+    b) Requiring preservation of specified reasonable legal notices or
+    author attributions in that material or in the Appropriate Legal
+    Notices displayed by works containing it; or
+
+    c) Prohibiting misrepresentation of the origin of that material, or
+    requiring that modified versions of such material be marked in
+    reasonable ways as different from the original version; or
+
+    d) Limiting the use for publicity purposes of names of licensors or
+    authors of the material; or
+
+    e) Declining to grant rights under trademark law for use of some
+    trade names, trademarks, or service marks; or
+
+    f) Requiring indemnification of licensors and authors of that
+    material by anyone who conveys the material (or modified versions of
+    it) with contractual assumptions of liability to the recipient, for
+    any liability that these contractual assumptions directly impose on
+    those licensors and authors.
+
+  All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10.  If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term.  If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+  If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+  Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+  8. Termination.
+
+  You may not propagate or modify a covered work except as expressly
+provided under this License.  Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+  However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+  Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+  Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License.  If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+  9. Acceptance Not Required for Having Copies.
+
+  You are not required to accept this License in order to receive or
+run a copy of the Program.  Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance.  However,
+nothing other than this License grants you permission to propagate or
+modify any covered work.  These actions infringe copyright if you do
+not accept this License.  Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+  10. Automatic Licensing of Downstream Recipients.
+
+  Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License.  You are not responsible
+for enforcing compliance by third parties with this License.
+
+  An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations.  If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+  You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License.  For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+  11. Patents.
+
+  A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based.  The
+work thus licensed is called the contributor's "contributor version".
+
+  A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version.  For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+  Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+  In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement).  To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+  If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients.  "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+  If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+  A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License.  You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+  Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+  12. No Surrender of Others' Freedom.
+
+  If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all.  For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+  13. Use with the GNU Affero General Public License.
+
+  Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work.  The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+  14. Revised Versions of this License.
+
+  The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+  Each version is given a distinguishing version number.  If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation.  If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+  If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+  Later license versions may give you additional or different
+permissions.  However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+  15. Disclaimer of Warranty.
+
+  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. Limitation of Liability.
+
+  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+  17. Interpretation of Sections 15 and 16.
+
+  If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the program's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This program 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 program 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/>.
+
+Also add information on how to contact you by electronic and paper mail.
+
+  If the program does terminal interaction, make it output a short
+notice like this when it starts in an interactive mode:
+
+    <program>  Copyright (C) <year>  <name of author>
+    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, your program's commands
+might be different; for a GUI interface, you would use an "about box".
+
+  You should also get your employer (if you work as a programmer) or school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.
+For more information on this, and how to apply and follow the GNU GPL, see
+<http://www.gnu.org/licenses/>.
+
+  The GNU General Public License does not permit incorporating your program
+into proprietary programs.  If your program is a subroutine library, you
+may consider it more useful to permit linking proprietary applications with
+the library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.  But first, please read
+<http://www.gnu.org/philosophy/why-not-lgpl.html>.

cmake/arkcmake/DefineCMakeDefaults.cmake

@@ -0,0 +1,27 @@
+# Always include srcdir and builddir in include path
+# This saves typing ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY} in
+# about every subdir
+# since cmake 2.4.0
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+
+# Put the include dirs which are in the source or build tree
+# before all other include dirs, so the headers in the sources
+# are prefered over the already installed ones
+# since cmake 2.4.1
+set(CMAKE_INCLUDE_DIRECTORIES_PROJECT_BEFORE ON)
+
+# Use colored output
+# since cmake 2.4.0
+set(CMAKE_COLOR_MAKEFILE ON)
+
+# Define the generic version of the libraries here
+set(GENERIC_LIB_VERSION "0.1.0")
+set(GENERIC_LIB_SOVERSION "0")
+
+# Set the default build type to release with debug info
+if (NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE RelWithDebInfo
+    CACHE STRING
+      "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel."
+  )
+endif (NOT CMAKE_BUILD_TYPE)

cmake/arkcmake/DefineCompilerFlags.cmake

@@ -0,0 +1,71 @@
+# define system dependent compiler flags
+
+include(CheckCCompilerFlag)
+include(MacroCheckCCompilerFlagSSP)
+
+if (UNIX AND NOT WIN32)
+    #
+    # Define GNUCC compiler flags
+    #
+    if (${CMAKE_C_COMPILER_ID} MATCHES GNU)
+        # add -Wconversion ?
+        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -pedantic -pedantic-errors")
+        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Wshadow -Wmissing-prototypes -Wdeclaration-after-statement")
+        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wunused -Wfloat-equal -Wpointer-arith -Wwrite-strings -Wformat-security")
+        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wmissing-format-attribute")
+
+        # with -fPIC
+        check_c_compiler_flag("-fPIC" WITH_FPIC)
+        if (WITH_FPIC)
+            set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC")
+        endif (WITH_FPIC)
+
+        check_c_compiler_flag_ssp("-fstack-protector" WITH_STACK_PROTECTOR)
+        if (WITH_STACK_PROTECTOR)
+            set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fstack-protector")
+        endif (WITH_STACK_PROTECTOR)
+
+        check_c_compiler_flag("-D_FORTIFY_SOURCE=2" WITH_FORTIFY_SOURCE)
+        if (WITH_FORTIFY_SOURCE)
+            set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -D_FORTIFY_SOURCE=2")
+        endif (WITH_FORTIFY_SOURCE)
+    endif (${CMAKE_C_COMPILER_ID} MATCHES GNU)
+
+    #
+    # Check for large filesystem support
+    #
+    if (CMAKE_SIZEOF_VOID_P MATCHES "8")
+        # with large file support
+        execute_process(
+            COMMAND
+                getconf LFS64_CFLAGS
+            OUTPUT_VARIABLE
+                _lfs_CFLAGS
+            ERROR_QUIET
+            OUTPUT_STRIP_TRAILING_WHITESPACE
+        )
+    else (CMAKE_SIZEOF_VOID_P MATCHES "8")
+        # with large file support
+        execute_process(
+            COMMAND
+                getconf LFS_CFLAGS
+            OUTPUT_VARIABLE
+                _lfs_CFLAGS
+            ERROR_QUIET
+            OUTPUT_STRIP_TRAILING_WHITESPACE
+        )
+    endif (CMAKE_SIZEOF_VOID_P MATCHES "8")
+    if (_lfs_CFLAGS)
+        string(REGEX REPLACE "[\r\n]" " " "${_lfs_CFLAGS}" "${${_lfs_CFLAGS}}")
+        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${_lfs_CFLAGS}")
+    endif (_lfs_CFLAGS)
+
+endif (UNIX AND NOT WIN32)
+
+if (MSVC)
+    # Use secure functions by defaualt and suppress warnings about
+    #"deprecated" functions
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /D _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES=1")
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /D _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES_COUNT=1")
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /D _CRT_NONSTDC_NO_WARNINGS=1 /D _CRT_SECURE_NO_WARNINGS=1")
+endif (MSVC)

cmake/arkcmake/DefineInstallationPaths.cmake

@@ -0,0 +1,104 @@
+if (WIN32)
+  # Same same
+  set(BIN_INSTALL_DIR "bin" CACHE PATH "-")
+  set(SBIN_INSTALL_DIR "." CACHE PATH "-")
+  set(LIB_INSTALL_DIR "lib" CACHE PATH "-")
+  set(INCLUDE_INSTALL_DIR "include" CACHE PATH "-")
+  set(PLUGIN_INSTALL_DIR "plugins" CACHE PATH "-")
+  set(HTML_INSTALL_DIR "doc/HTML" CACHE PATH "-")
+  set(ICON_INSTALL_DIR "." CACHE PATH "-")
+  set(SOUND_INSTALL_DIR "." CACHE PATH "-")
+  set(LOCALE_INSTALL_DIR "lang" CACHE PATH "-")
+elseif (UNIX OR OS2)
+  IF (NOT APPLICATION_NAME)
+    MESSAGE(STATUS "${PROJECT_NAME} is used as APPLICATION_NAME")
+    SET(APPLICATION_NAME ${PROJECT_NAME})
+  ENDIF (NOT APPLICATION_NAME)
+
+  # Suffix for Linux
+  SET(LIB_SUFFIX
+    CACHE STRING "Define suffix of directory name (32/64)"
+  )
+
+  SET(EXEC_INSTALL_PREFIX
+    "${CMAKE_INSTALL_PREFIX}"
+    CACHE PATH  "Base directory for executables and libraries"
+  )
+  SET(SHARE_INSTALL_PREFIX
+    "${CMAKE_INSTALL_PREFIX}/share"
+    CACHE PATH "Base directory for files which go to share/"
+  )
+  SET(DATA_INSTALL_PREFIX
+    "${SHARE_INSTALL_PREFIX}/${APPLICATION_NAME}"
+    CACHE PATH "The parent directory where applications can install their data")
+
+  # The following are directories where stuff will be installed to
+  SET(BIN_INSTALL_DIR
+    "${EXEC_INSTALL_PREFIX}/bin"
+    CACHE PATH "The ${APPLICATION_NAME} binary install dir (default prefix/bin)"
+  )
+  SET(SBIN_INSTALL_DIR
+    "${EXEC_INSTALL_PREFIX}/sbin"
+    CACHE PATH "The ${APPLICATION_NAME} sbin install dir (default prefix/sbin)"
+  )
+  SET(LIB_INSTALL_DIR
+    "${EXEC_INSTALL_PREFIX}/lib${LIB_SUFFIX}"
+    CACHE PATH "The subdirectory relative to the install prefix where libraries will be installed (default is prefix/lib)"
+  )
+  SET(LIBEXEC_INSTALL_DIR
+    "${EXEC_INSTALL_PREFIX}/libexec"
+    CACHE PATH "The subdirectory relative to the install prefix where libraries will be installed (default is prefix/libexec)"
+  )
+  SET(PLUGIN_INSTALL_DIR
+    "${LIB_INSTALL_DIR}/${APPLICATION_NAME}"
+    CACHE PATH "The subdirectory relative to the install prefix where plugins will be installed (default is prefix/lib/${APPLICATION_NAME})"
+  )
+  SET(INCLUDE_INSTALL_DIR
+    "${CMAKE_INSTALL_PREFIX}/include"
+    CACHE PATH "The subdirectory to the header prefix (default prefix/include)"
+  )
+
+  SET(DATA_INSTALL_DIR
+    "${DATA_INSTALL_PREFIX}"
+    CACHE PATH "The parent directory where applications can install their data (default prefix/share/${APPLICATION_NAME})"
+  )
+  SET(HTML_INSTALL_DIR
+    "${DATA_INSTALL_PREFIX}/doc/HTML"
+    CACHE PATH "The HTML install dir for documentation (default data/doc/html)"
+  )
+  SET(ICON_INSTALL_DIR
+    "${DATA_INSTALL_PREFIX}/icons"
+    CACHE PATH "The icon install dir (default data/icons/)"
+  )
+  SET(SOUND_INSTALL_DIR
+    "${DATA_INSTALL_PREFIX}/sounds"
+    CACHE PATH "The install dir for sound files (default data/sounds)"
+  )
+
+  SET(LOCALE_INSTALL_DIR
+    "${SHARE_INSTALL_PREFIX}/locale"
+    CACHE PATH "The install dir for translations (default prefix/share/locale)"
+  )
+
+  SET(XDG_APPS_DIR
+    "${SHARE_INSTALL_PREFIX}/applications/"
+    CACHE PATH "The XDG apps dir"
+  )
+  SET(XDG_DIRECTORY_DIR
+    "${SHARE_INSTALL_PREFIX}/desktop-directories"
+    CACHE PATH "The XDG directory"
+  )
+
+  SET(SYSCONF_INSTALL_DIR
+    "${EXEC_INSTALL_PREFIX}/etc"
+    CACHE PATH "The ${APPLICATION_NAME} sysconfig install dir (default prefix/etc)"
+  )
+  SET(MAN_INSTALL_DIR
+    "${SHARE_INSTALL_PREFIX}/man"
+    CACHE PATH "The ${APPLICATION_NAME} man install dir (default prefix/man)"
+  )
+  SET(INFO_INSTALL_DIR
+    "${SHARE_INSTALL_PREFIX}/info"
+    CACHE PATH "The ${APPLICATION_NAME} info install dir (default prefix/info)"
+  )
+endif ()

cmake/arkcmake/DefinePlatformDefaults.cmake

@@ -0,0 +1,28 @@
+# Set system vars
+
+if (CMAKE_SYSTEM_NAME MATCHES "Linux")
+    set(LINUX TRUE)
+endif(CMAKE_SYSTEM_NAME MATCHES "Linux")
+
+if (CMAKE_SYSTEM_NAME MATCHES "FreeBSD")
+    set(FREEBSD TRUE)
+    set(BSD TRUE)
+endif (CMAKE_SYSTEM_NAME MATCHES "FreeBSD")
+
+if (CMAKE_SYSTEM_NAME MATCHES "OpenBSD")
+    set(OPENBSD TRUE)
+    set(BSD TRUE)
+endif (CMAKE_SYSTEM_NAME MATCHES "OpenBSD")
+
+if (CMAKE_SYSTEM_NAME MATCHES "NetBSD")
+    set(NETBSD TRUE)
+    set(BSD TRUE)
+endif (CMAKE_SYSTEM_NAME MATCHES "NetBSD")
+
+if (CMAKE_SYSTEM_NAME MATCHES "(Solaris|SunOS)")
+    set(SOLARIS TRUE)
+endif (CMAKE_SYSTEM_NAME MATCHES "(Solaris|SunOS)")
+
+if (CMAKE_SYSTEM_NAME MATCHES "OS2")
+    set(OS2 TRUE)
+endif (CMAKE_SYSTEM_NAME MATCHES "OS2")

cmake/arkcmake/FindARKCOMM.cmake

@@ -0,0 +1,36 @@
+# - Try to find  ARKCOMM
+# Once done, this will define
+#
+#  ARKCOMM_FOUND - system has scicoslab 
+#  ARKCOMM_INCLUDE_DIRS - the scicoslab include directories
+#  ARKCOMM_LIBRARIES - libraries to link to
+
+include(LibFindMacros)
+include(MacroCommonPaths)
+
+MacroCommonPaths(ARKCOMM)
+
+# Include dir
+find_path(ARKCOMM_INCLUDE_DIR
+	NAMES arkcomm/AsyncSerial.hpp
+	PATHS ${COMMON_INCLUDE_PATHS_ARKCOMM}
+)
+
+# the library itself
+find_library(ARKCOMM_LIBRARY
+	NAMES arkcomm
+	PATHS ${COMMON_LIBRARY_PATHS_ARKCOMM}
+)
+
+# the import file
+find_path(ARKCOMM_LIBRARY_DIR
+	NAMES arkcomm/arkcomm-targets.cmake
+	PATHS ${COMMON_LIBRARY_PATHS_ARKCOMM}
+)
+set(ARKCOMM_LIB_IMPORT ${ARKCOMM_LIBRARY_DIR}/arkcomm/arkcomm-targets.cmake)
+
+# Set the include dir variables and the libraries and let libfind_process do the rest.
+# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
+set(ARKCOMM_PROCESS_INCLUDES ARKCOMM_INCLUDE_DIR)
+set(ARKCOMM_PROCESS_LIBS ARKCOMM_LIBRARY ARKCOMM_LIBRARIES)
+libfind_process(ARKCOMM)

cmake/arkcmake/FindARKMATH.cmake

@@ -0,0 +1,43 @@
+# - Try to find  ARKMATH
+# Once done, this will define
+#
+#  ARKMATH_FOUND - system has scicoslab 
+#  ARKMATH_INCLUDE_DIRS - the scicoslab include directories
+#  ARKMATH_LIBRARIES - libraries to link to
+
+include(LibFindMacros)
+include(MacroCommonPaths)
+
+MacroCommonPaths(ARKMATH)
+
+# Include dir
+find_path(ARKMATH_INCLUDE_DIR
+	NAMES arkmath/storage_adaptors.hpp
+	PATHS ${COMMON_INCLUDE_PATHS_ARKMATH}
+)
+
+# data dir
+find_path(ARKMATH_DATA_DIR_SEARCH
+	NAMES arkmath/data/WMM.COF
+	PATHS ${COMMON_DATA_PATHS_ARKMATH}
+)
+set(ARKMATH_DATA_DIR ${ARKMATH_DATA_DIR_SEARCH}/arkmath/data)
+
+# the library itself
+find_library(ARKMATH_LIBRARY
+	NAMES arkmath
+	PATHS ${COMMON_LIBRARY_PATHS_ARKMATH}
+)
+
+# the import file
+find_path(ARKMATH_LIBRARY_DIR
+	NAMES arkmath/arkmath-targets.cmake
+	PATHS ${COMMON_LIBRARY_PATHS_ARKMATH}
+)
+set(ARKMATH_LIB_IMPORT ${ARKMATH_LIBRARY_DIR}/arkmath/arkmath-targets.cmake)
+
+# Set the include dir variables and the libraries and let libfind_process do the rest.
+# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
+set(ARKMATH_PROCESS_INCLUDES ARKMATH_INCLUDE_DIR)
+set(ARKMATH_PROCESS_LIBS ARKMATH_LIBRARY ARKMATH_LIBRARIES)
+libfind_process(ARKMATH)

cmake/arkcmake/FindARKOSG.cmake

@@ -0,0 +1,43 @@
+# - Try to find  ARKOSG
+# Once done, this will define
+#
+#  ARKOSG_FOUND - system has scicoslab 
+#  ARKOSG_INCLUDE_DIRS - the scicoslab include directories
+#  ARKOSG_LIBRARIES - libraries to link to
+
+include(LibFindMacros)
+include(MacroCommonPaths)
+
+MacroCommonPaths(ARKOSG)
+
+# Include dir
+find_path(ARKOSG_INCLUDE_DIR
+	NAMES arkosg/osgUtils.hpp
+	PATHS ${COMMON_INCLUDE_PATHS_ARKOSG}
+)
+
+# data dir
+find_path(ARKOSG_DATA_DIR_SEARCH
+	NAMES arkosg/data/models/plane.ac
+	PATHS ${COMMON_DATA_PATHS_ARKOSG}
+)
+set(ARKOSG_DATA_DIR ${ARKOSG_DATA_DIR_SEARCH}/arkosg/data)
+
+# the library itself
+find_library(ARKOSG_LIBRARY
+	NAMES arkosg
+	PATHS ${COMMON_LIBRARY_PATHS_ARKOSG}
+)
+
+# the import file
+find_path(ARKOSG_LIBRARY_DIR
+	NAMES arkosg/arkosg-targets.cmake
+	PATHS ${COMMON_LIBRARY_PATHS_ARKOSG}
+)
+set(ARKOSG_LIB_IMPORT ${ARKOSG_LIBRARY_DIR}/arkosg/arkosg-targets.cmake)
+
+# Set the include dir variables and the libraries and let libfind_process do the rest.
+# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
+set(ARKOSG_PROCESS_INCLUDES ARKOSG_INCLUDE_DIR)
+set(ARKOSG_PROCESS_LIBS ARKOSG_LIBRARY ARKOSG_LIBRARIES)
+libfind_process(ARKOSG)

cmake/arkcmake/FindARKSCICOS.cmake

@@ -0,0 +1,36 @@
+# - Try to find  ARKSCICOS
+# Once done, this will define
+#
+#  ARKSCICOS_FOUND - system has scicoslab 
+#  ARKSCICOS_INCLUDE_DIRS - the scicoslab include directories
+#  ARKSCICOS_LIBRARIES - libraries to link to
+
+include(LibFindMacroos)
+include(MacroCommonPaths)
+
+MacroCommonPaths(ARKSCICOS)
+
+# Include dir
+find_path(ARKSCICOS_INCLUDE_DIR
+	NAMES definiotions.hpp
+	PATHS ${COMMON_INCLUDE_PATHS_ARKSCICOS}
+)
+
+# the library itself
+find_library(ARKSCICOS_LIBRARY
+	NAMES arkscicos
+	PATHS ${COMMON_LIBRARY_PATHS_ARKSCICOS}
+)
+
+# the import file
+find_path(ARKSCICOS_LIBRARY_DIR
+	NAMES arkscicos/arkscicos-targets.cmake
+	PATHS ${COMMON_LIBRARY_PATHS_ARKSCICOS}
+)
+set(ARKSCICOS_LIB_IMPORT ${ARKSCICOS_LIBRARY_DIR}/arkscicos/arkscicos-targets.cmake)
+
+# Set the include dir variables and the libraries and let libfind_process do the rest.
+# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
+set(ARKSCICOS_PROCESS_INCLUDES ARKSCICOS_INCLUDE_DIR)
+set(ARKSCICOS_PROCESS_LIBS ARKSCICOS_LIBRARY ARKSCICOS_LIBRARIES)
+libfind_process(ARKSCICOS)

cmake/arkcmake/FindBOOSTNUMERICBINDINGS.cmake

@@ -0,0 +1,21 @@
+# - Try to find  BOOSTNUMERICBINDINGS
+# Once done, this will define
+#
+#  BOOSTNUMERICBINDINGS_FOUND - system has scicoslab 
+#  BOOSTNUMERICBINDINGS_INCLUDE_DIRS - the scicoslab include directories
+
+include(LibFindMacros)
+include(MacroCommonPaths)
+
+MacroCommonPaths(BOOSTNUMERICBINDINGS)
+
+# Include dir
+find_path(BOOSTNUMERICBINDINGS_INCLUDE_DIR
+	NAMES boost/numeric/bindings/lapack/lapack.h
+	PATHS ${COMMON_INCLUDE_PATHS_BOOSTNUMERICBINDINGS}
+)
+
+# Set the include dir variables and the libraries and let libfind_process do the rest.
+# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
+set(BOOSTNUMERICBINDINGS_PROCESS_INCLUDES BOOSTNUMERICBINDINGS_INCLUDE_DIR)
+libfind_process(BOOSTNUMERICBINDINGS)

cmake/arkcmake/FindEASYSTAR.cmake

@@ -0,0 +1,21 @@
+# - Try to find  EASYSTAR
+# Once done, this will define
+#
+#  EASYSTAR_FOUND - system has easystar 
+#  EASYSTAR_INCLUDE_DIRS - the easystar include directories
+
+include(LibFindMacros)
+include(MacroCommonPaths)
+
+MacroCommonPaths(EASYSTAR)
+
+# Include dir
+find_path(EASYSTAR_INCLUDE_DIR
+	NAMES easystar/easystar.xml
+	PATHS ${COMMON_INCLUDE_PATHS_EASYSTAR}
+)
+
+# Set the include dir variables and the libraries and let libfind_process do the rest.
+# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
+set(EASYSTAR_PROCESS_INCLUDES EASYSTAR_INCLUDE_DIR)
+libfind_process(EASYSTAR)

cmake/arkcmake/FindJSBSIM.cmake

@@ -0,0 +1,39 @@
+# - Try to find  JSBSIM
+# Once done, this will define
+#
+#  JSBSIM_FOUND - system has scicoslab 
+#  JSBSIM_INCLUDE_DIRS - the scicoslab include directories
+#  JSBSIM_LIBRARIES - libraries to link to
+
+include(LibFindMacros)
+include(MacroCommonPaths)
+
+MacroCommonPaths(JSBSIM)
+
+# Include dir
+find_path(JSBSIM_INCLUDE_DIR
+	NAMES JSBSim/initialization/FGTrimmer.h
+	PATHS ${COMMON_INCLUDE_PATHS_JSBSIM}
+)
+
+# data dir
+find_path(JSBSIM_DATA_DIR_SEARCH
+	NAMES jsbsim/aircraft/aircraft_template.xml
+	PATHS ${COMMON_DATA_PATHS_JSBSIM}
+)
+set(JSBSIM_DATA_DIR ${JSBSIM_DATA_DIR_SEARCH}/jsbsim)
+
+# Finally the library itself
+find_library(JSBSIM_LIBRARY
+	NAMES JSBSim
+	PATHS ${COMMON_LIBRARY_PATHS_JSBSIM}
+)
+
+# Set the include dir variables and the libraries and let libfind_process do the rest.
+# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
+set(JSBSIM_PROCESS_INCLUDES JSBSIM_INCLUDE_DIR)
+set(JSBSIM_PROCESS_LIBS JSBSIM_LIBRARY JSBSIM_LIBRARIES)
+set(JSBSIM_INCLUDE_DIR ${JSBSIM_INCLUDE_DIR} ${JSBSIM_INCLUDE_DIR}/JSBSim)
+set(JSBSIM_INCLUDES ${JSBSIM_INCLUDES} ${JSBSIM_INCLUDE_DIR}/JSBSim)
+
+libfind_process(JSBSIM)

cmake/arkcmake/FindMAVLINK.cmake

@@ -0,0 +1,21 @@
+# - Try to find  MAVLINK
+# Once done, this will define
+#
+#  MAVLINK_FOUND - system has scicoslab 
+#  MAVLINK_INCLUDE_DIRS - the scicoslab include directories
+
+include(LibFindMacros)
+include(MacroCommonPaths)
+
+MacroCommonPaths(MAVLINK)
+
+# Include dir
+find_path(MAVLINK_INCLUDE_DIR
+	NAMES mavlink/mavlink_types.h
+	PATHS ${COMMON_INCLUDE_PATHS_MAVLINK}
+)
+
+# Set the include dir variables and the libraries and let libfind_process do the rest.
+# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
+set(MAVLINK_PROCESS_INCLUDES MAVLINK_INCLUDE_DIR)
+libfind_process(MAVLINK)

cmake/arkcmake/FindPLIB.cmake

@@ -0,0 +1,70 @@
+# Locate plib
+# This module defines
+# PLIB_LIBRARY
+# PLIB_FOUND, if false, do not try to link to plib
+# PLIB_INCLUDE_DIR, where to find the headers
+#
+# $PLIB_DIR is an environment variable that would
+# correspond to the ./configure --prefix=$PLIB_DIR
+#
+# Created David Guthrie with code by Robert Osfield. 
+
+FIND_PATH(PLIB_INCLUDE_DIR plib/js.h
+    $ENV{PLIB_DIR}/include
+    $ENV{PLIB_DIR}
+    $ENV{PLIB_ROOT}/include
+    ${DELTA3D_EXT_DIR}/inc
+    $ENV{DELTA_ROOT}/ext/inc
+    ~/Library/Frameworks
+    /Library/Frameworks
+    /usr/local/include
+    /usr/include
+    /sw/include # Fink
+    /opt/local/include # DarwinPorts
+    /opt/csw/include # Blastwave
+    /opt/include
+    [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\ Manager\\Environment;PLIB_ROOT]/include
+    /usr/freeware/include
+)
+
+MACRO(FIND_PLIB_LIBRARY MYLIBRARY MYLIBRARYNAMES)
+
+    FIND_LIBRARY(${MYLIBRARY}
+        NAMES ${MYLIBRARYNAMES}
+        PATHS
+        $ENV{PLIB_DIR}/lib
+        $ENV{PLIB_DIR}
+        $ENV{OSGDIR}/lib
+        $ENV{OSGDIR}
+        $ENV{PLIB_ROOT}/lib
+        ${DELTA3D_EXT_DIR}/lib
+        $ENV{DELTA_ROOT}/ext/lib
+        ~/Library/Frameworks
+        /Library/Frameworks
+        /usr/local/lib
+        /usr/lib
+        /sw/lib
+        /opt/local/lib
+        /opt/csw/lib
+        /opt/lib
+        [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\ Manager\\Environment;PLIB_ROOT]/lib
+        /usr/freeware/lib64
+    )
+
+ENDMACRO(FIND_PLIB_LIBRARY MYLIBRARY MYLIBRARYNAMES)
+
+SET(PLIB_RELEASE_JS_LIB_NAMES js plibjs)
+SET(PLIB_RELEASE_UL_LIB_NAMES ul plibul)
+SET(PLIB_DEBUG_JS_LIB_NAMES js_d plibjs_d)
+SET(PLIB_DEBUG_UL_LIB_NAMES ul_d plibul_d)
+
+
+FIND_PLIB_LIBRARY(PLIB_JS_LIBRARY "${PLIB_RELEASE_JS_LIB_NAMES}")
+FIND_PLIB_LIBRARY(PLIB_JS_LIBRARY_DEBUG "${PLIB_DEBUG_JS_LIB_NAMES}")
+FIND_PLIB_LIBRARY(PLIB_UL_LIBRARY "${PLIB_RELEASE_UL_LIB_NAMES}")
+FIND_PLIB_LIBRARY(PLIB_UL_LIBRARY_DEBUG "${PLIB_DEBUG_UL_LIB_NAMES}")
+
+SET(PLIB_FOUND "NO")
+IF(PLIB_JS_LIBRARY AND PLIB_INCLUDE_DIR)
+    SET(PLIB_FOUND "YES")
+ENDIF(PLIB_JS_LIBRARY AND PLIB_INCLUDE_DIR)

cmake/arkcmake/FindSCICOSLAB.cmake

@@ -0,0 +1,72 @@
+# - Try to find  SCICOSLAB
+# Once done, this will define
+#
+#  SCICOSLAB_FOUND - system has scicoslab 
+#  SCICOSLAB_INCLUDE_DIRS - the scicoslab include directories
+#  SCICOSLAB_CONTRIB_DIR - the scicoslab contrib directory
+
+include(LibFindMacros)
+
+# find scicos
+if (APPLE)
+    execute_process(COMMAND mdfind "kMDItemKind == Application && kMDItemDisplayName == ScicosLabGtk"
+        COMMAND head -1
+        RESULT_VARIABLE RESULT
+        OUTPUT_VARIABLE SCICOS_APP_BUNDLE
+        ERROR_VARIABLE ERROR_MESSAGE
+        OUTPUT_STRIP_TRAILING_WHITESPACE)
+    if (RESULT) 
+        MESSAGE(FATAL_ERROR "Could not locate 'ScicosLabGtk.app' - ${ERROR_MESSAGE}")
+    endif (RESULT)
+    execute_process(COMMAND find ${SCICOS_APP_BUNDLE} -name routines
+        COMMAND head -1
+        RESULT_VARIABLE RESULT
+        OUTPUT_VARIABLE SCICOSLAB_GUESS_INCLUDE_DIRS
+        ERROR_VARIABLE ERROR_MESSAGE
+        OUTPUT_STRIP_TRAILING_WHITESPACE)
+    if (RESULT) 
+        MESSAGE(FATAL_ERROR "Could not locate 'scicos_block4.h' in ScicosLabGtk.app - ${ERROR_MESSAGE}")
+    endif (RESULT)  
+    execute_process(COMMAND find ${SCICOS_APP_BUNDLE} -name contrib 
+        COMMAND head -1
+        RESULT_VARIABLE RESULT
+        OUTPUT_VARIABLE SCICOSLAB_GUESS_CONTRIB_DIRS
+        ERROR_VARIABLE ERROR_MESSAGE
+        OUTPUT_STRIP_TRAILING_WHITESPACE)
+    if (RESULT) 
+        MESSAGE(FATAL_ERROR "Could not locate 'loader.sce' in ScicosLabGtk.app - ${ERROR_MESSAGE}")
+    endif (RESULT)  
+elseif(UNIX)
+    set(SCICOSLAB_GUESS_INCLUDE_DIRS
+        /usr/lib/scicoslab-gtk-4.4b7/routines
+        /usr/lib/scicoslab-gtk-4.4/routines
+        /usr/lib/scicoslab-gtk-4.4.1/routines
+        )
+	set(SCICOSLAB_GUESS_CONTRIB_DIRS
+		/usr/lib/scicoslab-gtk-4.4b7/contrib
+		/usr/lib/scicoslab-gtk-4.4/contrib
+		/usr/lib/scicoslab-gtk-4.4.1/contrib
+	)
+elseif(WIN32)
+    message(FATAL_ERROR "scicoslab cmake find module doesn't work for windows")
+endif()
+
+
+# Include dir
+find_path(SCICOSLAB_INCLUDE_DIR
+  NAMES scicos/scicos_block4.h
+  PATHS ${SCICOSLAB_GUESS_INCLUDE_DIRS}
+)
+
+# Contrib dir
+find_path(SCICOSLAB_CONTRIB_DIR
+  NAMES loader.sce
+  PATHS ${SCICOSLAB_GUESS_CONTRIB_DIRS}
+)
+message(STATUS "contrib dir ${SCICOSLAB_CONTRIB_DIR}")
+
+# Set the include dir variables and the libraries and let libfind_process do the rest.
+# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
+set(SCICOSLAB_PROCESS_INCLUDES SCICOSLAB_INCLUDE_DIR)
+libfind_process(SCICOSLAB)
+# vim:ts=4:sw=4:expandtab

cmake/arkcmake/FindSIMGEAR.cmake

@@ -0,0 +1,29 @@
+# - Try to find  SIMGEAR
+# Once done, this will define
+#
+#  SIMGEAR_FOUND - system has scicoslab 
+#  SIMGEAR_INCLUDE_DIRS - the scicoslab include directories
+#  SIMGEAR_LIBRARIES - libraries to link to
+
+include(LibFindMacros)
+include(MacroCommonPaths)
+
+MacroCommonPaths(SIMGEAR)
+
+# Include dir
+find_path(SIMGEAR_INCLUDE_DIR
+	NAMES simgear/version.h
+	PATHS  ${COMMON_INCLUDE_PATHS_SIMGEAR}
+)
+
+# Finally the library itself
+find_library(SIMGEAR_LIBRARY
+	NAMES sgio
+	PATHS  ${COMMON_LIBRARY_PATHS_SIMGEAR}
+)
+
+# Set the include dir variables and the libraries and let libfind_process do the rest.
+# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
+set(SIMGEAR_PROCESS_INCLUDES SIMGEAR_INCLUDE_DIR)
+set(SIMGEAR_PROCESS_LIBS SIMGEAR_LIBRARY SIMGEAR_LIBRARIES)
+libfind_process(SIMGEAR)

cmake/arkcmake/LibFindMacros.cmake

@@ -0,0 +1,98 @@
+# Works the same as find_package, but forwards the "REQUIRED" and "QUIET" arguments
+# used for the current package. For this to work, the first parameter must be the
+# prefix of the current package, then the prefix of the new package etc, which are
+# passed to find_package.
+macro (libfind_package PREFIX)
+  set (LIBFIND_PACKAGE_ARGS ${ARGN})
+  if (${PREFIX}_FIND_QUIETLY)
+    set (LIBFIND_PACKAGE_ARGS ${LIBFIND_PACKAGE_ARGS} QUIET)
+  endif (${PREFIX}_FIND_QUIETLY)
+  if (${PREFIX}_FIND_REQUIRED)
+    set (LIBFIND_PACKAGE_ARGS ${LIBFIND_PACKAGE_ARGS} REQUIRED)
+  endif (${PREFIX}_FIND_REQUIRED)
+  find_package(${LIBFIND_PACKAGE_ARGS})
+endmacro (libfind_package)
+
+# CMake developers made the UsePkgConfig system deprecated in the same release (2.6)
+# where they added pkg_check_modules. Consequently I need to support both in my scripts
+# to avoid those deprecated warnings. Here's a helper that does just that.
+# Works identically to pkg_check_modules, except that no checks are needed prior to use.
+macro (libfind_pkg_check_modules PREFIX PKGNAME)
+  if (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4)
+    include(UsePkgConfig)
+    pkgconfig(${PKGNAME} ${PREFIX}_INCLUDE_DIRS ${PREFIX}_LIBRARY_DIRS ${PREFIX}_LDFLAGS ${PREFIX}_CFLAGS)
+  else (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4)
+    find_package(PkgConfig)
+    if (PKG_CONFIG_FOUND)
+      pkg_check_modules(${PREFIX} ${PKGNAME})
+    endif (PKG_CONFIG_FOUND)
+  endif (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4)
+endmacro (libfind_pkg_check_modules)
+
+# Do the final processing once the paths have been detected.
+# If include dirs are needed, ${PREFIX}_PROCESS_INCLUDES should be set to contain
+# all the variables, each of which contain one include directory.
+# Ditto for ${PREFIX}_PROCESS_LIBS and library files.
+# Will set ${PREFIX}_FOUND, ${PREFIX}_INCLUDE_DIRS and ${PREFIX}_LIBRARIES.
+# Also handles errors in case library detection was required, etc.
+macro (libfind_process PREFIX)
+  # Skip processing if already processed during this run
+  if (NOT ${PREFIX}_FOUND)
+    # Start with the assumption that the library was found
+    set (${PREFIX}_FOUND TRUE)
+
+    # Process all includes and set _FOUND to false if any are missing
+    foreach (i ${${PREFIX}_PROCESS_INCLUDES})
+      if (${i})
+        set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIRS} ${${i}})
+        mark_as_advanced(${i})
+      else (${i})
+        set (${PREFIX}_FOUND FALSE)
+      endif (${i})
+    endforeach (i)
+
+    # Process all libraries and set _FOUND to false if any are missing
+    foreach (i ${${PREFIX}_PROCESS_LIBS})
+      if (${i})
+        set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARIES} ${${i}})
+        mark_as_advanced(${i})
+      else (${i})
+        set (${PREFIX}_FOUND FALSE)
+      endif (${i})
+    endforeach (i)
+
+    # Print message and/or exit on fatal error
+    if (${PREFIX}_FOUND)
+      if (NOT ${PREFIX}_FIND_QUIETLY)
+        message (STATUS "Found ${PREFIX} ${${PREFIX}_VERSION}")
+      endif (NOT ${PREFIX}_FIND_QUIETLY)
+    else (${PREFIX}_FOUND)
+      if (${PREFIX}_FIND_REQUIRED)
+        foreach (i ${${PREFIX}_PROCESS_INCLUDES} ${${PREFIX}_PROCESS_LIBS})
+          message("${i}=${${i}}")
+        endforeach (i)
+        message (FATAL_ERROR "Required library ${PREFIX} NOT FOUND.\nInstall the library (dev version) and try again. If the library is already installed, use ccmake to set the missing variables manually.")
+      endif (${PREFIX}_FIND_REQUIRED)
+    endif (${PREFIX}_FOUND)
+  endif (NOT ${PREFIX}_FOUND)
+endmacro (libfind_process)
+
+macro(libfind_library PREFIX basename)
+  set(TMP "")
+  if(MSVC80)
+    set(TMP -vc80)
+  endif(MSVC80)
+  if(MSVC90)
+    set(TMP -vc90)
+  endif(MSVC90)
+  set(${PREFIX}_LIBNAMES ${basename}${TMP})
+  if(${ARGC} GREATER 2)
+    set(${PREFIX}_LIBNAMES ${basename}${TMP}-${ARGV2})
+    string(REGEX REPLACE "\\." "_" TMP ${${PREFIX}_LIBNAMES})
+    set(${PREFIX}_LIBNAMES ${${PREFIX}_LIBNAMES} ${TMP})
+  endif(${ARGC} GREATER 2)
+  find_library(${PREFIX}_LIBRARY
+    NAMES ${${PREFIX}_LIBNAMES}
+    PATHS ${${PREFIX}_PKGCONF_LIBRARY_DIRS}
+  )
+endmacro(libfind_library)

cmake/arkcmake/MacroAddCompileFlags.cmake

@@ -0,0 +1,21 @@
+# - MACRO_ADD_COMPILE_FLAGS(target_name flag1 ... flagN)
+
+# Copyright (c) 2006, Oswald Buddenhagen, <ossi@kde.org>
+# Copyright (c) 2006, Andreas Schneider, <mail@cynapses.org>
+#
+# Redistribution and use is allowed according to the terms of the BSD license.
+# For details see the accompanying COPYING-CMAKE-SCRIPTS file.
+
+
+macro (MACRO_ADD_COMPILE_FLAGS _target)
+
+  get_target_property(_flags ${_target} COMPILE_FLAGS)
+  if (_flags)
+    set(_flags ${_flags} ${ARGN})
+  else (_flags)
+    set(_flags ${ARGN})
+  endif (_flags)
+
+  set_target_properties(${_target} PROPERTIES COMPILE_FLAGS ${_flags})
+
+endmacro (MACRO_ADD_COMPILE_FLAGS)

cmake/arkcmake/MacroAddLinkFlags.cmake

@@ -0,0 +1,20 @@
+# - MACRO_ADD_LINK_FLAGS(target_name flag1 ... flagN)
+
+# Copyright (c) 2006, Oswald Buddenhagen, <ossi@kde.org>
+# Copyright (c) 2006, Andreas Schneider, <mail@cynapses.org>
+#
+# Redistribution and use is allowed according to the terms of the BSD license.
+# For details see the accompanying COPYING-CMAKE-SCRIPTS file.
+
+macro (MACRO_ADD_LINK_FLAGS _target)
+
+  get_target_property(_flags ${_target} LINK_FLAGS)
+  if (_flags)
+    set(_flags "${_flags} ${ARGN}")
+  else (_flags)
+    set(_flags "${ARGN}")
+  endif (_flags)
+
+  set_target_properties(${_target} PROPERTIES LINK_FLAGS "${_flags}")
+
+endmacro (MACRO_ADD_LINK_FLAGS)

cmake/arkcmake/MacroAddPlugin.cmake

@@ -0,0 +1,30 @@
+#  - MACRO_ADD_PLUGIN(name [WITH_PREFIX] file1 .. fileN)
+#
+#  Create a plugin from the given source files.
+#  If WITH_PREFIX is given, the resulting plugin will have the
+#  prefix "lib", otherwise it won't.
+#
+# Copyright (c) 2006, Alexander Neundorf, <neundorf@kde.org>
+# Copyright (c) 2006, Laurent Montel, <montel@kde.org>
+# Copyright (c) 2006, Andreas Schneider, <mail@cynapses.org>
+#
+# Redistribution and use is allowed according to the terms of the BSD license.
+# For details see the accompanying COPYING-CMAKE-SCRIPTS file.
+
+
+macro (MACRO_ADD_PLUGIN _target_NAME _with_PREFIX)
+
+  if (${_with_PREFIX} STREQUAL "WITH_PREFIX")
+    set(_first_SRC)
+  else (${_with_PREFIX} STREQUAL "WITH_PREFIX")
+    set(_first_SRC ${_with_PREFIX})
+  endif (${_with_PREFIX} STREQUAL "WITH_PREFIX")
+
+  add_library(${_target_NAME} MODULE ${_first_SRC} ${ARGN})
+
+  if (_first_SRC)
+    set_target_properties(${_target_NAME} PROPERTIES PREFIX "")
+  endif (_first_SRC)
+
+endmacro (MACRO_ADD_PLUGIN _name _sources)
+

cmake/arkcmake/MacroCheckCCompilerFlagSSP.cmake

@@ -0,0 +1,26 @@
+# - Check whether the C compiler supports a given flag in the
+# context of a stack checking compiler option.
+# CHECK_C_COMPILER_FLAG_SSP(FLAG VARIABLE)
+#
+#  FLAG - the compiler flag
+#  VARIABLE - variable to store the result
+# 
+#  This actually calls the check_c_source_compiles macro.
+#  See help for CheckCSourceCompiles for a listing of variables
+#  that can modify the build.
+
+# Copyright (c) 2006, Alexander Neundorf, <neundorf@kde.org>
+#
+# Redistribution and use is allowed according to the terms of the BSD license.
+# For details see the accompanying COPYING-CMAKE-SCRIPTS file.
+
+
+INCLUDE(CheckCSourceCompiles)
+
+MACRO (CHECK_C_COMPILER_FLAG_SSP _FLAG _RESULT)
+   SET(SAFE_CMAKE_REQUIRED_DEFINITIONS "${CMAKE_REQUIRED_DEFINITIONS}")
+   SET(CMAKE_REQUIRED_DEFINITIONS "${_FLAG}")
+   CHECK_C_SOURCE_COMPILES("int main(int argc, char **argv) { char buffer[256]; return buffer[argc]=0;}" ${_RESULT})
+   SET (CMAKE_REQUIRED_DEFINITIONS "${SAFE_CMAKE_REQUIRED_DEFINITIONS}")
+ENDMACRO (CHECK_C_COMPILER_FLAG_SSP)
+

cmake/arkcmake/MacroCommonPaths.cmake

@@ -0,0 +1,51 @@
+macro(MacroCommonPaths NAME)
+    set(COMMON_INCLUDE_PATHS_${NAME}
+        $ENV{${NAME}_DIR}/include
+        $ENV{${NAME}_DIR}
+        $ENV{${NAME}_ROOT}/include
+        $ENV{${NAME}_ROOT}
+        ~/Library/Frameworks
+        /Library/Frameworks
+        /usr/local/include
+        /usr/include
+        /sw/include # Fink
+        /opt/local/include # DarwinPorts
+        /opt/csw/include # Blastwave
+        /opt/include
+        [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_DIR]/include
+        [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_ROOT]/include
+        /usr/freeware/include
+        )
+    set(COMMON_LIB_PATHS_${NAME}
+        $ENV{${NAME}_DIR}/lib
+        $ENV{${NAME}_DIR}
+        ~/Library/Frameworks
+        /Library/Frameworks
+        /usr/local/lib
+        /usr/lib
+        /sw/lib
+        /opt/local/lib
+        /opt/csw/lib
+        /opt/lib
+        [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_DIR]/lib
+        [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_ROOT]/lib
+        /usr/freeware/lib64
+        )
+    set(COMMON_DATA_PATHS_${NAME}
+        $ENV{${NAME}_DIR}/share
+        $ENV{${NAME}_DIR}
+        ~/Library/Frameworks
+        /Library/Frameworks
+        /usr/local/share
+        /usr/share
+        /sw/share
+        /opt/local/share
+        /opt/csw/share
+        /opt/share
+        [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_DIR]/share
+        [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_ROOT]/share
+        /usr/freeware/share64
+        )
+endmacro(MacroCommonPaths)
+
+# vim:ts=4:sw=4:expandtab

cmake/arkcmake/MacroCopyFile.cmake

@@ -0,0 +1,33 @@
+# - macro_copy_file(_src _dst)
+# Copies a file to ${_dst} only if ${_src} is different (newer) than ${_dst}
+#
+# Example:
+# macro_copy_file(${CMAKE_CURRENT_SOURCE_DIR}/icon.png ${CMAKE_CURRENT_BINARY_DIR}/.)
+# Copies file icon.png to ${CMAKE_CURRENT_BINARY_DIR} directory
+#
+# Copyright (c) 2006-2007  Wengo
+# Copyright (c) 2006-2008  Andreas Schneider <mail@cynapses.org>
+#
+# Redistribution and use is allowed according to the terms of the BSD license.
+# For details see the accompanying COPYING file.
+
+
+macro (macro_copy_file _src _dst)
+  # Removes all path containing .svn or CVS or CMakeLists.txt during the copy
+  if (NOT ${_src} MATCHES ".*\\.svn|CVS|CMakeLists\\.txt.*")
+
+    if (CMAKE_VERBOSE_MAKEFILE)
+      message(STATUS "Copy file from ${_src} to ${_dst}")
+    endif (CMAKE_VERBOSE_MAKEFILE)
+
+    # Creates directory if necessary
+    get_filename_component(_path ${_dst} PATH)
+    file(MAKE_DIRECTORY ${_path})
+
+    execute_process(
+      COMMAND
+        ${CMAKE_COMMAND} -E copy_if_different ${_src} ${_dst}
+      OUTPUT_QUIET
+    )
+  endif (NOT ${_src} MATCHES ".*\\.svn|CVS|CMakeLists\\.txt.*")
+endmacro (macro_copy_file)

cmake/arkcmake/MacroEnsureOutOfSourceBuild.cmake

@@ -0,0 +1,19 @@
+# - MACRO_ENSURE_OUT_OF_SOURCE_BUILD(<errorMessage>)
+# MACRO_ENSURE_OUT_OF_SOURCE_BUILD(<errorMessage>)
+
+# Copyright (c) 2006, Alexander Neundorf, <neundorf@kde.org>
+#
+# Redistribution and use is allowed according to the terms of the BSD license.
+# For details see the accompanying COPYING-CMAKE-SCRIPTS file.
+
+macro (MACRO_ENSURE_OUT_OF_SOURCE_BUILD _errorMessage)
+
+    string(COMPARE EQUAL "${CMAKE_SOURCE_DIR}" "${CMAKE_BINARY_DIR}" _insource)
+    if (_insource)
+        file(REMOVE [CMakeCache.txt CMakeFiles]) 
+        message(FATAL_ERROR "${_errorMessage}")
+    endif (_insource)
+
+endmacro (MACRO_ENSURE_OUT_OF_SOURCE_BUILD)
+
+# vim:ts=4:sw=4:expandtab

cmake/arkcmake/MacroFindOrBuild.cmake

@@ -0,0 +1,18 @@
+macro(MacroFindOrBuild PACKAGE PACKAGE_PATH IS_GIT_SUBMODULE)
+	add_custom_target(${PACKAGE})
+	if (NOT ${PACKAGE}_BUILD_FROM_SOURCE)
+		find_package(${PACKAGE})
+	endif()
+	if (NOT ${PACKAGE}_FOUND)
+		set(${PACKAGE}_BUILD_FROM_SOURCE TRUE)
+		message(STATUS "could not find package ${PACKAGE}, building from source")
+		add_custom_target(${PACKAGE}_BUILD DEPENDS ${PACKAGE}_BUILD.stamp)
+		add_dependencies(${PACKAGE} ${PACKAGE}_BUILD)
+		set(${PACKAGE}_FOUND TRUE)
+		if (${IS_GIT_SUBMODULE})
+			message(STATUS "${PACKAGE} detected as git submodule, will attempt to initialize it")
+			list(APPEND GIT_SUBMODULES ${PACKAGE_PATH})
+			add_dependencies(${PACKAGE}_BUILD GIT)
+		endif()
+	endif()
+endmacro(MacroFindOrBuild)

cmake/arkcmake/MacroSetDefault.cmake

@@ -0,0 +1,6 @@
+macro(MacroSetDefault VAR DEFAULT)
+	if (NOT DEFINED ${VAR})
+		set(${VAR} ${DEFAULT})
+	endif()
+endmacro(MacroSetDefault)
+

cmake/arkcmake/README.md

@@ -0,0 +1,10 @@
+arktools cmake modules
+=============
+
+This repository contains the cmake modules used by arktools. 
+The python scripts do not function unless they are in an arktool. 
+From an arktool (ex. arktools/arkscicos) run:
+
+```console
+./cmake/updateArkcmake.py
+```

cmake/arkcmake/UseDoxygen.cmake

@@ -0,0 +1,100 @@
+# - Run Doxygen
+#
+# Adds a doxygen target that runs doxygen to generate the html
+# and optionally the LaTeX API documentation.
+# The doxygen target is added to the doc target as dependency.
+# i.e.: the API documentation is built with:
+#  make doc
+#
+# USAGE: INCLUDE IN PROJECT
+#
+#  set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR})
+#  include(UseDoxygen)
+# Add the Doxyfile.in and UseDoxygen.cmake files to the projects source directory.
+#
+#
+# Variables you may define are:
+#  DOXYFILE_OUTPUT_DIR - Path where the Doxygen output is stored. Defaults to "doc".
+#
+#  DOXYFILE_LATEX_DIR - Directory where the Doxygen LaTeX output is stored. Defaults to "latex".
+#
+#  DOXYFILE_HTML_DIR - Directory where the Doxygen html output is stored. Defaults to "html".
+#
+
+#
+#  Copyright (c) 2009-2010 Tobias Rautenkranz <tobias@rautenkranz.ch>
+#  Copyright (c) 2010      Andreas Schneider <mail@cynapses.org>
+#
+#  Redistribution and use is allowed according to the terms of the New
+#  BSD license.
+#  For details see the accompanying COPYING-CMAKE-SCRIPTS file.
+#
+
+macro(usedoxygen_set_default name value)
+    if(NOT DEFINED "${name}")
+        set("${name}" "${value}")
+    endif()
+endmacro()
+
+find_package(Doxygen)
+
+if(DOXYGEN_FOUND)
+    find_file(DOXYFILE_IN
+        NAMES
+            doxy.config.in
+        PATHS
+            ${CMAKE_CURRENT_SOURCE_DIR}
+            ${CMAKE_ROOT}/Modules/
+        NO_DEFAULT_PATH)
+    include(FindPackageHandleStandardArgs)
+    find_package_handle_standard_args(DOXYFILE_IN DEFAULT_MSG "DOXYFILE_IN")
+endif()
+
+if(DOXYGEN_FOUND AND DOXYFILE_IN_FOUND)
+    add_custom_target(doxygen ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/doxy.config)
+
+    usedoxygen_set_default(DOXYFILE_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}")
+    usedoxygen_set_default(DOXYFILE_HTML_DIR "html")
+
+    set_property(DIRECTORY APPEND PROPERTY
+            ADDITIONAL_MAKE_CLEAN_FILES "${DOXYFILE_OUTPUT_DIR}/${DOXYFILE_HTML_DIR}")
+
+    set(DOXYFILE_LATEX FALSE)
+    set(DOXYFILE_PDFLATEX FALSE)
+    set(DOXYFILE_DOT FALSE)
+
+    find_package(LATEX)
+    if(LATEX_COMPILER AND MAKEINDEX_COMPILER)
+        set(DOXYFILE_LATEX TRUE)
+        usedoxygen_set_default(DOXYFILE_LATEX_DIR "latex")
+
+        set_property(DIRECTORY APPEND PROPERTY
+                ADDITIONAL_MAKE_CLEAN_FILES
+                "${DOXYFILE_OUTPUT_DIR}/${DOXYFILE_LATEX_DIR}")
+
+        if(PDFLATEX_COMPILER)
+            set(DOXYFILE_PDFLATEX TRUE)
+        endif()
+        if(DOXYGEN_DOT_EXECUTABLE)
+            set(DOXYFILE_DOT TRUE)
+        endif()
+
+        add_custom_command(TARGET doxygen
+            POST_BUILD
+            COMMAND ${CMAKE_MAKE_PROGRAM}
+            WORKING_DIRECTORY "${DOXYFILE_OUTPUT_DIR}/${DOXYFILE_LATEX_DIR}")
+    endif()
+
+    configure_file(${DOXYFILE_IN} ${CMAKE_CURRENT_BINARY_DIR}/doxy.config ESCAPE_QUOTES IMMEDIATE @ONLY)
+    if (EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/doxy.trac.in)
+        configure_file(${CMAKE_CURRENT_SOURCE_DIR}/doxy.trac.in ${CMAKE_CURRENT_BINARY_DIR}/doxy.trac ESCAPE_QUOTES IMMEDIATE @ONLY)
+        add_custom_target(doxygen-trac ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/doxy.trac)
+    endif()
+
+    get_target_property(DOC_TARGET doc TYPE)
+    if(NOT DOC_TARGET)
+        add_custom_target(doc)
+    endif()
+
+    add_dependencies(doc doxygen)
+endif()

cmake/arkcmake/autobuild.py

@@ -0,0 +1,205 @@
+#!/usr/bin/python 
+# Author: Lenna X. Peterson (github.com/lennax)
+# based on bash script by James Goppert (github.com/jgoppert)
+
+# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
+# USAGE:                                                                      #
+# $ ./autobuild.py [1-9]                                                      #
+#   Then follow menu                                                          #
+# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
+
+# TODO: Error handling: 
+## check cmake success, etc.
+## catch CMake Warning:
+  #Manually-specified variables were not used by the project:
+		  #BUILD_TYPE
+#     (missing gprof flags)
+
+import sys # for sys.argv[] and sys.platform
+import os # for chdir()
+import subprocess # for check_call()
+import shutil # for rmtree()
+from optparse import OptionParser # for parsing options
+try: 
+	from get_build_path import get_build_path
+except ImportError: 
+	print "Could not find 'get_build_path.py' "
+	print "in '%s'" % os.path.dirname(os.path.abspath(__file__))
+	print "This module is required."
+	raise SystemExit
+
+## Move to directory containing CMakeLists.txt and src/
+build_path = get_build_path()
+if build_path:
+	os.chdir(build_path)
+else: 
+	print "The script was unable to find a build directory."
+	raise SystemExit
+
+makeargs = "-j8"
+cmakecall = ["cmake", ".."]
+build_dir = "build"
+
+## Parse command line options
+## TODO: add makeargs/cmakeargs etc.
+## (with 'action="append"' to append arg(s) to list)
+usage = "usage: %prog [options] [1-9]"
+parser = OptionParser(usage=usage)
+parser.set_defaults(verbose=False, makeargs="-j8")
+parser.add_option("-v", "--verbose",
+				action="store_true", dest="verbose",
+				help="Verbose mode")
+parser.add_option("-c", "--cmake",
+				action="store", dest="cm",
+				help="Specify one or more arguments for CMake")
+#parser.add_option("--makeargs", 
+#                  action="store", type="string", dest="makeargs",
+#                  help="Argument to `make` [default '-j8']")
+(options, args) = parser.parse_args()
+if options.verbose:
+    os.environ['VERBOSE'] = "1"
+### Split cmakeargs, reassemble, and insert into cmake call
+if options.cm:
+	cm_raw = options.cm
+	cm_list = cm_raw.split("-D")
+	if cm_list[0] == "":
+		cm_list.pop(0)
+	for cm in cm_list:
+		cm = cm.lstrip()
+		cmakearg = "-D" + cm
+		cmakecall.insert(1, cmakearg)
+#if options.makeargs:
+#    makeargs = options.makeargs
+
+
+def install_build(cmakecall, exitVal=True):
+    if not os.path.isdir(build_dir): 
+        os.mkdir(build_dir)
+    os.chdir(build_dir)
+    subprocess.check_call(cmakecall)
+    subprocess.check_call(["make", makeargs])
+    if exitVal == True:
+        raise SystemExit
+	
+def dev_build():
+	cmakecall.insert(1, "-DDEV_MODE::bool=TRUE")
+	install_build(cmakecall)
+
+def grab_deps():
+	if 'linux' in sys.platform:
+		try: 
+			subprocess.check_call('sudo apt-get install cmake', shell=True)
+		except: 
+			print "Error installing dependencies: ", sys.exc_info()[0]
+			print "apt-get is available on Debian and Ubuntu" 
+			raise SystemExit
+	elif 'darwin' in sys.platform:
+		try: 
+			subprocess.check_call('sudo port install cmake', shell=True)
+		except: 
+			print "Error installing dependencies: ", sys.exc_info()[0]
+			print "Please install Macports (http://www.macports.org)"
+			raise SystemExit
+	else: 
+		print "Platform not recognized (did not match linux or darwin)"
+		print "Script doesn't download dependencies for this platform"
+        raise SystemExit
+
+def package_source():
+    install_build(cmakecall, exitVal=False)
+    subprocess.check_call(["make", "package_source"])
+    raise SystemExit
+
+def package():
+    install_build(cmakecall, exitVal=False)
+    subprocess.check_call(["make", "package"])
+    raise SystemExit
+
+def remake():
+    if not os.path.isdir(build_dir): 
+        print "Directory '%s' does not exist" % build_dir
+        print "You must make before you can remake."
+        return 1
+    os.chdir(build_dir)
+    subprocess.check_call(["make", makeargs])
+    raise SystemExit
+
+def clean():
+	if 'posix' in os.name: 
+		print "Cleaning '%s' with rm -rf" % build_dir
+		subprocess.check_call(["rm", "-rf", build_dir])
+	else: 
+		print "Cleaning '%s' with shutil.rmtree()" % build_dir
+		print "(may be very slow)"
+		shutil.rmtree(build_dir, ignore_errors=True)
+	print "Build cleaned"
+
+# requires PROFILE definition in CMakeLists.txt:
+# set(CMAKE_BUILD_TYPE PROFILE)
+# set(CMAKE_CXX_FLAGS_PROFILE "-g -pg")
+# set(CMAKE_C_FLAGS_PROFILE "-g -pg")
+def profile():
+	cmakecall.insert(1, "-DDEV_MODE::bool=TRUE")
+	cmakecall.insert(2, "-DBUILD_TYPE=PROFILE")
+	install_build(cmakecall)
+	
+def menu():
+	print "1. developer build: used for development."
+	print "2. install build: used for building before final installation to the system."
+	print "3. grab dependencies: installs all the required packages for debian based systems (ubuntu maverick/ debian squeeze,lenny) or darwin with macports."
+	print "4. package source: creates a source package for distribution."
+	print "5. package: creates binary packages for distribution."
+	print "6. remake: calls make again after project has been configured as install or in source build."
+	print "7. clean: removes the build directory."
+	print "8. profile: compiles for gprof."
+	print "9. end."
+	opt = raw_input("Please choose an option: ")
+	return opt
+
+try: 
+    loop_num = 0
+    # continues until a function raises system exit or ^C
+    while (1): 	
+        if len(args) == 1 and loop_num == 0:
+            opt = args[0]
+            loop_num += 1
+        else:
+            opt = menu()
+
+        try:
+            opt = int(opt)
+        except ValueError:
+            pass
+            
+        if   opt == 1:
+            print "You chose developer build"
+            dev_build()
+        elif opt == 2:
+            print "You chose install build"
+            install_build(cmakecall)
+        elif opt == 3: 
+            print "You chose to install dependencies"
+            grab_deps()
+        elif opt == 4:
+            print "You chose to package the source"
+            package_source()
+        elif opt == 5:
+            print "You chose to package the binary"
+            package()
+        elif opt == 6:
+            print "You chose to re-call make on the previously configured build"
+            remake()
+        elif opt == 7:
+            print "You chose to clean the build"
+            clean()
+        elif opt == 8:
+            # requires definition in CMakeLists.txt (see def above)
+            print "You chose to compile for gprof"
+            profile()
+        elif opt == 9:
+            raise SystemExit
+        else:
+            print "Invalid option. Please try again: " 
+		
+except KeyboardInterrupt: 
+	print "\n" 

cmake/arkcmake/get_build_path.py

@@ -0,0 +1,62 @@
+#!/usr/bin/python
+# Author: Lenna X. Peterson (github.com/lennax)
+# Determines appropriate path for CMake
+#  Looks for "CMakeLists.txt" and "src/"
+#   ( BUILDFILE and SRCDIR in find_build_dir() )
+#  Searches the following paths:
+#   1 Path of call
+#   2 Path where script is located
+#   3 Path above 2 (parent directory)
+#   4 Path above 3 (grandparent directory)
+
+import os # for getcwd(), os.path
+
+def get_build_path():
+
+	build_dir=""
+
+	## Initialize search paths
+	call_dir = os.getcwd()
+	script_dir = os.path.dirname(os.path.abspath(__file__))
+	script_mom = os.path.abspath(script_dir + os.sep + os.pardir)
+	script_grandma = os.path.abspath(script_mom + os.sep + os.pardir)
+	if script_mom == call_dir:
+		script_mom = ""
+	if script_grandma == call_dir:
+		script_grandma = ""
+
+	## Define function to search for required components for build
+	def find_build_dir(search_dir):
+		BUILDFILE = "CMakeLists.txt"
+		#SRCDIR = "src"
+		os.chdir(search_dir)
+		if os.path.isfile(BUILDFILE):
+			return search_dir
+		return False
+
+	## Class to emulate if temp = x 
+	#      (checking equality of x while assigning it to temp)
+	#      Borrowed from Alex Martelli
+	class Holder(object):
+		def set(self, value):
+			self.value = value
+			return value
+		def get(self):
+			return self.value
+
+	temp = Holder()
+
+	## Search paths for build components
+	if temp.set(find_build_dir(call_dir)): 
+		build_dir = temp.get()
+	elif temp.set(find_build_dir(script_dir)): 
+		build_dir = temp.get()
+	elif script_mom and temp.set(find_build_dir(script_mom)): 
+		build_dir = temp.get()
+	elif script_grandma and temp.set(find_build_dir(script_grandma)):
+		build_dir = temp.get()
+	else: 
+		return 0
+	
+	print "I go now. Good luck, everybody!"
+	return build_dir

cmake/arkcmake/language_support_v2.cmake

@@ -0,0 +1,65 @@
+# cmake/modules/language_support.cmake
+#
+# Temporary additional general language support is contained within this
+# file.  
+
+# This additional function definition is needed to provide a workaround for
+# CMake bug 9220.
+
+# On debian testing (cmake 2.6.2), I get return code zero when calling 
+# cmake the first time, but cmake crashes when running a second time
+# as follows:
+#
+#  -- The Fortran compiler identification is unknown
+#  CMake Error at /usr/share/cmake-2.6/Modules/CMakeFortranInformation.cmake:7 (GET_FILENAME_COMPONENT):
+#    get_filename_component called with incorrect number of arguments
+#  Call Stack (most recent call first):
+#    CMakeLists.txt:3 (enable_language)
+#
+# My workaround is to invoke cmake twice.  If both return codes are zero, 
+# it is safe to invoke ENABLE_LANGUAGE(Fortran OPTIONAL)
+
+function(workaround_9220 language language_works)
+  #message("DEBUG: language = ${language}")
+  set(text
+    "project(test NONE)
+cmake_minimum_required(VERSION 2.6.0)
+enable_language(${language} OPTIONAL)
+"
+    )
+  file(REMOVE_RECURSE ${CMAKE_BINARY_DIR}/language_tests/${language})
+  file(MAKE_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language})
+  file(WRITE ${CMAKE_BINARY_DIR}/language_tests/${language}/CMakeLists.txt
+    ${text})
+  execute_process(
+    COMMAND ${CMAKE_COMMAND} .
+    WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language}
+    RESULT_VARIABLE return_code
+    OUTPUT_QUIET
+    ERROR_QUIET
+    )
+
+  if(return_code EQUAL 0)
+    # Second run
+    execute_process (
+      COMMAND ${CMAKE_COMMAND} .
+      WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language}
+      RESULT_VARIABLE return_code
+      OUTPUT_QUIET
+      ERROR_QUIET
+      )
+    if(return_code EQUAL 0)
+      set(${language_works} ON PARENT_SCOPE)
+    else(return_code EQUAL 0)
+      set(${language_works} OFF PARENT_SCOPE)
+    endif(return_code EQUAL 0)
+  else(return_code EQUAL 0)
+    set(${language_works} OFF PARENT_SCOPE)
+  endif(return_code EQUAL 0)
+endfunction(workaround_9220)
+
+# Temporary tests of the above function.
+#workaround_9220(CXX CXX_language_works)
+#message("CXX_language_works = ${CXX_language_works}")
+#workaround_9220(CXXp CXXp_language_works)
+#message("CXXp_language_works = ${CXXp_language_works}")

cmake/arkcmake/updateArkcmake.py

@@ -0,0 +1,19 @@
+#!/usr/bin/python
+# Author: Lenna X. Peterson (github.com/lennax)
+# Based on bash script by James Goppert (github.com/jgoppert)
+#
+# script used to update cmake modules from git repo, can't make this
+# a submodule otherwise it won't know how to interpret the CMakeLists.txt
+# # # # # # subprocess# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
+
+import os # for os.path
+import subprocess # for check_call()
+
+clone_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+print clone_path
+os.chdir(clone_path)
+subprocess.check_call(["git", "clone", "git://github.com/arktools/arkcmake.git","arkcmake_tmp"])
+subprocess.check_call(["rm", "-rf", "arkcmake_tmp/.git"])
+if os.path.isdir("arkcmake"):
+	subprocess.check_call(["rm", "-rf", "arkcmake"])
+subprocess.check_call(["mv", "arkcmake_tmp", "arkcmake"])

cmake/modules/ApmMakeLib.cmake

@@ -0,0 +1,7 @@
+string(REGEX REPLACE ".*/" "" LIB_NAME ${CMAKE_CURRENT_SOURCE_DIR})
+#message(STATUS "building lib: ${LIB_NAME}")
+file(GLOB ${LIB_NAME}_SRCS *.cpp)
+file(GLOB ${LIB_NAME}_HDRS *.h)
+set(${LIB_NAME}_BOARD ${BOARD})
+generate_arduino_library(${LIB_NAME})
+set_target_properties(${LIB_NAME} PROPERTIES LINKER_LANGUAGE CXX)

cmake/modules/ApmMakeSketch.cmake

@@ -0,0 +1,68 @@
+string(REGEX REPLACE ".*/" "" PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR})
+project(${PROJECT_NAME} C CXX)
+
+set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde)
+
+find_package(Arduino 22 REQUIRED)
+
+if (NOT DEFINED BOARD)
+	message(STATUS "board not defined, assuming mega, use cmake -DBOARD=mega2560 , etc. to specify")
+	set(BOARD "mega")
+endif()
+message(STATUS "Board configured as: ${BOARD}")
+
+# need to configure based on host operating system
+set(${PROJECT_NAME}_PORT COM2)
+set(${PROJECT_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X )
+	
+include_directories(
+${ARDUINO_LIBRARIES_PATH}/Wire
+${CMAKE_SOURCE_DIR}/libraries/APO
+${CMAKE_SOURCE_DIR}/libraries/AP_Common
+${CMAKE_SOURCE_DIR}/libraries/FastSerial
+${CMAKE_SOURCE_DIR}/libraries/ModeFilter
+${CMAKE_SOURCE_DIR}/libraries/AP_Compass
+${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder
+${CMAKE_SOURCE_DIR}/libraries/AP_GPS
+${CMAKE_SOURCE_DIR}/libraries/AP_IMU
+${CMAKE_SOURCE_DIR}/libraries/AP_ADC
+${CMAKE_SOURCE_DIR}/libraries/AP_DCM
+${CMAKE_SOURCE_DIR}/libraries/APM_RC
+${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink
+${CMAKE_SOURCE_DIR}/libraries/APM_BMP085
+)
+
+add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs")
+
+set(${PROJECT_NAME}_BOARD ${BOARD})
+file(GLOB ${PROJECT_NAME}_SKETCHES *.pde)
+file(GLOB ${PROJECT_NAME}_SRCS *.cpp)
+file(GLOB ${PROJECT_NAME}_HDRS *.h)
+set(${PROJECT_NAME}_LIBS
+	c
+    m
+    APO
+    FastSerial
+    AP_Common
+    GCS_MAVLink
+    AP_GPS
+    APM_RC
+    AP_DCM
+    AP_ADC
+    AP_Compass
+    AP_IMU
+    AP_RangeFinder
+    APM_BMP085
+	ModeFilter
+)
+
+
+
+
+
+generate_arduino_firmware(${PROJECT_NAME})
+
+install(FILES
+	${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.hex 
+	DESTINATION bin
+	)

libraries/APM_BMP085/CMakeLists.txt

@@ -1,22 +1 @@
-set(LIB_NAME APM_BMP085)
+include(ApmMakeLib)
-	
-set(${LIB_NAME}_SRCS 
-		APM_BMP085.cpp		
-	)  # Firmware sources
-
-set(${LIB_NAME}_HDRS  		
-		APM_BMP085.h		
-	)
-	
-include_directories(
- 
-  -
-  #${CMAKE_SOURCE_DIR}/libraries/AP_Math
-  #${CMAKE_SOURCE_DIR}/libraries/AP_Common
-	#${CMAKE_SOURCE_DIR}/libraries/FastSerial
-#	
-	)
-
-set(${LIB_NAME}_BOARD mega2560)
-
-generate_arduino_library(${LIB_NAME})

libraries/APM_PI/CMakeLists.txt

@@ -1,24 +1 @@
-set(LIB_NAME APM_PI)
+include(ApmMakeLib)
-	
-set(${LIB_NAME}_SRCS 
-		APM_PI.cpp
-		#AP_OpticalFlow_ADNS3080.cpp
-	)  # Firmware sources
-
-set(${LIB_NAME}_HDRS  		
-		APM_PI.h
-		#AP_OpticalFlow_ADNS3080.h
-	)
-	
-include_directories(
- 
-  -
-  #${CMAKE_SOURCE_DIR}/libraries/AP_Math
-  ${CMAKE_SOURCE_DIR}/libraries/AP_Common
-	#${CMAKE_SOURCE_DIR}/libraries/FastSerial
-#	
-	)
-
-set(${LIB_NAME}_BOARD mega2560)
-
-generate_arduino_library(${LIB_NAME})

libraries/APM_PerfMon/CMakeLists.txt

@@ -0,0 +1 @@
+include(ApmMakeLib)

libraries/APM_RC/CMakeLists.txt

@@ -1,22 +1 @@
-set(LIB_NAME APM_RC)
+include(ApmMakeLib)
-	
-set(${LIB_NAME}_SRCS 
-		APM_RC.cpp		
-	)  # Firmware sources
-
-set(${LIB_NAME}_HDRS  		
-		APM_RC.h		
-	)
-	
-include_directories(
- 
-  -
-  #${CMAKE_SOURCE_DIR}/libraries/AP_Math
-  #${CMAKE_SOURCE_DIR}/libraries/AP_Common
-	#${CMAKE_SOURCE_DIR}/libraries/FastSerial
-#	
-	)
-
-set(${LIB_NAME}_BOARD mega2560)
-
-generate_arduino_library(${LIB_NAME})

libraries/APO/APO.cpp

@@ -0,0 +1,8 @@
+/*
+ * APO.cpp
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#include "APO.h"

libraries/APO/APO.h

@@ -0,0 +1,20 @@
+/*
+ * APO.h
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef APO_H_
+#define APO_H_
+
+#include "AP_Autopilot.h"
+#include "AP_Guide.h"
+#include "AP_Controller.h"
+#include "AP_HardwareAbstractionLayer.h"
+#include "AP_MavlinkCommand.h"
+#include "AP_Navigator.h"
+#include "AP_RcChannel.h"
+//#include "AP_Var_keys.h"
+
+#endif /* APO_H_ */

libraries/APO/APO_DefaultSetup.h

@@ -0,0 +1,161 @@
+#ifndef _APO_COMMON_H
+#define _APO_COMMON_H
+
+FastSerialPort0(Serial);
+FastSerialPort1(Serial1);
+FastSerialPort2(Serial2);
+FastSerialPort3(Serial3);
+
+/*
+ * 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();
+}
+
+#endif _APO_COMMON_H

libraries/APO/AP_Autopilot.cpp

@@ -0,0 +1,246 @@
+/*
+ * AP_Autopilot.cpp
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#include "AP_Autopilot.h"
+
+namespace apo {
+
+class AP_HardwareAbstractionLayer;
+
+AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
+		AP_Controller * controller, AP_HardwareAbstractionLayer * hal,
+		float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) :
+	Loop(loop0Rate, callback0, this), _navigator(navigator), _guide(guide),
+			_controller(controller), _hal(hal), _loop0Rate(loop0Rate),
+			_loop1Rate(loop1Rate), _loop2Rate(loop2Rate), _loop3Rate(loop3Rate),
+			_loop4Rate(loop3Rate) {
+
+	hal->setState(MAV_STATE_BOOT);
+	hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
+	hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
+
+	/*
+	 * Calibration
+	 */
+	hal->setState(MAV_STATE_CALIBRATING);
+	hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
+	hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
+	navigator->calibrate();
+
+	// start clock
+	//uint32_t timeStart = millis();
+	//uint16_t gpsWaitTime = 5000; // 5 second wait for gps
+
+	/*
+	 * Look for valid initial state
+	 */
+	while (1) {
+		// letc gcs known we are alive
+		hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
+		hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
+		hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
+		delay(1000);
+		if (hal->getMode() == MODE_LIVE) {
+			_navigator->updateSlow(0);
+			if (_hal->gps) {
+				if (hal->gps->fix) {
+					break;
+				} else {
+					hal->gcs->sendText(SEVERITY_LOW,
+							PSTR("waiting for gps lock\n"));
+					hal->debug->printf_P(PSTR("waiting for gps lock\n"));
+				}
+			} else { // no gps, can skip
+				break;
+			}
+		} else if (hal->getMode() == MODE_HIL_CNTL) { // hil
+			_hal->hil->receive();
+			Serial.println("HIL Recieve Called");
+			if (_navigator->getTimeStamp() != 0) {
+				// give hil a chance to send some packets
+				for (int i = 0; i < 5; i++) {
+					hal->debug->println_P(PSTR("reading initial hil packets"));
+					hal->gcs->sendText(SEVERITY_LOW,
+							PSTR("reading initial hil packets"));
+					delay(1000);
+				}
+				break;
+			}
+			hal->debug->println_P(PSTR("waiting for hil packet"));
+		}
+	}
+
+	AP_MavlinkCommand::home.setAlt(_navigator->getAlt());
+	AP_MavlinkCommand::home.setLat(_navigator->getLat());
+	AP_MavlinkCommand::home.setLon(_navigator->getLon());
+	AP_MavlinkCommand::home.save();
+	_hal->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg\n"),
+			AP_MavlinkCommand::home.getLat()*rad2Deg,
+			AP_MavlinkCommand::home.getLon()*rad2Deg);
+	AP_MavlinkCommand::home.load();
+	_hal->debug->printf_P(PSTR("home after load lat: %f deg, lon: %f deg\n"),
+			AP_MavlinkCommand::home.getLat()*rad2Deg,
+			AP_MavlinkCommand::home.getLon()*rad2Deg);
+
+	/*
+	 * Attach loops
+	 */
+	hal->debug->println_P(PSTR("attaching loops"));
+	subLoops().push_back(new Loop(getLoopRate(1), callback1, this));
+	subLoops().push_back(new Loop(getLoopRate(2), callback2, this));
+	subLoops().push_back(new Loop(getLoopRate(3), callback3, this));
+	subLoops().push_back(new Loop(getLoopRate(4), callback4, this));
+
+	hal->debug->println_P(PSTR("running"));
+	hal->gcs->sendText(SEVERITY_LOW, PSTR("running"));
+
+	if (hal->getMode() == MODE_LIVE) {
+		hal->setState(MAV_STATE_ACTIVE);
+	} else {
+		hal->setState(MAV_STATE_HILSIM);
+	}
+
+	/*
+	 * Radio setup
+	 */
+	hal->debug->println_P(PSTR("initializing radio"));
+	APM_RC.Init(); // APM Radio initialization,
+	// start this after control loop is running
+}
+
+void AP_Autopilot::callback0(void * data) {
+	AP_Autopilot * apo = (AP_Autopilot *) data;
+	//apo->hal()->debug->println_P(PSTR("callback 0"));
+
+	/*
+	 * ahrs update
+	 */
+	if (apo->getNavigator())
+		apo->getNavigator()->updateFast(1.0 / apo->getLoopRate(0));
+}
+
+void AP_Autopilot::callback1(void * data) {
+	AP_Autopilot * apo = (AP_Autopilot *) data;
+	//apo->getHal()->debug->println_P(PSTR("callback 1"));
+
+	/*
+	 * hardware in the loop
+	 */
+	if (apo->getHal()->hil && apo->getHal()->getMode() != MODE_LIVE) {
+		apo->getHal()->hil->receive();
+		apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
+	}
+
+	/*
+	 * update guidance laws
+	 */
+	if (apo->getGuide())
+	{
+		//apo->getHal()->debug->println_P(PSTR("updating guide"));
+		apo->getGuide()->update();
+	}
+
+	/*
+	 * update control laws
+	 */
+	if (apo->getController()) {
+		//apo->getHal()->debug->println_P(PSTR("updating controller"));
+		apo->getController()->update(1. / apo->getLoopRate(1));
+	}
+	/*
+	 char msg[50];
+	 sprintf(msg, "c_hdg: %f, c_thr: %f", apo->guide()->headingCommand, apo->guide()->groundSpeedCommand);
+	 apo->hal()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg);
+	 */
+}
+
+void AP_Autopilot::callback2(void * data) {
+	AP_Autopilot * apo = (AP_Autopilot *) data;
+	//apo->getHal()->debug->println_P(PSTR("callback 2"));
+
+	/*
+	 * send telemetry
+	 */
+	if (apo->getHal()->gcs) {
+		// send messages
+		apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW);
+		apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE);
+		//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
+		//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION);
+		apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
+		//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
+	}
+
+	/*
+	 * slow navigation loop update
+	 */
+	if (apo->getNavigator()) {
+		apo->getNavigator()->updateSlow(1.0 / apo->getLoopRate(2));
+	}
+
+	/*
+	 * handle ground control station communication
+	 */
+	if (apo->getHal()->gcs) {
+		// send messages
+		apo->getHal()->gcs->requestCmds();
+		apo->getHal()->gcs->sendParameters();
+
+		// receive messages
+		apo->getHal()->gcs->receive();
+	}
+
+	/*
+	 * navigator debug
+	 */
+	/*
+	 if (apo->navigator()) {
+	 apo->getHal()->debug->printf_P(PSTR("roll: %f deg\tpitch: %f deg\tyaw: %f deg\n"),
+	 apo->navigator()->getRoll()*rad2Deg,
+	 apo->navigator()->getPitch()*rad2Deg,
+	 apo->navigator()->getYaw()*rad2Deg);
+	 apo->getHal()->debug->printf_P(PSTR("lat: %f deg\tlon: %f deg\talt: %f m\n"),
+	 apo->navigator()->getLat()*rad2Deg,
+	 apo->navigator()->getLon()*rad2Deg,
+	 apo->navigator()->getAlt());
+	 }
+	 */
+}
+
+void AP_Autopilot::callback3(void * data) {
+	AP_Autopilot * apo = (AP_Autopilot *) data;
+	//apo->getHal()->debug->println_P(PSTR("callback 3"));
+
+	/*
+	 * send heartbeat
+	 */
+	apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
+
+	/*
+	 * load/loop rate/ram debug
+	 */
+	apo->getHal()->load = apo->load();
+	apo->getHal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"),
+			apo->load(),1.0/apo->dt(),freeMemory());
+
+	apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
+
+	/*
+	 * adc debug
+	 */
+	//apo->getDebug().printf_P(PSTR("adc: %d %d %d %d %d %d %d %d\n"),
+	//apo->adc()->Ch(0), apo->adc()->Ch(1), apo->adc()->Ch(2),
+	//apo->adc()->Ch(3), apo->adc()->Ch(4), apo->adc()->Ch(5),
+	//apo->adc()->Ch(6), apo->adc()->Ch(7), apo->adc()->Ch(8));
+}
+
+void AP_Autopilot::callback4(void * data) {
+	//AP_Autopilot * apo = (AP_Autopilot *) data;
+	//apo->getHal()->debug->println_P(PSTR("callback 4"));
+}
+
+} // apo

libraries/APO/AP_Autopilot.h

@@ -0,0 +1,165 @@
+/*
+ * AP_Autopilot.h
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef AP_AUTOPILOT_H_
+#define AP_AUTOPILOT_H_
+
+/*
+ * AVR runtime
+ */
+#include <avr/io.h>
+#include <avr/eeprom.h>
+#include <avr/pgmspace.h>
+#include <math.h>
+/*
+ * Libraries
+ */
+#include "../AP_Common/AP_Common.h"
+#include "../FastSerial/FastSerial.h"
+#include "../AP_GPS/GPS.h"
+#include "../APM_RC/APM_RC.h"
+#include "../AP_ADC/AP_ADC.h"
+#include "../APM_BMP085/APM_BMP085.h"
+#include "../AP_Compass/AP_Compass.h"
+#include "../AP_Math/AP_Math.h"
+#include "../AP_IMU/AP_IMU.h"
+#include "../AP_DCM/AP_DCM.h"
+#include "../AP_Common/AP_Loop.h"
+#include "../GCS_MAVLink/GCS_MAVLink.h"
+#include "../AP_RangeFinder/AP_RangeFinder.h"
+/*
+ * Local Modules
+ */
+#include "AP_HardwareAbstractionLayer.h"
+#include "AP_RcChannel.h"
+#include "AP_Controller.h"
+#include "AP_Navigator.h"
+#include "AP_Guide.h"
+#include "AP_CommLink.h"
+
+/**
+ * ArduPilotOne namespace to protect varibles
+ * from overlap with avr and libraries etc.
+ * ArduPilotOne does not use any global
+ * variables.
+ */
+namespace apo {
+
+// forward declarations
+class AP_CommLink;
+
+/**
+ * This class encapsulates the entire autopilot system
+ * The constructor takes guide, navigator, and controller
+ * as well as the hardware abstraction layer.
+ *
+ * It inherits from loop to manage
+ * the subloops and sets the overall
+ * frequency for the autopilot.
+ *
+
+ */
+class AP_Autopilot: public Loop {
+public:
+	/**
+	 * Default constructor
+	 */
+	AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
+			AP_Controller * controller, AP_HardwareAbstractionLayer * hal,
+			float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate);
+
+	/**
+	 * Accessors
+	 */
+	AP_Navigator * getNavigator() {
+		return _navigator;
+	}
+	AP_Guide * getGuide() {
+		return _guide;
+	}
+	AP_Controller * getController() {
+		return _controller;
+	}
+	AP_HardwareAbstractionLayer * getHal() {
+		return _hal;
+	}
+
+	float getLoopRate(uint8_t i) {
+		switch(i) {
+		case 0: return _loop0Rate;
+		case 1: return _loop1Rate;
+		case 2: return _loop2Rate;
+		case 3: return _loop3Rate;
+		case 4: return _loop4Rate;
+		default: return 0;
+		}
+	}
+
+private:
+
+	/**
+	 * Loop 0 Callbacks (fastest)
+	 * - inertial navigation
+	 * @param data A void pointer used to pass the apo class
+	 *  so that the apo public interface may be accessed.
+	 */
+	static void callback0(void * data);
+	float _loop0Rate;
+
+	/**
+	 * Loop 1 Callbacks
+	 * - control
+	 * - compass reading
+	 * @see callback0
+	 */
+	static void callback1(void * data);
+	float _loop1Rate;
+
+	/**
+	 * Loop 2 Callbacks
+	 * - gps sensor fusion
+	 * - compass sensor fusion
+	 * @see callback0
+	 */
+	static void callback2(void * data);
+	float _loop2Rate;
+
+	/**
+	 * Loop 3 Callbacks
+	 * - slow messages
+	 * @see callback0
+	 */
+	static void callback3(void * data);
+	float _loop3Rate;
+
+	/**
+	 * Loop 4 Callbacks
+	 * - super slow mesages
+	 * - log writing
+	 * @see callback0
+	 */
+	static void callback4(void * data);
+	float _loop4Rate;
+
+	/**
+	 * Components
+	 */
+	AP_Navigator * _navigator;
+	AP_Guide * _guide;
+	AP_Controller * _controller;
+	AP_HardwareAbstractionLayer * _hal;
+
+	/**
+	 * Constants
+	 */
+	static const float deg2rad = M_PI / 180;
+	static const float rad2deg = 180 / M_PI;
+};
+
+} // namespace apo
+
+#endif /* AP_AUTOPILOT_H_ */

libraries/APO/AP_CommLink.cpp

@@ -0,0 +1,15 @@
+/*
+ * AP_CommLink.cpp
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#include "AP_CommLink.h"
+
+namespace apo {
+
+uint8_t MavlinkComm::_nChannels = 0;
+uint8_t MavlinkComm::_paramNameLengthMax = 13;
+
+} // apo

libraries/APO/AP_CommLink.h

@@ -0,0 +1,790 @@
+/*
+ * AP_CommLink.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_CommLink_H
+#define AP_CommLink_H
+
+#include "AP_HardwareAbstractionLayer.h"
+#include "AP_MavlinkCommand.h"
+#include "AP_Controller.h"
+
+namespace apo {
+
+class AP_Controller;
+class AP_Navigator;
+class AP_Guide;
+class AP_HardwareAbstractionLayer;
+
+enum {
+	SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH
+};
+
+// forward declarations
+//class ArduPilotOne;
+//class AP_Controller;
+
+/// CommLink class
+class AP_CommLink {
+public:
+
+	AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide,
+			AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
+		_link(link), _navigator(navigator), _guide(guide),
+				_controller(controller), _hal(hal) {
+	}
+	virtual void send() = 0;
+	virtual void receive() = 0;
+	virtual void sendMessage(uint8_t id, uint32_t param = 0) = 0;
+	virtual void sendText(uint8_t severity, const char *str) = 0;
+	virtual void sendText(uint8_t severity, const prog_char_t *str) = 0;
+	virtual void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) = 0;
+	virtual void sendParameters() = 0;
+	virtual void requestCmds() = 0;
+
+protected:
+	FastSerial * _link;
+	AP_Navigator * _navigator;
+	AP_Guide * _guide;
+	AP_Controller * _controller;
+	AP_HardwareAbstractionLayer * _hal;
+};
+
+class MavlinkComm: public AP_CommLink {
+public:
+	MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide,
+			AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
+				AP_CommLink(link, nav, guide, controller, hal),
+
+				// options
+				_useRelativeAlt(true),
+
+				// commands
+				_sendingCmds(false), _receivingCmds(false),
+				_cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()),
+				_cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0),
+				_cmdMax(30), _cmdNumberRequested(0),
+
+				// parameters
+				_parameterCount(0), _queuedParameter(NULL),
+				_queuedParameterIndex(0) {
+
+		switch (_nChannels) {
+		case 0:
+			mavlink_comm_0_port = link;
+			_channel = MAVLINK_COMM_0;
+			_nChannels++;
+			break;
+		case 1:
+			mavlink_comm_1_port = link;
+			_channel = MAVLINK_COMM_1;
+			_nChannels++;
+			break;
+		default:
+			// signal that number of channels exceeded
+			_channel = MAVLINK_COMM_3;
+			break;
+		}
+	}
+
+	virtual void send() {
+		// if number of channels exceeded return
+		if (_channel == MAVLINK_COMM_3)
+			return;
+	}
+
+	void sendMessage(uint8_t id, uint32_t param = 0) {
+		//_hal->debug->printf_P(PSTR("send message\n"));
+
+		// if number of channels exceeded return
+		if (_channel == MAVLINK_COMM_3)
+			return;
+
+		uint64_t timeStamp = micros();
+
+		switch (id) {
+
+		case MAVLINK_MSG_ID_HEARTBEAT: {
+			mavlink_msg_heartbeat_send(_channel, mavlink_system.type,
+					MAV_AUTOPILOT_ARDUPILOTMEGA);
+			break;
+		}
+
+		case MAVLINK_MSG_ID_ATTITUDE: {
+			mavlink_msg_attitude_send(_channel, timeStamp,
+					_navigator->getRoll(), _navigator->getPitch(),
+					_navigator->getYaw(), _navigator->getRollRate(),
+					_navigator->getPitchRate(), _navigator->getYawRate());
+			break;
+		}
+
+		case MAVLINK_MSG_ID_GLOBAL_POSITION: {
+			mavlink_msg_global_position_send(_channel, timeStamp,
+					_navigator->getLat() * rad2Deg,
+					_navigator->getLon() * rad2Deg, _navigator->getAlt(),
+					_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->getYaw() * rad2Deg);
+			break;
+		}
+
+			/*
+			 case MAVLINK_MSG_ID_GPS_RAW_INT: {
+			 mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(),
+			 _navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0,
+			 _navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg);
+			 break;
+			 }
+			 */
+
+		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
+		}
+
+		case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
+			int16_t ch[8];
+			for (int i = 0; i < 8; i++)
+				ch[i] = 0;
+			for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) {
+				ch[i] = 10000 * _hal->rc[i]->getPosition();
+				//_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]);
+			}
+			mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2],
+					ch[3], ch[4], ch[5], ch[6], ch[7], 255);
+			break;
+		}
+
+		case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
+			int16_t ch[8];
+			for (int i = 0; i < 8; i++)
+				ch[i] = 0;
+			for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++)
+				ch[i] = _hal->rc[i]->getPwm();
+			mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2],
+					ch[3], ch[4], ch[5], ch[6], ch[7], 255);
+			break;
+		}
+
+		case MAVLINK_MSG_ID_SYS_STATUS: {
+
+			float batteryVoltage, temp;
+			temp = analogRead(0);
+			batteryVoltage = ((temp * 5 / 1023) / 0.28);
+
+			mavlink_msg_sys_status_send(_channel, _controller->getMode(),
+					_guide->getMode(), _hal->getState(), _hal->load * 10,
+					batteryVoltage * 1000,
+					(batteryVoltage - 3.3) / (4.2 - 3.3) * 1000, _packetDrops);
+			break;
+		}
+
+		case MAVLINK_MSG_ID_WAYPOINT_ACK: {
+			sendText(SEVERITY_LOW, PSTR("waypoint ack"));
+			//mavlink_waypoint_ack_t packet;
+			uint8_t type = 0; // ok (0), error(1)
+			mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId,
+					_cmdDestCompId, type);
+
+			// turn off waypoint send
+			_receivingCmds = false;
+			break;
+		}
+
+		case MAVLINK_MSG_ID_WAYPOINT_CURRENT: {
+			mavlink_msg_waypoint_current_send(_channel,
+					_guide->getCurrentIndex());
+			break;
+		}
+
+		default: {
+			char msg[50];
+			sprintf(msg, "autopilot sending unknown command with id: %d", id);
+			sendText(SEVERITY_HIGH, msg);
+		}
+
+		} // switch
+	} // send message
+
+	virtual void receive() {
+		//_hal->debug->printf_P(PSTR("receive\n"));
+		// if number of channels exceeded return
+		//
+		if (_channel == MAVLINK_COMM_3)
+			return;
+
+		// receive new packets
+		mavlink_message_t msg;
+		mavlink_status_t status;
+
+		// process received bytes
+		while (comm_get_available(_channel)) {
+			uint8_t c = comm_receive_ch(_channel);
+
+			// Try to get a new message
+			if (mavlink_parse_char(_channel, c, &msg, &status))
+				_handleMessage(&msg);
+		}
+
+		// Update packet drops counter
+		_packetDrops += status.packet_rx_drop_count;
+	}
+
+	void sendText(uint8_t severity, const char *str) {
+		mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str);
+	}
+
+	void sendText(uint8_t severity, const prog_char_t *str) {
+		mavlink_statustext_t m;
+		uint8_t i;
+		for (i = 0; i < sizeof(m.text); i++) {
+			m.text[i] = pgm_read_byte((const prog_char *) (str++));
+		}
+		if (i < sizeof(m.text))
+			m.text[i] = 0;
+		sendText(severity, (const char *) m.text);
+	}
+
+	void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {
+	}
+
+	/**
+	 * sends parameters one at a time
+	 */
+	void sendParameters() {
+		//_hal->debug->printf_P(PSTR("send parameters\n"));
+		// Check to see if we are sending parameters
+		while (NULL != _queuedParameter) {
+			AP_Var *vp;
+			float value;
+
+			// copy the current parameter and prepare to move to the next
+			vp = _queuedParameter;
+			_queuedParameter = _queuedParameter->next();
+
+			// if the parameter can be cast to float, report it here and break out of the loop
+			value = vp->cast_to_float();
+			if (!isnan(value)) {
+
+				char paramName[_paramNameLengthMax];
+				vp->copy_name(paramName, sizeof(paramName));
+
+				mavlink_msg_param_value_send(_channel, (int8_t*) paramName,
+						value, _countParameters(), _queuedParameterIndex);
+
+				_queuedParameterIndex++;
+				break;
+			}
+		}
+
+	}
+
+	/**
+	 * request commands one at a time
+	 */
+	void requestCmds() {
+		//_hal->debug->printf_P(PSTR("requesting commands\n"));
+		// request cmds one by one
+		if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) {
+			mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId,
+					_cmdDestCompId, _cmdRequestIndex);
+		}
+	}
+
+private:
+
+	// options
+	bool _useRelativeAlt;
+
+	// commands
+	bool _sendingCmds;
+	bool _receivingCmds;
+	uint16_t _cmdTimeLastSent;
+	uint16_t _cmdTimeLastReceived;
+	uint16_t _cmdDestSysId;
+	uint16_t _cmdDestCompId;
+	uint16_t _cmdRequestIndex;
+	uint16_t _cmdNumberRequested;
+	uint16_t _cmdMax;
+	Vector<mavlink_command_t *> _cmdList;
+
+	// parameters
+	static uint8_t _paramNameLengthMax;
+	uint16_t _parameterCount;
+	AP_Var * _queuedParameter;
+	uint16_t _queuedParameterIndex;
+
+	// channel
+	mavlink_channel_t _channel;
+	uint16_t _packetDrops;
+	static uint8_t _nChannels;
+
+	void _handleMessage(mavlink_message_t * msg) {
+
+		uint32_t timeStamp = micros();
+
+		switch (msg->msgid) {
+		//_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid);
+
+		case MAVLINK_MSG_ID_HEARTBEAT: {
+			mavlink_heartbeat_t packet;
+			mavlink_msg_heartbeat_decode(msg, &packet);
+			_hal->lastHeartBeat = micros();
+			break;
+		}
+
+		case MAVLINK_MSG_ID_GPS_RAW: {
+			// decode
+			mavlink_gps_raw_t packet;
+			mavlink_msg_gps_raw_decode(msg, &packet);
+
+			_navigator->setTimeStamp(timeStamp);
+			_navigator->setLat(packet.lat * deg2Rad);
+			_navigator->setLon(packet.lon * deg2Rad);
+			_navigator->setAlt(packet.alt);
+			_navigator->setYaw(packet.hdg * deg2Rad);
+			_navigator->setGroundSpeed(packet.v);
+			_navigator->setAirSpeed(packet.v);
+			//_hal->debug->printf_P(PSTR("received hil gps raw packet\n"));
+			/*
+			 _hal->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"),
+			 packet.lat,
+			 packet.lon,
+			 packet.alt);
+			 */
+			break;
+		}
+
+		case MAVLINK_MSG_ID_ATTITUDE: {
+			// decode
+			mavlink_attitude_t packet;
+			mavlink_msg_attitude_decode(msg, &packet);
+
+			// set dcm hil sensor
+			_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);
+			//_hal->debug->printf_P(PSTR("received hil attitude packet\n"));
+			break;
+		}
+
+		case MAVLINK_MSG_ID_ACTION: {
+			// decode
+			mavlink_action_t packet;
+			mavlink_msg_action_decode(msg, &packet);
+			if (_checkTarget(packet.target, packet.target_component))
+				break;
+
+			// do action
+			sendText(SEVERITY_LOW, PSTR("action received"));
+			switch (packet.action) {
+
+			case MAV_ACTION_STORAGE_READ:
+				AP_Var::load_all();
+				break;
+
+			case MAV_ACTION_STORAGE_WRITE:
+				AP_Var::save_all();
+				break;
+
+			case MAV_ACTION_CALIBRATE_RC:
+			case MAV_ACTION_CALIBRATE_GYRO:
+			case MAV_ACTION_CALIBRATE_MAG:
+			case MAV_ACTION_CALIBRATE_ACC:
+			case MAV_ACTION_CALIBRATE_PRESSURE:
+			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:
+				sendText(SEVERITY_LOW, PSTR("unknown action"));
+				break;
+			}
+			break;
+		}
+
+		case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: {
+			sendText(SEVERITY_LOW, PSTR("waypoint request list"));
+
+			// decode
+			mavlink_waypoint_request_list_t packet;
+			mavlink_msg_waypoint_request_list_decode(msg, &packet);
+			if (_checkTarget(packet.target_system, packet.target_component))
+				break;
+
+			// Start sending waypoints
+			mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid,
+					_guide->getNumberOfCommands());
+
+			_cmdTimeLastSent = millis();
+			_cmdTimeLastReceived = millis();
+			_sendingCmds = true;
+			_receivingCmds = false;
+			_cmdDestSysId = msg->sysid;
+			_cmdDestCompId = msg->compid;
+			break;
+		}
+
+		case MAVLINK_MSG_ID_WAYPOINT_REQUEST: {
+			sendText(SEVERITY_LOW, PSTR("waypoint request"));
+
+			// Check if sending waypiont
+			if (!_sendingCmds)
+				break;
+
+			// decode
+			mavlink_waypoint_request_t packet;
+			mavlink_msg_waypoint_request_decode(msg, &packet);
+			if (_checkTarget(packet.target_system, packet.target_component))
+				break;
+
+			_hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq);
+			AP_MavlinkCommand cmd(packet.seq);
+			cmd.load();
+
+			mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex());
+			mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId,
+					wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue,
+					wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y,
+					wp.z);
+
+			// update last waypoint comm stamp
+			_cmdTimeLastSent = millis();
+			break;
+		}
+
+		case MAVLINK_MSG_ID_WAYPOINT_ACK: {
+			sendText(SEVERITY_LOW, PSTR("waypoint ack"));
+
+			// decode
+			mavlink_waypoint_ack_t packet;
+			mavlink_msg_waypoint_ack_decode(msg, &packet);
+			if (_checkTarget(packet.target_system, packet.target_component))
+				break;
+
+			// check for error
+			//uint8_t type = packet.type; // ok (0), error(1)
+
+			// turn off waypoint send
+			_sendingCmds = false;
+			break;
+		}
+
+		case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
+			sendText(SEVERITY_LOW, PSTR("param request list"));
+
+			// decode
+			mavlink_param_request_list_t packet;
+			mavlink_msg_param_request_list_decode(msg, &packet);
+			if (_checkTarget(packet.target_system, packet.target_component))
+				break;
+
+			// Start sending parameters - next call to ::update will kick the first one out
+
+			_queuedParameter = AP_Var::first();
+			_queuedParameterIndex = 0;
+			break;
+		}
+
+		case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: {
+			sendText(SEVERITY_LOW, PSTR("waypoint clear all"));
+
+			// decode
+			mavlink_waypoint_clear_all_t packet;
+			mavlink_msg_waypoint_clear_all_decode(msg, &packet);
+			if (_checkTarget(packet.target_system, packet.target_component))
+				break;
+
+			// clear all waypoints
+			uint8_t type = 0; // ok (0), error(1)
+			_guide->setNumberOfCommands(1);
+			_guide->setCurrentIndex(0);
+
+			// send acknowledgement 3 times to makes sure it is received
+			for (int i = 0; i < 3; i++)
+				mavlink_msg_waypoint_ack_send(_channel, msg->sysid,
+						msg->compid, type);
+
+			break;
+		}
+
+		case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: {
+			sendText(SEVERITY_LOW, PSTR("waypoint set current"));
+
+			// decode
+			mavlink_waypoint_set_current_t packet;
+			mavlink_msg_waypoint_set_current_decode(msg, &packet);
+			Serial.print("Packet Sequence:");
+			Serial.println(packet.seq);
+			if (_checkTarget(packet.target_system, packet.target_component))
+				break;
+
+			// set current waypoint
+			Serial.print("Current Index:");
+			Serial.println(_guide->getCurrentIndex());
+			Serial.flush();
+			_guide->setCurrentIndex(packet.seq);
+			mavlink_msg_waypoint_current_send(_channel,
+					_guide->getCurrentIndex());
+			break;
+		}
+
+		case MAVLINK_MSG_ID_WAYPOINT_COUNT: {
+			sendText(SEVERITY_LOW, PSTR("waypoint count"));
+
+			// decode
+			mavlink_waypoint_count_t packet;
+			mavlink_msg_waypoint_count_decode(msg, &packet);
+			if (_checkTarget(packet.target_system, packet.target_component))
+				break;
+
+			// start waypoint receiving
+			if (packet.count > _cmdMax) {
+				packet.count = _cmdMax;
+			}
+			_cmdNumberRequested = packet.count;
+			_cmdTimeLastReceived = millis();
+			_receivingCmds = true;
+			_sendingCmds = false;
+			_cmdRequestIndex = 0;
+			break;
+		}
+
+		case MAVLINK_MSG_ID_WAYPOINT: {
+			sendText(SEVERITY_LOW, PSTR("waypoint"));
+
+			// Check if receiving waypiont
+			if (!_receivingCmds) {
+				//sendText(SEVERITY_HIGH, PSTR("not receiving commands"));
+				break;
+			}
+
+			// decode
+			mavlink_waypoint_t packet;
+			mavlink_msg_waypoint_decode(msg, &packet);
+			if (_checkTarget(packet.target_system, packet.target_component))
+				break;
+
+			// check if this is the requested waypoint
+			if (packet.seq != _cmdRequestIndex) {
+				char warningMsg[50];
+				sprintf(warningMsg,
+				"waypoint request out of sequence: (packet) %d / %d (ap)",
+				packet.seq, _cmdRequestIndex);
+				sendText(SEVERITY_HIGH, warningMsg);
+				break;
+			}
+
+			_hal->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"),
+					packet.x,
+					packet.y,
+					packet.z);
+
+			// store waypoint
+			AP_MavlinkCommand command(packet);
+			//sendText(SEVERITY_HIGH, PSTR("waypoint stored"));
+			_cmdRequestIndex++;
+			if (_cmdRequestIndex == _cmdNumberRequested) {
+				sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK);
+				_receivingCmds = false;
+				_guide->setNumberOfCommands(_cmdNumberRequested);
+				//sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
+			} else if (_cmdRequestIndex > _cmdNumberRequested) {
+				_receivingCmds = false;
+			}
+			_cmdTimeLastReceived = millis();
+			break;
+		}
+
+		case MAVLINK_MSG_ID_PARAM_SET: {
+			sendText(SEVERITY_LOW, PSTR("param set"));
+			AP_Var *vp;
+			AP_Meta_class::Type_id var_type;
+
+			// decode
+			mavlink_param_set_t packet;
+			mavlink_msg_param_set_decode(msg, &packet);
+			if (_checkTarget(packet.target_system, packet.target_component))
+				break;
+
+			// set parameter
+
+			char key[_paramNameLengthMax + 1];
+			strncpy(key, (char *) packet.param_id, _paramNameLengthMax);
+			key[_paramNameLengthMax] = 0;
+
+			// find the requested parameter
+			vp = AP_Var::find(key);
+			if ((NULL != vp) && // exists
+					!isnan(packet.param_value) && // not nan
+					!isinf(packet.param_value)) { // not inf
+
+				// add a small amount before casting parameter values
+				// from float to integer to avoid truncating to the
+				// next lower integer value.
+				const float rounding_addition = 0.01;
+
+				// fetch the variable type ID
+				var_type = vp->meta_type_id();
+
+				// handle variables with standard type IDs
+				if (var_type == AP_Var::k_typeid_float) {
+					((AP_Float *) vp)->set_and_save(packet.param_value);
+
+				} else if (var_type == AP_Var::k_typeid_float16) {
+					((AP_Float16 *) vp)->set_and_save(packet.param_value);
+
+				} else if (var_type == AP_Var::k_typeid_int32) {
+					((AP_Int32 *) vp)->set_and_save(
+							packet.param_value + rounding_addition);
+
+				} else if (var_type == AP_Var::k_typeid_int16) {
+					((AP_Int16 *) vp)->set_and_save(
+							packet.param_value + rounding_addition);
+
+				} else if (var_type == AP_Var::k_typeid_int8) {
+					((AP_Int8 *) vp)->set_and_save(
+							packet.param_value + rounding_addition);
+				} else {
+					// we don't support mavlink set on this parameter
+					break;
+				}
+
+				// Report back the new value if we accepted the change
+				// we send the value we actually set, which could be
+				// different from the value sent, in case someone sent
+				// a fractional value to an integer type
+				mavlink_msg_param_value_send(_channel, (int8_t *) key,
+						vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is...
+			}
+
+			break;
+		} // end case
+
+
+		}
+	}
+
+	uint16_t _countParameters() {
+		// if we haven't cached the parameter count yet...
+		if (0 == _parameterCount) {
+			AP_Var *vp;
+
+			vp = AP_Var::first();
+			do {
+				// if a parameter responds to cast_to_float then we are going to be able to report it
+				if (!isnan(vp->cast_to_float())) {
+					_parameterCount++;
+				}
+			} while (NULL != (vp = vp->next()));
+		}
+		return _parameterCount;
+	}
+
+	AP_Var * _findParameter(uint16_t index) {
+		AP_Var *vp;
+
+		vp = AP_Var::first();
+		while (NULL != vp) {
+
+			// if the parameter is reportable
+			if (!(isnan(vp->cast_to_float()))) {
+				// if we have counted down to the index we want
+				if (0 == index) {
+					// return the parameter
+					return vp;
+				}
+				// count off this parameter, as it is reportable but not
+				// the one we want
+				index--;
+			}
+			// and move to the next parameter
+			vp = vp->next();
+		}
+		return NULL;
+	}
+
+	// check the target
+	uint8_t _checkTarget(uint8_t sysid, uint8_t compid) {
+		/*
+		 char msg[50];
+		 sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid,
+		 mavlink_system.sysid, compid, mavlink_system.compid);
+		 sendText(SEVERITY_LOW, msg);
+		 */
+		if (sysid != mavlink_system.sysid) {
+			//sendText(SEVERITY_LOW, PSTR("system id mismatch"));
+			return 1;
+
+		} else if (compid != mavlink_system.compid) {
+			//sendText(SEVERITY_LOW, PSTR("component id mismatch"));
+			return 0; // XXX currently not receiving correct compid from gcs
+
+		} else {
+			return 0; // no error
+		}
+	}
+
+};
+
+} // namespace apo
+
+#endif // AP_CommLink_H
+// vim:ts=4:sw=4:tw=78:expandtab

libraries/APO/AP_Controller.cpp

@@ -0,0 +1,8 @@
+/*
+ * AP_Controller.cpp
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#include "AP_Controller.h"

libraries/APO/AP_Controller.h

@@ -0,0 +1,304 @@
+/*
+ * AP_Controller.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_Controller_H
+#define AP_Controller_H
+
+#include "AP_Navigator.h"
+#include "AP_Guide.h"
+#include "AP_HardwareAbstractionLayer.h"
+#include "../AP_Common/AP_Vector.h"
+#include "../AP_Common/AP_Var.h"
+
+namespace apo {
+
+/// Controller class
+class AP_Controller {
+public:
+	AP_Controller(AP_Navigator * nav, AP_Guide * guide,
+			AP_HardwareAbstractionLayer * hal) :
+		_nav(nav), _guide(guide), _hal(hal) {
+	}
+
+	virtual void update(const float & dt) = 0;
+
+	virtual MAV_MODE getMode() = 0;
+
+	void setAllRadioChannelsToNeutral() {
+		for (uint8_t i = 0; i < _hal->rc.getSize(); i++) {
+			_hal->rc[i]->setPosition(0.0);
+		}
+	}
+
+	void setAllRadioChannelsManually() {
+		for (uint8_t i = 0; i < _hal->rc.getSize(); i++) {
+			_hal->rc[i]->setUsingRadio();
+		}
+	}
+
+protected:
+	AP_Navigator * _nav;
+	AP_Guide * _guide;
+	AP_HardwareAbstractionLayer * _hal;
+};
+
+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, 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),
+				_kD(group, _blockSaturation.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
+				* derivative;
+		return _blockSaturation.update(y);
+	}
+protected:
+	BlockP _blockP;
+	BlockI _blockI;
+	BlockSaturation _blockSaturation;
+	AP_Float _kD; /// derivative gain
+};
+
+} // apo
+
+#endif // AP_Controller_H
+// vim:ts=4:sw=4:expandtab

libraries/APO/AP_Guide.cpp

@@ -0,0 +1,8 @@
+/*
+ * AP_Guide.cpp
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#include "AP_Guide.h"

libraries/APO/AP_Guide.h

@@ -0,0 +1,359 @@
+/*
+ * AP_Guide.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_Guide_H
+#define AP_Guide_H
+
+#include "../GCS_MAVLink/GCS_MAVLink.h"
+#include "AP_HardwareAbstractionLayer.h"
+#include "AP_Navigator.h"
+#include "../AP_Common/AP_Common.h"
+#include "../AP_Common/AP_Vector.h"
+#include "AP_MavlinkCommand.h"
+#include "constants.h"
+
+//#include "AP_CommLink.h"
+
+namespace apo {
+
+/// Guide class
+class AP_Guide {
+public:
+
+	/**
+	 * This is the constructor, which requires a link to the navigator.
+	 * @param navigator This is the navigator pointer.
+	 */
+	AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) :
+		_navigator(navigator), _hal(hal), _command(0), _previousCommand(0),
+				_headingCommand(0), _airSpeedCommand(0),
+				_groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0),
+				_pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST),
+				_numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0),
+				_nextCommandTimer(0) {
+	}
+
+	virtual void update() = 0;
+
+	virtual void nextCommand() = 0;
+
+	MAV_NAV getMode() const {
+		return _mode;
+	}
+	uint8_t getCurrentIndex() {
+		return _cmdIndex;
+	}
+
+	void setCurrentIndex(uint8_t val) {
+		_cmdIndex.set_and_save(val);
+		_command = AP_MavlinkCommand(getCurrentIndex());
+		_command.load();
+		_previousCommand = AP_MavlinkCommand(getPreviousIndex());
+		_previousCommand.load();
+		//_hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT);
+	}
+
+	uint8_t getNumberOfCommands() {
+		return _numberOfCommands;
+	}
+
+	void setNumberOfCommands(uint8_t val) {
+		_numberOfCommands.set_and_save(val);
+	}
+
+	uint8_t getPreviousIndex() {
+		// find previous waypoint, TODO, handle non-nav commands
+		int16_t prevIndex = int16_t(getCurrentIndex()) - 1;
+		if (prevIndex < 0)
+			prevIndex = getNumberOfCommands() - 1;
+		return (uint8_t) prevIndex;
+	}
+
+	uint8_t getNextIndex() {
+		// find previous waypoint, TODO, handle non-nav commands
+		int16_t nextIndex = int16_t(getCurrentIndex()) + 1;
+		if (nextIndex > (getNumberOfCommands() - 1))
+			nextIndex = 0;
+		return nextIndex;
+	}
+
+	float getHeadingCommand() {
+		return _headingCommand;
+	}
+	float getAirSpeedCommand() {
+		return _airSpeedCommand;
+	}
+	float getGroundSpeedCommand() {
+		return _groundSpeedCommand;
+	}
+	float getAltitudeCommand() {
+		return _altitudeCommand;
+	}
+	float getPNCmd() {
+		return _pNCmd;
+	}
+	float getPECmd() {
+		return _pECmd;
+	}
+	float getPDCmd() {
+		return _pDCmd;
+	}
+	MAV_NAV getMode() {
+		return _mode;
+	}
+	uint8_t getCommandIndex() {
+		return _cmdIndex;
+	}
+
+protected:
+	AP_Navigator * _navigator;
+	AP_HardwareAbstractionLayer * _hal;
+	AP_MavlinkCommand _command, _previousCommand;
+	float _headingCommand;
+	float _airSpeedCommand;
+	float _groundSpeedCommand;
+	float _altitudeCommand;
+	float _pNCmd;
+	float _pECmd;
+	float _pDCmd;
+	MAV_NAV _mode;
+	AP_Uint8 _numberOfCommands;
+	AP_Uint8 _cmdIndex;
+	uint16_t _nextCommandCalls;
+	uint16_t _nextCommandTimer;
+};
+
+class MavlinkGuide: public AP_Guide {
+public:
+	MavlinkGuide(AP_Navigator * navigator,
+			AP_HardwareAbstractionLayer * hal) :
+		AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),
+				_rangeFinderLeft(), _rangeFinderRight(),
+				_group(k_guide, PSTR("guide_")),
+				_velocityCommand(&_group, 1, 1, PSTR("velCmd")),
+				_crossTrackGain(&_group, 2, 2, PSTR("xt")),
+				_crossTrackLim(&_group, 3, 10, PSTR("xtLim")) {
+
+		for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) {
+			RangeFinder * rF = _hal->rangeFinders[i];
+			if (rF == NULL)
+				continue;
+			if (rF->orientation_x == 1 && rF->orientation_y == 0
+					&& rF->orientation_z == 0)
+				_rangeFinderFront = rF;
+			else if (rF->orientation_x == -1 && rF->orientation_y == 0
+					&& rF->orientation_z == 0)
+				_rangeFinderBack = rF;
+			else if (rF->orientation_x == 0 && rF->orientation_y == 1
+					&& rF->orientation_z == 0)
+				_rangeFinderRight = rF;
+			else if (rF->orientation_x == 0 && rF->orientation_y == -1
+					&& rF->orientation_z == 0)
+				_rangeFinderLeft = rF;
+
+		}
+	}
+
+	virtual void update() {
+//		_hal->debug->printf_P(
+//				PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),
+//				getNumberOfCommands(),
+//				getCurrentIndex(),
+//				getPreviousIndex());
+
+		// if we don't have enough waypoint for cross track calcs
+		// go home
+		if (_numberOfCommands == 1) {
+			_mode = MAV_NAV_RETURNING;
+			_altitudeCommand = AP_MavlinkCommand::home.getAlt();
+			_headingCommand = AP_MavlinkCommand::home.bearingTo(
+					_navigator->getLat_degInt(), _navigator->getLon_degInt())
+					+ 180 * deg2Rad;
+			if (_headingCommand > 360 * deg2Rad)
+				_headingCommand -= 360 * deg2Rad;
+
+//			_hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"),
+//			headingCommand,AP_MavlinkCommand::home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt()));
+		} else {
+			_mode = MAV_NAV_WAYPOINT;
+			_altitudeCommand = _command.getAlt();
+			// TODO wrong behavior if 0 selected as waypoint, says previous 0
+			float dXt = _command.crossTrack(_previousCommand,
+					_navigator->getLat_degInt(),
+					_navigator->getLon_degInt());
+			float temp = dXt * _crossTrackGain * deg2Rad; // crosstrack gain, rad/m
+			if (temp > _crossTrackLim * deg2Rad)
+				temp = _crossTrackLim * deg2Rad;
+			if (temp < -_crossTrackLim * deg2Rad)
+				temp = -_crossTrackLim * deg2Rad;
+			float bearing = _previousCommand.bearingTo(_command);
+			_headingCommand = bearing - temp;
+			float alongTrack = _command.alongTrack(_previousCommand,
+					_navigator->getLat_degInt(),
+					_navigator->getLon_degInt());
+			float distanceToNext = _command.distanceTo(
+					_navigator->getLat_degInt(), _navigator->getLon_degInt());
+			float segmentLength = _previousCommand.distanceTo(_command);
+			if (distanceToNext < _command.getRadius() || alongTrack
+					> segmentLength)
+			{
+				Serial.println("radius reached");
+				nextCommand();
+			}
+//			_hal->debug->printf_P(
+//					PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"),
+//					bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack);
+		}
+
+		_groundSpeedCommand = _velocityCommand;
+
+		// TODO : 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());
+
+		// process mavlink commands
+		//handleCommand();
+
+		// obstacle avoidance overrides
+		// stop if your going to drive into something in front of you
+		for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++)
+			_hal->rangeFinders[i]->read();
+		float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc
+		if (_rangeFinderFront && frontDistance < 2) {
+			_mode = MAV_NAV_VECTOR;
+			//airSpeedCommand = 0;
+			//groundSpeedCommand = 0;
+//			_headingCommand -= 45 * deg2Rad;
+//			_hal->debug->print("Obstacle Distance (m): ");
+//			_hal->debug->println(frontDistance);
+//			_hal->debug->print("Obstacle avoidance Heading Command: ");
+//			_hal->debug->println(headingCommand);
+//			_hal->debug->printf_P(
+//					PSTR("Front Distance, %f\n"),
+//					frontDistance);
+		}
+		if (_rangeFinderBack && _rangeFinderBack->distance < 5) {
+			_airSpeedCommand = 0;
+			_groundSpeedCommand = 0;
+
+		}
+
+		if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) {
+			_airSpeedCommand = 0;
+			_groundSpeedCommand = 0;
+		}
+
+		if (_rangeFinderRight && _rangeFinderRight->distance < 5) {
+			_airSpeedCommand = 0;
+			_groundSpeedCommand = 0;
+		}
+	}
+
+	void nextCommand() {
+		// within 1 seconds, check if more than 5 calls to next command occur
+		// if they do, go to home waypoint
+		if (millis() - _nextCommandTimer < 1000) {
+			if (_nextCommandCalls > 5) {
+				Serial.println("commands loading too fast, returning home");
+				setCurrentIndex(0);
+				setNumberOfCommands(1);
+				_nextCommandCalls = 0;
+				_nextCommandTimer = millis();
+				return;
+			}
+			_nextCommandCalls++;
+		} else {
+			_nextCommandTimer = millis();
+			_nextCommandCalls = 0;
+		}
+
+		_cmdIndex = getNextIndex();
+		Serial.print("cmd       : "); Serial.println(int(_cmdIndex));
+		Serial.print("cmd prev  : "); Serial.println(int(getPreviousIndex()));
+		Serial.print("cmd num    : "); Serial.println(int(getNumberOfCommands()));
+		_command = AP_MavlinkCommand(getCurrentIndex());
+		_previousCommand = AP_MavlinkCommand(getPreviousIndex());
+	}
+
+	void handleCommand(AP_MavlinkCommand command,
+			AP_MavlinkCommand previousCommand) {
+		// TODO handle more commands
+		switch (command.getCommand()) {
+		case MAV_CMD_NAV_WAYPOINT: {
+			// if within radius, increment
+			float d = previousCommand.distanceTo(_navigator->getLat_degInt(),
+					_navigator->getLon_degInt());
+			if (d < command.getRadius()) {
+				nextCommand();
+				Serial.println("radius reached");
+			}
+			break;
+		}
+//		case MAV_CMD_CONDITION_CHANGE_ALT:
+//		case MAV_CMD_CONDITION_DELAY:
+//		case MAV_CMD_CONDITION_DISTANCE:
+//		case MAV_CMD_CONDITION_LAST:
+//		case MAV_CMD_CONDITION_YAW:
+//		case MAV_CMD_DO_CHANGE_SPEED:
+//		case MAV_CMD_DO_CONTROL_VIDEO:
+//		case MAV_CMD_DO_JUMP:
+//	    case MAV_CMD_DO_LAST:
+//		case MAV_CMD_DO_LAST:
+//		case MAV_CMD_DO_REPEAT_RELAY:
+//		case MAV_CMD_DO_REPEAT_SERVO:
+//		case MAV_CMD_DO_SET_HOME:
+//		case MAV_CMD_DO_SET_MODE:
+//		case MAV_CMD_DO_SET_PARAMETER:
+//		case MAV_CMD_DO_SET_RELAY:
+//		case MAV_CMD_DO_SET_SERVO:
+//		case MAV_CMD_PREFLIGHT_CALIBRATION:
+//		case MAV_CMD_PREFLIGHT_STORAGE:
+//		case MAV_CMD_NAV_LAND:
+//		case MAV_CMD_NAV_LAST:
+//		case MAV_CMD_NAV_LOITER_TIME:
+//		case MAV_CMD_NAV_LOITER_TURNS:
+//		case MAV_CMD_NAV_LOITER_UNLIM:
+//		case MAV_CMD_NAV_ORIENTATION_TARGET:
+//		case MAV_CMD_NAV_PATHPLANNING:
+//		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+//		case MAV_CMD_NAV_TAKEOFF:
+		default:
+			// unhandled command, skip
+			Serial.println("unhandled command");
+			nextCommand();
+			break;
+		}
+	}
+private:
+	RangeFinder * _rangeFinderFront;
+	RangeFinder * _rangeFinderBack;
+	RangeFinder * _rangeFinderLeft;
+	RangeFinder * _rangeFinderRight;
+	AP_Var_group _group;
+	AP_Float _velocityCommand;
+	AP_Float _crossTrackGain;
+	AP_Float _crossTrackLim;
+};
+
+} // namespace apo
+
+#endif // AP_Guide_H
+// vim:ts=4:sw=4:expandtab

libraries/APO/AP_HardwareAbstractionLayer.cpp

@@ -0,0 +1,8 @@
+/*
+ * AP_HardwareAbstractionLayer.cpp
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#include "AP_HardwareAbstractionLayer.h"

libraries/APO/AP_HardwareAbstractionLayer.h

@@ -0,0 +1,166 @@
+/*
+ * AP_HardwareAbstractionLayer.h
+ *
+ *  Created on: Apr 4, 2011
+ *
+ */
+
+#ifndef AP_HARDWAREABSTRACTIONLAYER_H_
+#define AP_HARDWAREABSTRACTIONLAYER_H_
+
+#include "../AP_Common/AP_Common.h"
+#include "../FastSerial/FastSerial.h"
+#include "../AP_Common/AP_Vector.h"
+#include "../GCS_MAVLink/GCS_MAVLink.h"
+
+#include "../AP_ADC/AP_ADC.h"
+#include "../AP_IMU/AP_IMU.h"
+#include "../AP_GPS/GPS.h"
+#include "../APM_BMP085/APM_BMP085.h"
+#include "../AP_Compass/AP_Compass.h"
+#include "AP_RcChannel.h"
+#include "../AP_RangeFinder/AP_RangeFinder.h"
+#include "../GCS_MAVLink/GCS_MAVLink.h"
+
+class AP_ADC;
+class IMU;
+class GPS;
+class APM_BMP085_Class;
+class Compass;
+class BetterStream;
+class RangeFinder;
+
+namespace apo {
+
+class AP_RcChannel;
+class AP_CommLink;
+
+// enumerations
+enum halMode_t {
+	MODE_LIVE, MODE_HIL_CNTL,
+/*MODE_HIL_NAV*/};
+enum board_t {
+	BOARD_ARDUPILOTMEGA_1280, BOARD_ARDUPILOTMEGA_2560, BOARD_ARDUPILOTMEGA_2
+};
+enum vehicle_t {
+	VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE, VEHICLE_BOAT, VEHICLE_TANK
+};
+
+class AP_HardwareAbstractionLayer {
+
+public:
+
+	AP_HardwareAbstractionLayer(halMode_t mode, board_t board,
+			vehicle_t vehicle, uint8_t heartBeatTimeout) :
+		adc(), gps(), baro(), compass(), rangeFinders(), imu(), rc(), gcs(),
+				hil(), debug(), load(), lastHeartBeat(),
+				_heartBeatTimeout(heartBeatTimeout), _mode(mode),
+				_board(board), _vehicle(vehicle), _state(MAV_STATE_UNINIT) {
+
+		/*
+		 * Board specific hardware initialization
+		 */
+		if (board == BOARD_ARDUPILOTMEGA_1280) {
+			slideSwitchPin = 40;
+			pushButtonPin = 41;
+			aLedPin = 37;
+			bLedPin = 36;
+			cLedPin = 35;
+			eepromMaxAddr = 1024;
+			pinMode(aLedPin, OUTPUT); //  extra led
+			pinMode(bLedPin, OUTPUT); //  imu ledclass AP_CommLink;
+			pinMode(cLedPin, OUTPUT); //  gps led
+			pinMode(slideSwitchPin, INPUT);
+			pinMode(pushButtonPin, INPUT);
+			DDRL |= B00000100; // set port L, pint 2 to output for the relay
+		} else if (board == BOARD_ARDUPILOTMEGA_2560) {
+			slideSwitchPin = 40;
+			pushButtonPin = 41;
+			aLedPin = 37;
+			bLedPin = 36;
+			cLedPin = 35;
+			eepromMaxAddr = 2048;
+			pinMode(aLedPin, OUTPUT); //  extra led
+			pinMode(bLedPin, OUTPUT); //  imu ledclass AP_CommLink;
+			pinMode(cLedPin, OUTPUT); //  gps led
+			pinMode(slideSwitchPin, INPUT);
+			pinMode(pushButtonPin, INPUT);
+			DDRL |= B00000100; // set port L, pint 2 to output for the relay
+		}
+	}
+
+	/**
+	 * Sensors
+	 */
+	AP_ADC * adc;
+	GPS * gps;
+	APM_BMP085_Class * baro;
+	Compass * compass;
+	Vector<RangeFinder *> rangeFinders;
+	IMU * imu;
+
+	/**
+	 * Radio Channels
+	 */
+	Vector<AP_RcChannel *> rc;
+
+	/**
+	 * Communication Channels
+	 */
+	AP_CommLink * gcs;
+	AP_CommLink * hil;
+	FastSerial * debug;
+
+	/**
+	 * data
+	 */
+	uint8_t load;
+	uint32_t lastHeartBeat;
+
+	/**
+	 * settings
+	 */
+	uint8_t slideSwitchPin;
+	uint8_t pushButtonPin;
+	uint8_t aLedPin;
+	uint8_t bLedPin;
+	uint8_t cLedPin;
+	uint16_t eepromMaxAddr;
+
+	// accessors
+	halMode_t getMode() {
+		return _mode;
+	}
+	board_t getBoard() {
+		return _board;
+	}
+	vehicle_t getVehicle() {
+		return _vehicle;
+	}
+	MAV_STATE getState() {
+		return _state;
+	}
+	void setState(MAV_STATE state) {
+		_state = state;
+	}
+
+	bool heartBeatLost() {
+		if (_heartBeatTimeout == 0)
+			return false;
+		else
+			return ((micros() - lastHeartBeat) / 1e6) > _heartBeatTimeout;
+	}
+
+private:
+
+	// enumerations
+	uint8_t _heartBeatTimeout;
+	halMode_t _mode;
+	board_t _board;
+	vehicle_t _vehicle;
+	MAV_STATE _state;
+};
+
+}
+
+#endif /* AP_HARDWAREABSTRACTIONLAYER_H_ */

libraries/APO/AP_MavlinkCommand.cpp

@@ -0,0 +1,175 @@
+/*
+ * AP_MavlinkCommand.cpp
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#include "AP_MavlinkCommand.h"
+
+namespace apo {
+
+AP_MavlinkCommand::AP_MavlinkCommand(const AP_MavlinkCommand & v) :
+	_data(v._data), _seq(v._seq) {
+	//if (FastSerial::getInitialized(0)) Serial.println("copy ctor");
+}
+
+AP_MavlinkCommand::AP_MavlinkCommand(uint16_t index, bool doLoad) :
+	_data(k_commands + index), _seq(index) {
+
+	if (FastSerial::getInitialized(0)) {
+		Serial.println("index ctor");
+		Serial.println("++");
+		Serial.print("index: "); Serial.println(index);
+		Serial.print("key: "); Serial.println(k_commands + index);
+		Serial.println("++");
+	}
+	// This is a failsafe measure to stop trying to load a command if it can't load
+	if (doLoad && !load()) {
+		Serial.println("load failed, reverting to home waypoint");
+		_data = AP_MavlinkCommand::home._data;
+		_seq = AP_MavlinkCommand::home._seq;
+	}
+}
+
+AP_MavlinkCommand::AP_MavlinkCommand(mavlink_waypoint_t cmd) :
+	_data(k_commands + cmd.seq), _seq(cmd.seq) {
+	setCommand(MAV_CMD(cmd.command));
+	setAutocontinue(cmd.autocontinue);
+	setFrame(MAV_FRAME(cmd.frame));
+	setParam1(cmd.param1);
+	setParam2(cmd.param2);
+	setParam3(cmd.param3);
+	setParam4(cmd.param4);
+	setX(cmd.x);
+	setY(cmd.y);
+	setZ(cmd.z);
+	save();
+
+	// reload home if sent
+	if (cmd.seq == 0) home.load();
+
+	Serial.println("============================================================");
+	Serial.println("storing new command from mavlink_waypoint_t");
+	Serial.print("key: "); Serial.println(_data.key(),DEC);
+	Serial.print("number: "); Serial.println(cmd.seq,DEC);
+	Serial.print("command: "); Serial.println(getCommand());
+	Serial.print("autocontinue: "); Serial.println(getAutocontinue(),DEC);
+	Serial.print("frame: "); Serial.println(getFrame(),DEC);
+	Serial.print("1000*param1: "); Serial.println(int(1000*getParam1()),DEC);
+	Serial.print("1000*param2: "); Serial.println(int(1000*getParam2()),DEC);
+	Serial.print("1000*param3: "); Serial.println(int(1000*getParam3()),DEC);
+	Serial.print("1000*param4: "); Serial.println(int(1000*getParam4()),DEC);
+	Serial.print("1000*x0: "); Serial.println(int(1000*cmd.x),DEC);
+	Serial.print("1000*y0: "); Serial.println(int(1000*cmd.y),DEC);
+	Serial.print("1000*z0: "); Serial.println(int(1000*cmd.z),DEC);
+	Serial.print("1000*x: "); Serial.println(int(1000*getX()),DEC);
+	Serial.print("1000*y: "); Serial.println(int(1000*getY()),DEC);
+	Serial.print("1000*z: "); Serial.println(int(1000*getZ()),DEC);
+
+	load();
+
+	Serial.print("1000*x1: "); Serial.println(int(1000*getX()),DEC);
+	Serial.print("1000*y1: "); Serial.println(int(1000*getY()),DEC);
+	Serial.print("1000*z1: "); Serial.println(int(1000*getZ()),DEC);
+	Serial.println("============================================================");
+	Serial.flush();
+}
+
+mavlink_waypoint_t AP_MavlinkCommand::convert(uint8_t current) {
+	mavlink_waypoint_t mavCmd;
+	mavCmd.seq = getSeq();
+	mavCmd.command = getCommand();
+	mavCmd.frame = getFrame();
+	mavCmd.param1 = getParam1();
+	mavCmd.param2 = getParam2();
+	mavCmd.param3 = getParam3();
+	mavCmd.param4 = getParam4();
+	mavCmd.x = getX();
+	mavCmd.y = getY();
+	mavCmd.z = getZ();
+	mavCmd.autocontinue = getAutocontinue();
+	mavCmd.current = (getSeq() == current);
+	mavCmd.target_component = mavlink_system.compid;
+	mavCmd.target_system = mavlink_system.sysid;
+	return mavCmd;
+}
+
+float AP_MavlinkCommand::bearingTo(AP_MavlinkCommand next) const {
+	float deltaLon = next.getLon() - getLon();
+	/*
+	 Serial.print("Lon: "); Serial.println(getLon());
+	 Serial.print("nextLon: "); Serial.println(next.getLon());
+	 Serial.print("deltaLonDeg * 1e7: "); Serial.println(deltaLon*rad2DegInt);
+	 */
+	float bearing = atan2(
+			sin(deltaLon) * cos(next.getLat()),
+			cos(getLat()) * sin(next.getLat()) - sin(getLat()) * cos(
+					next.getLat()) * cos(deltaLon));
+	return bearing;
+}
+
+float AP_MavlinkCommand::bearingTo(int32_t latDegInt, int32_t lonDegInt) const {
+	// have to be careful to maintain the precision of the gps coordinate
+	float deltaLon = (lonDegInt - getLon_degInt()) * degInt2Rad;
+	float nextLat = latDegInt * degInt2Rad;
+	float bearing = atan2(
+			sin(deltaLon) * cos(nextLat),
+			cos(getLat()) * sin(nextLat) - sin(getLat()) * cos(nextLat)
+					* cos(deltaLon));
+	if (bearing < 0)
+		bearing += 2 * M_PI;
+	return bearing;
+}
+
+float AP_MavlinkCommand::distanceTo(AP_MavlinkCommand next) const {
+	float sinDeltaLat2 = sin((getLat() - next.getLat()) / 2);
+	float sinDeltaLon2 = sin((getLon() - next.getLon()) / 2);
+	float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos(
+			next.getLat()) * sinDeltaLon2 * sinDeltaLon2;
+	float c = 2 * atan2(sqrt(a), sqrt(1 - a));
+	return rEarth * c;
+}
+
+float AP_MavlinkCommand::distanceTo(int32_t lat_degInt, int32_t lon_degInt) const {
+	float sinDeltaLat2 = sin(
+			(lat_degInt - getLat_degInt()) * degInt2Rad / 2);
+	float sinDeltaLon2 = sin(
+			(lon_degInt - getLon_degInt()) * degInt2Rad / 2);
+	float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos(
+			lat_degInt * degInt2Rad) * sinDeltaLon2 * sinDeltaLon2;
+	float c = 2 * atan2(sqrt(a), sqrt(1 - a));
+	/*
+	 Serial.print("wp lat_degInt: "); Serial.println(getLat_degInt());
+	 Serial.print("wp lon_degInt: "); Serial.println(getLon_degInt());
+	 Serial.print("lat_degInt: "); Serial.println(lat_degInt);
+	 Serial.print("lon_degInt: "); Serial.println(lon_degInt);
+	 Serial.print("sinDeltaLat2: "); Serial.println(sinDeltaLat2);
+	 Serial.print("sinDeltaLon2: "); Serial.println(sinDeltaLon2);
+	 */
+	return rEarth * c;
+}
+
+//calculates cross track of a current location
+float AP_MavlinkCommand::crossTrack(AP_MavlinkCommand previous,
+		int32_t lat_degInt, int32_t lon_degInt) {
+		float d = previous.distanceTo(lat_degInt, lon_degInt);
+		float bCurrent = previous.bearingTo(lat_degInt, lon_degInt);
+		float bNext = previous.bearingTo(*this);
+		return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth;
+	return 0;
+}
+
+// calculates along  track distance of a current location
+float AP_MavlinkCommand::alongTrack(AP_MavlinkCommand previous,
+		int32_t lat_degInt, int32_t lon_degInt) {
+	// ignores lat/lon since single prec.
+	float dXt = this->crossTrack(previous,lat_degInt, lon_degInt);
+	float d = previous.distanceTo(lat_degInt, lon_degInt);
+	return dXt / tan(asin(dXt / d));
+}
+
+
+AP_MavlinkCommand AP_MavlinkCommand::home = AP_MavlinkCommand(0,false);
+
+}

libraries/APO/AP_MavlinkCommand.h

@@ -0,0 +1,359 @@
+/*
+ * AP_MavlinkCommand.h
+ *
+ *  Created on: Apr 4, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef AP_MAVLINKCOMMAND_H_
+#define AP_MAVLINKCOMMAND_H_
+
+#include "../GCS_MAVLink/GCS_MAVLink.h"
+#include "../AP_Common/AP_Common.h"
+#include "AP_Var_keys.h"
+#include "constants.h"
+#include "../FastSerial/FastSerial.h"
+
+namespace apo {
+
+class AP_MavlinkCommand {
+private:
+	struct CommandStorage {
+		MAV_CMD command;
+		bool autocontinue;
+		MAV_FRAME frame;
+		float param1;
+		float param2;
+		float param3;
+		float param4;
+		float x;
+		float y;
+		float z;
+	};
+	AP_VarS<CommandStorage> _data;
+	uint16_t _seq;
+public:
+	static AP_MavlinkCommand home;
+
+	/**
+	 * Copy Constructor
+	 */
+	AP_MavlinkCommand(const AP_MavlinkCommand & v);
+
+	/**
+	 * Basic Constructor
+	 * @param index Start at zero.
+	 */
+	AP_MavlinkCommand(uint16_t index, bool doLoad = true);
+
+	/**
+	 * Constructor for copying/ saving from a mavlink waypoint.
+	 * @param cmd The mavlink_waopint_t structure for the command.
+	 */
+	AP_MavlinkCommand(mavlink_waypoint_t cmd);
+
+	bool save() {
+		return _data.save();
+	}
+	bool load() {
+		return _data.load();
+	}
+	uint8_t getSeq() const {
+		return _seq;
+	}
+	bool getAutocontinue() const {
+		return _data.get().autocontinue;
+	}
+	void setAutocontinue(bool val) {
+		_data.get().autocontinue = val;
+	}
+	void setSeq(uint8_t val) {
+		_seq = val;
+	}
+	MAV_CMD getCommand() const {
+		return _data.get().command;
+	}
+	void setCommand(MAV_CMD val) {
+		_data.get().command = val;
+	}
+	MAV_FRAME getFrame() const {
+		return _data.get().frame;
+	}
+	void setFrame(MAV_FRAME val) {
+		_data.get().frame = val;
+	}
+	float getParam1() const {
+		return _data.get().param1;
+	}
+	void setParam1(float val) {
+		_data.get().param1 = val;
+	}
+	float getParam2() const {
+		return _data.get().param2;
+	}
+	void setParam2(float val) {
+		_data.get().param2 = val;
+	}
+	float getParam3() const {
+		return _data.get().param3;
+	}
+	void setParam3(float val) {
+		_data.get().param3 = val;
+	}
+	float getParam4() const {
+		return _data.get().param4;
+	}
+	void setParam4(float val) {
+		_data.get().param4 = val;
+	}
+	float getX() const {
+		return _data.get().x;
+	}
+	void setX(float val) {
+		_data.get().x = val;
+	}
+	float getY() const {
+		return _data.get().y;
+	}
+	void setY(float val) {
+		_data.get().y = val;
+	}
+	float getZ() const {
+		return _data.get().z;
+	}
+	void setZ(float val) {
+		_data.get().z = val;
+	}
+	float getLatDeg() const {
+		switch (getFrame()) {
+		case MAV_FRAME_GLOBAL:
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+			return getX();
+			break;
+		case MAV_FRAME_LOCAL:
+		case MAV_FRAME_MISSION:
+		default:
+			return 0;
+			break;
+		}
+	}
+	void setLatDeg(float val) {
+		switch (getFrame()) {
+		case MAV_FRAME_GLOBAL:
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+			setX(val);
+			break;
+		case MAV_FRAME_LOCAL:
+		case MAV_FRAME_MISSION:
+		default:
+			break;
+		}
+	}
+	float getLonDeg() const {
+		switch (getFrame()) {
+		case MAV_FRAME_GLOBAL:
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+			return getY();
+			break;
+		case MAV_FRAME_LOCAL:
+		case MAV_FRAME_MISSION:
+		default:
+			return 0;
+			break;
+		}
+	}
+	void setLonDeg(float val) {
+		switch (getFrame()) {
+		case MAV_FRAME_GLOBAL:
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+			setY(val);
+			break;
+		case MAV_FRAME_LOCAL:
+		case MAV_FRAME_MISSION:
+		default:
+			break;
+		}
+	}
+	void setLon(float val) {
+		setLonDeg(val * rad2Deg);
+	}
+	void setLon_degInt(int32_t val) {
+		setLonDeg(val / 1.0e7);
+	}
+	void setLat_degInt(int32_t val) {
+		setLatDeg(val / 1.0e7);
+	}
+	int32_t getLon_degInt() const {
+		return getLonDeg() * 1e7;
+	}
+	int32_t getLat_degInt() const {
+		return getLatDeg() * 1e7;
+	}
+	float getLon() const {
+		return getLonDeg() * deg2Rad;
+	}
+	float getLat() const {
+		return getLatDeg() * deg2Rad;
+	}
+	void setLat(float val) {
+		setLatDeg(val * rad2Deg);
+	}
+	float getAlt() const {
+		switch (getFrame()) {
+		case MAV_FRAME_GLOBAL:
+			return getZ();
+			break;
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+		case MAV_FRAME_LOCAL:
+			return getZ() + home.getAlt();
+			break;
+		case MAV_FRAME_MISSION:
+		default:
+			return 0;
+			break;
+		}
+	}
+	/**
+	 * set the altitude in meters
+	 */
+	void setAlt(float val) {
+		switch (getFrame()) {
+		case MAV_FRAME_GLOBAL:
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+			setZ(val);
+			break;
+		case MAV_FRAME_LOCAL:
+			setZ(val - home.getLonDeg());
+			break;
+		case MAV_FRAME_MISSION:
+		default:
+			break;
+		}
+	}
+	/**
+	 * Get the relative altitude to home
+	 * @return relative altitude in meters
+	 */
+	float getRelAlt() const {
+		switch (getFrame()) {
+		case MAV_FRAME_GLOBAL:
+			return getZ() - home.getAlt();
+			break;
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+		case MAV_FRAME_LOCAL:
+			return getZ();
+			break;
+		case MAV_FRAME_MISSION:
+		default:
+			return 0;
+			break;
+		}
+	}
+	/**
+	 * set the relative altitude in meters from home
+	 */
+	void setRelAlt(float val) {
+		switch (getFrame()) {
+		case MAV_FRAME_GLOBAL:
+			setZ(val + home.getAlt());
+			break;
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+		case MAV_FRAME_LOCAL:
+			setZ(val);
+			break;
+		case MAV_FRAME_MISSION:
+			break;
+		}
+	}
+
+	float getRadius() const {
+		return getParam2();
+	}
+
+	void setRadius(float val) {
+		setParam2(val);
+	}
+
+	/**
+	 * conversion for outbound packets to ground station
+	 * @return output the mavlink_waypoint_t packet
+	 */
+	mavlink_waypoint_t convert(uint8_t current);
+
+	/**
+	 * Calculate the bearing from this command to the next command
+	 * @param next The command to calculate the bearing to.
+	 * @return the bearing
+	 */
+	float bearingTo(AP_MavlinkCommand next) const;
+
+	/**
+	 * Bearing form this command to a gps coordinate in integer units
+	 * @param latDegInt latitude in degrees E-7
+	 * @param lonDegInt longitude in degrees E-7
+	 * @return
+	 */
+	float bearingTo(int32_t latDegInt, int32_t lonDegInt) const;
+
+	/**
+	 * Distance to another command
+	 * @param next The command to measure to.
+	 * @return The distance in meters.
+	 */
+	float distanceTo(AP_MavlinkCommand next) const;
+
+	/**
+	 * Distance to a gps coordinate in integer units
+	 * @param latDegInt latitude in degrees E-7
+	 * @param lonDegInt longitude in degrees E-7
+	 * @return The distance in meters.
+	 */
+	float distanceTo(int32_t lat_degInt, int32_t lon_degInt) const;
+
+	float getPN(int32_t lat_degInt, int32_t lon_degInt) const {
+		// local tangent approximation at this waypoint
+		float deltaLat = (lat_degInt - getLat_degInt()) * degInt2Rad;
+		return deltaLat * rEarth;
+	}
+
+	float getPE(int32_t lat_degInt, int32_t lon_degInt) const {
+		// local tangent approximation at this waypoint
+		float deltaLon = (lon_degInt - getLon_degInt()) * degInt2Rad;
+		return cos(getLat()) * deltaLon * rEarth;
+	}
+
+	float getPD(int32_t alt_intM) const {
+		return -(alt_intM / scale_m - getAlt());
+	}
+
+	float getLat(float pN) const {
+
+		return pN / rEarth + getLat();
+	}
+
+	float getLon(float pE) const {
+
+		return pE / rEarth / cos(getLat()) + getLon();
+	}
+
+	/**
+	 * Gets altitude in meters
+	 * @param pD alt in meters
+	 * @return
+	 */
+	float getAlt(float pD) const {
+
+		return getAlt() - pD;
+	}
+
+	//calculates cross track of a current location
+	float crossTrack(AP_MavlinkCommand previous, int32_t lat_degInt, int32_t lon_degInt);
+
+	// calculates along  track distance of a current location
+	float alongTrack(AP_MavlinkCommand previous, int32_t lat_degInt, int32_t lon_degInt);
+};
+
+} // namespace apo
+
+
+#endif /* AP_MAVLINKCOMMAND_H_ */

libraries/APO/AP_Navigator.cpp

@@ -0,0 +1,8 @@
+/*
+ * AP_Navigator.cpp
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#include "AP_Navigator.h"

libraries/APO/AP_Navigator.h

@@ -0,0 +1,391 @@
+/*
+ * AP_Navigator.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_Navigator_H
+#define AP_Navigator_H
+
+#include "AP_HardwareAbstractionLayer.h"
+#include "../AP_DCM/AP_DCM.h"
+#include "../AP_Math/AP_Math.h"
+#include "../AP_Compass/AP_Compass.h"
+#include "AP_MavlinkCommand.h"
+#include "constants.h"
+#include "AP_Var_keys.h"
+#include "../AP_RangeFinder/AP_RangeFinder.h"
+#include "../AP_IMU/AP_IMU.h"
+
+namespace apo {
+
+/// Navigator class
+class AP_Navigator {
+public:
+	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),
+				_lon_degInt(0), _alt_intM(0) {
+	}
+	virtual void calibrate() {
+	}
+	virtual void updateFast(float dt) = 0;
+	virtual void updateSlow(float dt) = 0;
+	float getPD() const {
+		return AP_MavlinkCommand::home.getPD(getAlt_intM());
+	}
+
+	float getPE() const {
+		return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt());
+	}
+
+	float getPN() const {
+		return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt());
+	}
+
+	void setPD(float _pD) {
+		setAlt(AP_MavlinkCommand::home.getAlt(_pD));
+	}
+
+	void setPE(float _pE) {
+		setLat(AP_MavlinkCommand::home.getLat(_pE));
+	}
+
+	void setPN(float _pN) {
+		setLon(AP_MavlinkCommand::home.getLon(_pN));
+	}
+
+	float getAirSpeed() const {
+		return _airSpeed;
+	}
+
+	int32_t getAlt_intM() const {
+		return _alt_intM;
+	}
+
+	float getAlt() const {
+		return _alt_intM / scale_m;
+	}
+
+	void setAlt(float _alt) {
+		this->_alt_intM = _alt * scale_m;
+	}
+
+	float getLat() const {
+		//Serial.print("getLatfirst");
+		//Serial.println(_lat_degInt * degInt2Rad);
+		return _lat_degInt * degInt2Rad;
+	}
+
+	void setLat(float _lat) {
+		//Serial.print("setLatfirst");
+		//Serial.println(_lat * rad2DegInt);
+		setLat_degInt(_lat*rad2DegInt);
+	}
+
+	float getLon() const {
+		return _lon_degInt * degInt2Rad;
+	}
+
+	void setLon(float _lon) {
+		this->_lon_degInt = _lon * rad2DegInt;
+	}
+
+	float getVD() const {
+		return _vD;
+	}
+
+	float getVE() const {
+		return sin(getYaw()) * getGroundSpeed();
+	}
+
+	float getGroundSpeed() const {
+		return _groundSpeed;
+	}
+
+	int32_t getLat_degInt() const {
+		//Serial.print("getLat_degInt");
+		//Serial.println(_lat_degInt);
+		return _lat_degInt;
+
+	}
+
+	int32_t getLon_degInt() const {
+		return _lon_degInt;
+	}
+
+	float getVN() const {
+		return cos(getYaw()) * getGroundSpeed();
+	}
+
+	float getPitch() const {
+		return _pitch;
+	}
+
+	float getPitchRate() const {
+		return _pitchRate;
+	}
+
+	float getRoll() const {
+		return _roll;
+	}
+
+	float getRollRate() const {
+		return _rollRate;
+	}
+
+	float getYaw() const {
+		return _yaw;
+	}
+
+	float getYawRate() const {
+		return _yawRate;
+	}
+
+	void setAirSpeed(float airSpeed) {
+		_airSpeed = airSpeed;
+	}
+
+	void setAlt_intM(int32_t alt_intM) {
+		_alt_intM = alt_intM;
+	}
+
+	void setVD(float vD) {
+		_vD = vD;
+	}
+
+	void setGroundSpeed(float groundSpeed) {
+		_groundSpeed = groundSpeed;
+	}
+
+	void setLat_degInt(int32_t lat_degInt) {
+		_lat_degInt = lat_degInt;
+		//Serial.print("setLat_degInt");
+		//Serial.println(_lat_degInt);
+	}
+
+	void setLon_degInt(int32_t lon_degInt) {
+		_lon_degInt = lon_degInt;
+	}
+
+	void setPitch(float pitch) {
+		_pitch = pitch;
+	}
+
+	void setPitchRate(float pitchRate) {
+		_pitchRate = pitchRate;
+	}
+
+	void setRoll(float roll) {
+		_roll = roll;
+	}
+
+	void setRollRate(float rollRate) {
+		_rollRate = rollRate;
+	}
+
+	void setYaw(float yaw) {
+		_yaw = yaw;
+	}
+
+	void setYawRate(float yawRate) {
+		_yawRate = yawRate;
+	}
+	void setTimeStamp(int32_t timeStamp) {
+		_timeStamp = timeStamp;
+	}
+	int32_t getTimeStamp() const {
+		return _timeStamp;
+	}
+
+protected:
+	AP_HardwareAbstractionLayer * _hal;
+private:
+	int32_t _timeStamp; // micros clock
+	float _roll; // rad
+	float _rollRate; //rad/s
+	float _pitch; // rad
+	float _pitchRate; // rad/s
+	float _yaw; // rad
+	float _yawRate; // rad/s
+	float _airSpeed; // m/s
+	float _groundSpeed; // m/s
+	float _vD; // m/s
+	int32_t _lat_degInt; // deg / 1e7
+	int32_t _lon_degInt; // deg / 1e7
+	int32_t _alt_intM; // meters / 1e3
+};
+
+class DcmNavigator: public AP_Navigator {
+private:
+	/**
+	 * Sensors
+	 */
+
+	RangeFinder * _rangeFinderDown;
+	AP_DCM * _dcm;
+	IMU * _imu;
+	uint16_t _imuOffsetAddress;
+
+public:
+	DcmNavigator(AP_HardwareAbstractionLayer * hal) :
+		AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) {
+
+		// if orientation equal to front, store as front
+		/**
+		 * rangeFinder<direction> is assigned values based on orientation which
+		 * is specified in ArduPilotOne.pde.
+		 */
+		for (uint8_t i = 0; i < _hal-> rangeFinders.getSize(); i++) {
+			if (_hal->rangeFinders[i] == NULL)
+				continue;
+			if (_hal->rangeFinders[i]->orientation_x == 0
+					&& _hal->rangeFinders[i]->orientation_y == 0
+					&& _hal->rangeFinders[i]->orientation_z == 1)
+				_rangeFinderDown = _hal->rangeFinders[i];
+		}
+
+		if (_hal->getMode() == MODE_LIVE) {
+			if (_hal->adc)
+				_hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib);
+			if (_hal->imu)
+				_dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass);
+			if (_hal->compass) {
+				_dcm->set_compass(_hal->compass);
+
+			}
+		}
+	}
+	virtual void calibrate() {
+
+		AP_Navigator::calibrate();
+
+		// TODO: handle cold restart
+		if (_hal->imu) {
+			/*
+			 * Gyro has built in warm up cycle and should
+			 * run first */
+			_hal->imu->init_gyro(NULL);
+			_hal->imu->init_accel(NULL);
+		}
+	}
+	virtual void updateFast(float dt) {
+		if (_hal->getMode() != MODE_LIVE)
+			return;
+
+		setTimeStamp(micros()); // if running in live mode, record new time stamp
+
+
+		//_hal->debug->println_P(PSTR("nav loop"));
+
+		/**
+		 * The altitued is read off the barometer by implementing the following formula:
+		 * altitude (in m) = 44330*(1-(p/po)^(1/5.255)),
+		 * where, po is pressure in Pa at sea level (101325 Pa).
+		 * See http://www.sparkfun.com/tutorials/253 or type this formula
+		 * in a search engine for more information.
+		 * altInt contains the altitude in meters.
+		 */
+		if (_hal->baro) {
+
+			if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695)
+				setAlt(_rangeFinderDown->distance);
+
+			else {
+				float tmp = (_hal->baro->Press / 101325.0);
+				tmp = pow(tmp, 0.190295);
+				//setAlt(44330 * (1.0 - tmp)); //sets the altitude in meters XXX wrong, baro reads 0 press
+				setAlt(0.0);
+			}
+		}
+
+		// dcm class for attitude
+		if (_dcm) {
+			_dcm->update_DCM();
+			setRoll(_dcm->roll);
+			setPitch(_dcm->pitch);
+			setYaw(_dcm->yaw);
+			setRollRate(_dcm->get_gyro().x);
+			setPitchRate(_dcm->get_gyro().y);
+			setYawRate(_dcm->get_gyro().z);
+
+			/*
+			 * 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);
+			 */
+		}
+	}
+	virtual void updateSlow(float dt) {
+		if (_hal->getMode() != MODE_LIVE)
+			return;
+
+		setTimeStamp(micros()); // if running in live mode, record new time stamp
+
+		if (_hal->gps) {
+			_hal->gps->update();
+			updateGpsLight();
+			if (_hal->gps->fix && _hal->gps->new_data) {
+				setLat_degInt(_hal->gps->latitude);
+				setLon_degInt(_hal->gps->longitude);
+				setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm
+				setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s
+			}
+		}
+
+		if (_hal->compass) {
+			_hal->compass->read();
+			_hal->compass->calculate(getRoll(), getPitch());
+			_hal->compass->null_offsets(_dcm->get_dcm_matrix());
+		}
+	}
+	void updateGpsLight(void) {
+		// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
+		// ---------------------------------------------------------------------
+		static bool GPS_light = false;
+		switch (_hal->gps->status()) {
+		case (2):
+			//digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
+			break;
+
+		case (1):
+			if (_hal->gps->valid_read == true) {
+				GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
+				if (GPS_light) {
+					digitalWrite(_hal->cLedPin, LOW);
+				} else {
+					digitalWrite(_hal->cLedPin, HIGH);
+				}
+				_hal->gps->valid_read = false;
+			}
+			break;
+
+		default:
+			digitalWrite(_hal->cLedPin, LOW);
+			break;
+		}
+	}
+
+};
+
+} // namespace apo
+
+#endif // AP_Navigator_H
+// vim:ts=4:sw=4:expandtab

libraries/APO/AP_RcChannel.cpp

@@ -0,0 +1,110 @@
+/*
+ AP_RcChannel.cpp - Radio library for Arduino
+ Code by Jason Short, James Goppert. DIYDrones.com
+
+ This library is free software; you can redistribute it and / or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+
+ */
+
+#include <math.h>
+#include <avr/eeprom.h>
+#include "AP_RcChannel.h"
+#include "../AP_Common/AP_Common.h"
+#include <HardwareSerial.h>
+
+namespace apo {
+
+AP_RcChannel::AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name,
+		APM_RC_Class & rc, const uint8_t & ch, const uint16_t & pwmMin,
+		const uint16_t & pwmNeutral, const uint16_t & pwmMax,
+		const rcMode_t & rcMode, const bool & reverse) :
+	AP_Var_group(keyValue, name), _ch(this, 1, ch, PSTR("ch")),
+			_pwmMin(this, 2, pwmMin, PSTR("pMin")),
+			_pwmNeutral(this, 3, pwmNeutral, PSTR("pNtrl")),
+			_pwmMax(this, 4, pwmMax, PSTR("pMax")), _rcMode(rcMode),
+			_reverse(this, 5, reverse, PSTR("rev")), _rc(rc), _pwm(pwmNeutral) {
+	//Serial.print("pwm after ctor: "); Serial.println(pwmNeutral);
+	if (rcMode == RC_MODE_IN)
+		return;
+	//Serial.print("pwm set for ch: "); Serial.println(int(ch));
+	rc.OutputCh(ch, pwmNeutral);
+
+}
+
+uint16_t AP_RcChannel::getRadioPwm() {
+	if (_rcMode == RC_MODE_OUT) {
+		Serial.print("tryng to read from output channel: ");
+		Serial.println(int(_ch));
+		return _pwmNeutral; // if this happens give a safe value of neutral
+	}
+	return _rc.InputCh(_ch);
+}
+
+void AP_RcChannel::setUsingRadio() {
+	setPwm(getRadioPwm());
+}
+
+void AP_RcChannel::setPwm(uint16_t pwm) {
+	//Serial.printf("pwm in setPwm: %d\n", pwm);
+	//Serial.printf("reverse: %s\n", (reverse)?"true":"false");
+
+	// apply reverse
+	if (_reverse)
+		pwm = int16_t(_pwmNeutral - pwm) + _pwmNeutral;
+	//Serial.printf("pwm after reverse: %d\n", pwm);
+
+	// apply saturation
+	if (_pwm > uint8_t(_pwmMax))
+		_pwm = _pwmMax;
+	if (_pwm < uint8_t(_pwmMin))
+		_pwm = _pwmMin;
+	_pwm = pwm;
+
+	//Serial.print("ch: "); Serial.print(ch); Serial.print(" pwm: "); Serial.println(pwm);
+	if (_rcMode == RC_MODE_IN)
+		return;
+	_rc.OutputCh(_ch, _pwm);
+}
+
+void AP_RcChannel::setPosition(float position) {
+	if (position > 1.0)
+		position = 1.0;
+	else if (position < -1.0)
+		position = -1.0;
+	setPwm(_positionToPwm(position));
+}
+
+uint16_t AP_RcChannel::_positionToPwm(const float & position) {
+	uint16_t pwm;
+	//Serial.printf("position: %f\n", position);
+	if (position < 0)
+		pwm = position * int16_t(_pwmNeutral - _pwmMin) + _pwmNeutral;
+	else
+		pwm = position * int16_t(_pwmMax - _pwmNeutral) + _pwmNeutral;
+
+	if (pwm > uint16_t(_pwmMax))
+		pwm = _pwmMax;
+	if (pwm < uint16_t(_pwmMin))
+		pwm = _pwmMin;
+	return pwm;
+}
+
+float AP_RcChannel::_pwmToPosition(const uint16_t & pwm) {
+	float position;
+	if (pwm < uint8_t(_pwmNeutral))
+		position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t(
+				_pwmNeutral - _pwmMin);
+	else
+		position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t(
+				_pwmMax - _pwmNeutral);
+	if (position > 1)
+		position = 1;
+	if (position < -1)
+		position = -1;
+	return position;
+}
+
+} // namespace apo

libraries/APO/AP_RcChannel.h

@@ -0,0 +1,69 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AP_RcChannel.h
+/// @brief	AP_RcChannel manager
+
+#ifndef AP_RCCHANNEL_H
+#define AP_RCCHANNEL_H
+
+#include <stdint.h>
+#include "../APM_RC/APM_RC.h"
+#include "../AP_Common/AP_Common.h"
+#include "../AP_Common/AP_Var.h"
+
+namespace apo {
+
+enum rcMode_t {
+	RC_MODE_IN, RC_MODE_OUT, RC_MODE_INOUT
+};
+
+/// @class	AP_RcChannel
+/// @brief	Object managing one RC channel
+class AP_RcChannel: public AP_Var_group {
+
+public:
+
+	/// Constructor
+	AP_RcChannel(AP_Var::Key key, const prog_char_t * name, APM_RC_Class & rc,
+			const uint8_t & ch, const uint16_t & pwmMin,
+			const uint16_t & pwmNeutral, const uint16_t & pwmMax,
+			const rcMode_t & rcMode = RC_MODE_INOUT,
+			const bool & reverse = false);
+
+	// configuration
+	AP_Uint8 _ch;AP_Uint16 _pwmMin;AP_Uint16 _pwmNeutral;AP_Uint16 _pwmMax;
+	rcMode_t _rcMode;AP_Bool _reverse;
+
+	// set
+	void setUsingRadio();
+	void setPwm(uint16_t pwm);
+	void setPosition(float position);
+
+	// get
+	uint16_t getPwm() {
+		return _pwm;
+	}
+	uint16_t getRadioPwm();
+	float getPosition() {
+		return _pwmToPosition(_pwm);
+	}
+	float getRadioPosition() {
+		return _pwmToPosition(getRadioPwm());
+	}
+
+private:
+
+	// configuration
+	APM_RC_Class & _rc;
+
+	// internal states
+	uint16_t _pwm; // this is the internal state, position is just created when needed
+
+	// private methods
+	uint16_t _positionToPwm(const float & position);
+	float _pwmToPosition(const uint16_t & pwm);
+};
+
+} // apo
+
+#endif	// AP_RCCHANNEL_H

libraries/APO/AP_Var_keys.h

@@ -0,0 +1,24 @@
+#ifndef AP_Var_keys_H
+#define AP_Var_keys_H
+
+enum keys {
+
+	// general
+	k_config = 0,
+	k_cntrl,
+	k_guide,
+	k_sensorCalib,
+
+	k_radioChannelsStart=10,
+
+	k_controllersStart=30,
+
+	k_customStart=100,
+
+	// 200-256 reserved for commands
+	k_commands = 200
+};
+
+// max 256 keys
+
+#endif

libraries/APO/CMakeLists.txt

@@ -0,0 +1 @@
+include(ApmMakeLib)

libraries/APO/constants.h

@@ -0,0 +1,23 @@
+/*
+ * constants.h
+ *
+ *  Created on: Apr 7, 2011
+ *      Author: nkgentry
+ */
+
+#ifndef CONSTANTS_H_
+#define CONSTANTS_H_
+
+#include "math.h"
+
+const float scale_deg = 1e7; // scale of integer degrees/ degree
+const float scale_m = 1e3; // scale of integer meters/ meter
+const float rEarth = 6371000; // radius of earth, meters
+const float rad2Deg = 180 / M_PI; // radians to degrees
+const float deg2Rad = M_PI / 180; // degrees to radians
+const float rad2DegInt = rad2Deg * scale_deg; // radians to degrees x 1e7
+const float degInt2Rad = deg2Rad / scale_deg; // degrees x 1e7 to radians
+
+#define MAV_MODE_FAILSAFE MAV_MODE_TEST3
+
+#endif /* CONSTANTS_H_ */

libraries/APO/examples/MavlinkTest/Makefile

@@ -0,0 +1,2 @@
+BOARD	=	mega
+include ../../../AP_Common/Arduino.mk

libraries/APO/examples/MavlinkTest/MavlinkTest.pde

@@ -0,0 +1,49 @@
+/*
+ AnalogReadSerial
+ Reads an analog input on pin 0, prints the result to the serial monitor 
+ 
+ This example code is in the public domain.
+ */
+
+#include <GCS_MAVLink.h>
+
+int packetDrops = 0;
+
+void handleMessage(mavlink_message_t * msg) {
+	Serial.print(", received mavlink message: ");
+	Serial.print(msg->msgid, DEC);
+}
+
+void setup() {
+	Serial.begin(57600);
+	Serial3.begin(57600);
+	mavlink_comm_0_port = &Serial3;
+	packetDrops = 0;
+}
+
+void loop() {
+	mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type,
+			MAV_AUTOPILOT_ARDUPILOTMEGA);
+	Serial.print("heartbeat sent");
+
+	// receive new packets
+	mavlink_message_t msg;
+	mavlink_status_t status;
+
+	Serial.print(", bytes available: ");
+	Serial.print(comm_get_available(MAVLINK_COMM_0));
+	while (comm_get_available( MAVLINK_COMM_0)) {
+		uint8_t c = comm_receive_ch(MAVLINK_COMM_0);
+
+		// Try to get a new message
+		if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
+			handleMessage(&msg);
+	}
+
+	// Update packet drops counter
+	packetDrops += status.packet_rx_drop_count;
+
+	Serial.print(", dropped packets: ");
+	Serial.println(packetDrops);
+	delay(1000);
+}

libraries/APO/examples/ServoManual/Makefile

@@ -0,0 +1,2 @@
+BOARD	=	mega
+include ../../../AP_Common/Arduino.mk

libraries/APO/examples/ServoManual/ServoManual.pde

@@ -0,0 +1,109 @@
+/*
+ Example of RC_Channel library.
+ Code by James Goppert/ Jason Short. 2010
+ DIYDrones.com
+
+ */
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <APO.h>
+#include <AP_RcChannel.h> 	// ArduPilot Mega RC Library
+#include <APM_RC.h>
+#include <AP_Vector.h>
+FastSerialPort0(Serial)
+; // make sure this proceeds variable declarations
+
+// test settings
+
+using namespace apo;
+
+class RadioTest {
+private:
+	float testPosition;
+	int8_t testSign;
+	enum {
+		version,
+		rollKey,
+		pitchKey,
+		thrKey,
+		yawKey,
+		ch5Key,
+		ch6Key,
+		ch7Key,
+		ch8Key
+	};
+	Vector<AP_RcChannel *> ch;
+public:
+	RadioTest() :
+		testPosition(2), testSign(1) {
+		ch.push_back(
+				new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100,
+						1500, 1900));
+		ch.push_back(
+				new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500,
+						1900));
+
+		Serial.begin(115200);
+		delay(2000);
+		Serial.println("ArduPilot RC Channel test");
+		APM_RC.Init(); // APM Radio initialization
+		delay(2000);
+	}
+
+	void update() {
+		// read the radio 
+		for (uint8_t i = 0; i < ch.getSize(); i++)
+			ch[i]->setUsingRadio();
+
+		// print channel names
+		Serial.print("\t\t");
+		static char name[7];
+		for (uint8_t i = 0; i < ch.getSize(); i++) {
+			ch[i]->copy_name(name, 7);
+			Serial.printf("%7s\t", name);
+		}
+		Serial.println();
+
+		// print pwm
+		Serial.printf("pwm      :\t");
+		for (uint8_t i = 0; i < ch.getSize(); i++)
+			Serial.printf("%7d\t", ch[i]->getPwm());
+		Serial.println();
+
+		// print position
+		Serial.printf("position :\t");
+		for (uint8_t i = 0; i < ch.getSize(); i++)
+			Serial.printf("%7.2f\t", ch[i]->getPosition());
+		Serial.println();
+
+		delay(500);
+	}
+};
+
+RadioTest * test;
+
+void setup() {
+	test = new RadioTest;
+}
+
+void loop() {
+	test->update();
+}

libraries/APO/examples/ServoSweep/Makefile

@@ -0,0 +1,2 @@
+BOARD	=	mega
+include ../../../AP_Common/Arduino.mk

libraries/APO/examples/ServoSweep/ServoSweep.pde

@@ -0,0 +1,125 @@
+/*
+ Example of RC_Channel library.
+ Code by James Goppert/ Jason Short. 2010
+ DIYDrones.com
+
+ */
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <APO.h>
+#include <AP_RcChannel.h>   // ArduPilot Mega RC Library
+#include <APM_RC.h>
+#include <AP_Vector.h>
+FastSerialPort0(Serial)
+; // make sure this proceeds variable declarations
+
+// test settings
+
+using namespace apo;
+
+class RadioTest {
+private:
+	float testPosition;
+	int8_t testSign;
+	enum {
+		version,
+		rollKey,
+		pitchKey,
+		thrKey,
+		yawKey,
+		ch5Key,
+		ch6Key,
+		ch7Key,
+		ch8Key
+	};
+	Vector<AP_RcChannel *> ch;
+public:
+	RadioTest() :
+		testPosition(2), testSign(1) {
+		ch.push_back(
+				new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100,
+						1500, 1900));
+		ch.push_back(
+				new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500,
+						1900));
+		ch.push_back(
+				new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500,
+						1900));
+
+		Serial.begin(115200);
+		delay(2000);
+		Serial.println("ArduPilot RC Channel test");
+		APM_RC.Init(); // APM Radio initialization
+		delay(2000);
+	}
+
+	void update() {
+		// update test value
+		testPosition += testSign * .1;
+		if (testPosition > 1) {
+			//eepromRegistry.print(Serial); // show eeprom map
+			testPosition = 1;
+			testSign = -1;
+		} else if (testPosition < -1) {
+			testPosition = -1;
+			testSign = 1;
+		}
+
+		// set channel positions
+		for (uint8_t i = 0; i < ch.getSize(); i++)
+			ch[i]->setPosition(testPosition);
+
+		// print test position
+		Serial.printf("\nnormalized position (%f)\n", testPosition);
+
+		// print channel names
+		Serial.print("\t\t");
+		static char name[7];
+		for (uint8_t i = 0; i < ch.getSize(); i++) {
+			ch[i]->copy_name(name, 7);
+			Serial.printf("%7s\t", name);
+		}
+		Serial.println();
+
+		// print pwm
+		Serial.printf("pwm      :\t");
+		for (uint8_t i = 0; i < ch.getSize(); i++)
+			Serial.printf("%7d\t", ch[i]->getRadioPwm());
+		Serial.println();
+
+		// print position
+		Serial.printf("position :\t");
+		for (uint8_t i = 0; i < ch.getSize(); i++)
+			Serial.printf("%7.2f\t", ch[i]->getRadioPosition());
+		Serial.println();
+
+		delay(500);
+	}
+};
+
+RadioTest * test;
+
+void setup() {
+	test = new RadioTest;
+}
+
+void loop() {
+	test->update();
+}
+
+// vim:ts=4:sw=4:expandtab

libraries/APO/template.h

@@ -0,0 +1,32 @@
+/*
+ * Class.h
+ * Copyright (C) Author 2011 <email>
+ *
+ * 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 Class_H
+#define Class_H
+
+namespace apo {
+
+/// Class desciprtion
+class Class {
+public:
+}
+
+} // namespace apo
+
+#endif // Class_H
+// vim:ts=4:sw=4:expandtab

libraries/AP_ADC/CMakeLists.txt

@@ -1,26 +1 @@
-set(LIB_NAME AP_ADC)
+include(ApmMakeLib)
-	
-set(${LIB_NAME}_SRCS 
-		AP_ADC_HIL.cpp
-		AP_ADC_ADS7844.cpp
-		AP_ADC.cpp
-	)  # Firmware sources
-
-set(${LIB_NAME}_HDRS  		
-		AP_ADC_HIL.h
-		AP_ADC_ADS7844.h
-		AP_ADC.h
-	)
-	
-include_directories(
- 
-  -
-  #${CMAKE_SOURCE_DIR}/libraries/AP_Math
-  #${CMAKE_SOURCE_DIR}/libraries/AP_Common
-	#${CMAKE_SOURCE_DIR}/libraries/FastSerial
-#	
-	)
-
-set(${LIB_NAME}_BOARD mega2560)
-
-generate_arduino_library(${LIB_NAME})

libraries/AP_Common/AP_Var.cpp

@@ -9,19 +9,21 @@
 /// @file   AP_Var.cpp
 /// @brief  The AP variable store.
 
-#if 0
-# include <FastSerial.h>
-extern "C" { extern void delay(unsigned long); }
-# define debug(fmt, args...)  do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(100); } while(0)
-#else
-# define debug(fmt, args...)
-#endif
 
 #include <AP_Common.h>
 
 #include <math.h>
 #include <string.h>
 
+//#define ENABLE_FASTSERIAL_DEBUG
+
+#ifdef ENABLE_FASTSERIAL_DEBUG
+# include <FastSerial.h>
+# define serialDebug(fmt, args...)  if (FastSerial::getInitialized(0)) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0)
+#else
+# define serialDebug(fmt, args...)
+#endif
+
 // Global constants exported for general use.
 //
 AP_Float AP_Float_unity         ( 1.0, AP_Var::k_key_none, NULL, AP_Var::k_flag_unlisted);
@@ -71,7 +73,11 @@ AP_Var::AP_Var(AP_Var_group *pGroup, Key index, const prog_char_t *name, Flags f
     // to the pointer to the node that we are considering inserting in front of.
     //
     vp = &_grouped_variables;
+    size_t loopCount = 0;
     while (*vp != NULL) {
+
+    	if (loopCount++>k_num_max) return;
+
         if ((*vp)->_key >= _key) {
                break;
         }
@@ -98,13 +104,20 @@ AP_Var::~AP_Var(void)
     }
 
     // Scan the list and remove this if we find it
+
+    {
+    	size_t loopCount = 0;
 		while (*vp) {
+
+			if (loopCount++>k_num_max) return;
+
 			if (*vp == this) {
 				*vp = _link;
 				break;
 			}
 			vp = &((*vp)->_link);
 		}
+    }
 
     // If we are destroying a group, remove all its variables from the list
     //
@@ -112,8 +125,12 @@ AP_Var::~AP_Var(void)
 
         // Scan the list and remove any variable that has this as its group
         vp = &_grouped_variables;
+        size_t loopCount = 0;
+
         while (*vp) {
 
+        	if (loopCount++>k_num_max) return;
+
             // Does the variable claim us as its group?
             if ((*vp)->_group == this) {
                 *vp = (*vp)->_link;
@@ -145,7 +162,12 @@ AP_Var::find(const char *name)
 {
     AP_Var  *vp;
 
+    size_t loopCount = 0;
+
     for (vp = first(); vp; vp = vp->next()) {
+
+    	if (loopCount++>k_num_max) return NULL;
+
         char    name_buffer[32];
 
         // copy the variable's name into our scratch buffer
@@ -165,9 +187,9 @@ AP_Var *
 AP_Var::find(Key key)
 {
     AP_Var  *vp;
-
+    size_t loopCount = 0;
     for (vp = first(); vp; vp = vp->next()) {
-
+    	if (loopCount++>k_num_max) return NULL;
         if (key == vp->key()) {
             return vp;
         }
@@ -188,11 +210,11 @@ bool AP_Var::save(void)
         return _group->save();
     }
 
-    debug("save: %S", _name ? _name : PSTR("??"));
+    serialDebug("save: %S", _name ? _name : PSTR("??"));
 
     // locate the variable in EEPROM, allocating space as required
     if (!_EEPROM_locate(true)) {
-        debug("locate failed");
+        serialDebug("locate failed");
         return false;
     }
 
@@ -200,13 +222,13 @@ bool AP_Var::save(void)
     size = serialize(vbuf, sizeof(vbuf));
     if (0 == size) {
         // variable cannot be serialised into the buffer
-        debug("cannot save (too big or not supported)");
+        serialDebug("cannot save (too big or not supported)");
         return false;
     }
 
     // if it fit in the buffer, save it to EEPROM
     if (size <= sizeof(vbuf)) {
-        debug("saving %u to %u", size, _key);
+        serialDebug("saving %u to %u", size, _key);
         // XXX this should use eeprom_update_block if/when Arduino moves to
         // avr-libc >= 1.7
         uint8_t *ep = (uint8_t *)_key;
@@ -219,7 +241,7 @@ bool AP_Var::save(void)
             // now read it back
             newv = eeprom_read_byte(ep);
             if (newv != vbuf[i]) {
-                debug("readback failed at offset %p: got %u, expected %u",
+                serialDebug("readback failed at offset %p: got %u, expected %u",
                       ep, newv, vbuf[i]);
                 return false;
             }
@@ -241,11 +263,11 @@ bool AP_Var::load(void)
         return _group->load();
     }
 
-    debug("load: %S", _name ? _name : PSTR("??"));
+    serialDebug("load: %S", _name ? _name : PSTR("??"));
 
     // locate the variable in EEPROM, but do not allocate space
     if (!_EEPROM_locate(false)) {
-        debug("locate failed");
+        serialDebug("locate failed");
         return false;
     }
 
@@ -255,7 +277,7 @@ bool AP_Var::load(void)
     //
     size = serialize(NULL, 0);
     if (0 == size) {
-        debug("cannot load (too big or not supported)");
+        serialDebug("cannot load (too big or not supported)");
         return false;
     }
 
@@ -263,7 +285,7 @@ bool AP_Var::load(void)
     // has converted _key into an EEPROM address.
     //
     if (size <= sizeof(vbuf)) {
-        debug("loading %u from %u", size, _key);
+        serialDebug("loading %u from %u", size, _key);
         eeprom_read_block(vbuf, (void *)_key, size);
         return unserialize(vbuf, size);
     }
@@ -278,7 +300,12 @@ bool AP_Var::save_all(void)
     bool result = true;
     AP_Var *vp = _variables;
 
+    size_t loopCount = 0;
+
     while (vp) {
+
+    	if (loopCount++>k_num_max) return false;
+
         if (!vp->has_flags(k_flag_no_auto_load) &&  // not opted out of autosave
             (vp->_key != k_key_none)) {              // has a key
 
@@ -298,7 +325,12 @@ bool AP_Var::load_all(void)
     bool result = true;
     AP_Var *vp = _variables;
 
+    size_t loopCount = 0;
+
     while (vp) {
+
+    	if (loopCount++>k_num_max) return false;
+
         if (!vp->has_flags(k_flag_no_auto_load) &&  // not opted out of autoload
             (vp->_key != k_key_none)) {             // has a key
 
@@ -322,13 +354,19 @@ AP_Var::erase_all()
     AP_Var  *vp;
     uint16_t i;
 
-    debug("erase EEPROM");
+    serialDebug("erase EEPROM");
 
     // Scan the list of variables/groups, fetching their key values and
     // reverting them to their not-located state.
     //
     vp = _variables;
+
+    size_t loopCount = 0;
+
     while (vp) {
+
+    	if (loopCount++>k_num_max) return;
+
         vp->_key = vp->key() | k_key_not_located;
         vp = vp->_link;
     }
@@ -405,9 +443,15 @@ AP_Var::first_member(AP_Var_group *group)
 
     vp = &_grouped_variables;
 
-    debug("seeking %p", group);
+    serialDebug("seeking %p", group);
+
+    size_t loopCount = 0;
+
     while (*vp) {
-        debug("consider %p with %p", *vp, (*vp)->_group);
+
+    	if (loopCount++>k_num_max) return NULL;
+
+        serialDebug("consider %p with %p", *vp, (*vp)->_group);
         if ((*vp)->_group == group) {
             return *vp;
         }
@@ -423,7 +467,12 @@ AP_Var::next_member()
     AP_Var  *vp;
 
     vp = _link;
+    size_t loopCount = 0;
+
     while (vp) {
+
+    	if (loopCount++>k_num_max) return NULL;
+
         if (vp->_group == _group) {
             return vp;
         }
@@ -451,7 +500,7 @@ bool AP_Var::_EEPROM_scan(void)
     if ((ee_header.magic != k_EEPROM_magic) ||
         (ee_header.revision != k_EEPROM_revision)) {
 
-        debug("no header, magic 0x%x revision %u", ee_header.magic, ee_header.revision);
+        serialDebug("no header, magic 0x%x revision %u", ee_header.magic, ee_header.revision);
         return false;
     }
 
@@ -460,17 +509,22 @@ bool AP_Var::_EEPROM_scan(void)
     // Avoid trying to read a header when there isn't enough space left.
     //
     eeprom_address = sizeof(ee_header);
+
+    size_t loopCount = 0;
+
     while (eeprom_address < (k_EEPROM_size - sizeof(var_header) - 1)) {
 
+    	if (loopCount++>k_num_max) return NULL;
+
         // Read a variable header
         //
-        debug("reading header from %u", eeprom_address);
+        serialDebug("reading header from %u", eeprom_address);
         eeprom_read_block(&var_header, (void *)eeprom_address, sizeof(var_header));
 
         // If the header is for the sentinel, scanning is complete
         //
         if (var_header.key == k_key_sentinel) {
-            debug("found tail sentinel");
+            serialDebug("found tail sentinel");
             break;
         }
 
@@ -482,23 +536,25 @@ bool AP_Var::_EEPROM_scan(void)
                 var_header.size + 1 +   // data for this variable
                 sizeof(var_header))) {  // header for sentinel
 
-            debug("header overruns EEPROM");
+            serialDebug("header overruns EEPROM");
             return false;
         }
 
         // look for a variable with this key
         vp = _variables;
-        while (vp) {
+        size_t loopCount2 = 0;
+        while(vp) {
+        	if (loopCount2++>k_num_max) return false;
             if (vp->key() == var_header.key) {
                 // adjust the variable's key to point to this entry
                 vp->_key = eeprom_address + sizeof(var_header);
-                debug("update %p with key %u -> %u", vp, var_header.key, vp->_key);
+                serialDebug("update %p with key %u -> %u", vp, var_header.key, vp->_key);
                 break;
             }
             vp = vp->_link;
         }
         if (!vp) {
-            debug("key %u not claimed (already scanned or unknown)", var_header.key);
+            serialDebug("key %u not claimed (already scanned or unknown)", var_header.key);
         }
 
         // move to the next variable header
@@ -514,16 +570,18 @@ bool AP_Var::_EEPROM_scan(void)
     // mark all the rest as not allocated.
     //
     vp = _variables;
-    while (vp) {
+    size_t loopCount3 = 0;
+    while(vp) {
+    	if (loopCount3++>k_num_max) return false;
         if (vp->_key & k_key_not_located) {
             vp->_key |= k_key_not_allocated;
-            debug("key %u not allocated", vp->key());
+            serialDebug("key %u not allocated", vp->key());
         }
         vp = vp->_link;
     }
 
     // Scanning is complete
-    debug("scan done");
+    serialDebug("scan done");
     _tail_sentinel = eeprom_address;
     return true;
 }
@@ -539,7 +597,7 @@ bool AP_Var::_EEPROM_locate(bool allocate)
     // Is it a group member, or does it have a no-location key?
     //
     if (_group || (_key == k_key_none)) {
-        debug("not addressable");
+        serialDebug("not addressable");
         return false;               // it is/does, and thus it has no location
     }
 
@@ -554,7 +612,7 @@ bool AP_Var::_EEPROM_locate(bool allocate)
     // try scanning to see if we can locate it.
     //
     if (!(_key & k_key_not_allocated)) {
-        debug("need scan");
+        serialDebug("need scan");
         _EEPROM_scan();
 
         // Has the variable now been located?
@@ -569,7 +627,7 @@ bool AP_Var::_EEPROM_locate(bool allocate)
     if (!allocate) {
         return false;
     }
-    debug("needs allocation");
+    serialDebug("needs allocation");
 
     // Ask the serializer for the size of the thing we are allocating, and fail
     // if it is too large or if it has no size, as we will not be able to allocate
@@ -577,7 +635,7 @@ bool AP_Var::_EEPROM_locate(bool allocate)
     //
     size = serialize(NULL, 0);
     if ((size == 0) || (size > k_size_max)) {
-        debug("size %u out of bounds", size);
+        serialDebug("size %u out of bounds", size);
         return false;
     }
 
@@ -585,7 +643,7 @@ bool AP_Var::_EEPROM_locate(bool allocate)
     // header and the new tail sentinel.
     //
     if ((_tail_sentinel + size + sizeof(Var_header) * 2) > k_EEPROM_size) {
-        debug("no space in EEPROM");
+        serialDebug("no space in EEPROM");
         return false;
     }
 
@@ -595,7 +653,7 @@ bool AP_Var::_EEPROM_locate(bool allocate)
     if (0 == _tail_sentinel) {
         uint8_t pad_size;
 
-        debug("writing header");
+        serialDebug("writing header");
         EEPROM_header   ee_header;
 
         ee_header.magic = k_EEPROM_magic;
@@ -621,7 +679,7 @@ bool AP_Var::_EEPROM_locate(bool allocate)
     //
     new_location = _tail_sentinel;
     _tail_sentinel += sizeof(var_header) + size;
-    debug("allocated %u/%u for key %u new sentinel %u", new_location, size, key(), _tail_sentinel);
+    serialDebug("allocated %u/%u for key %u new sentinel %u", new_location, size, key(), _tail_sentinel);
 
     // Write the new sentinel first.  If we are interrupted during this operation
     // the old sentinel will still correctly terminate the EEPROM image.
@@ -670,17 +728,22 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali
     // Traverse the list of group members, serializing each in order
     //
     vp = first_member(this);
-    debug("starting with %p", vp);
+    serialDebug("starting with %p", vp);
     total_size = 0;
+
+    size_t loopCount = 0;
+
     while (vp) {
 
+    	if (loopCount++>k_num_max) return false;
+
         // (un)serialise the group member
         if (do_serialize) {
             size = vp->serialize(buf, buf_size);
-            debug("serialize %p -> %u", vp, size);
+            serialDebug("serialize %p -> %u", vp, size);
         } else {
             size = vp->unserialize(buf, buf_size);
-            debug("unserialize %p -> %u", vp, size);
+            serialDebug("unserialize %p -> %u", vp, size);
         }
 
         // Unserialize will return zero if the buffer is too small
@@ -688,7 +751,7 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali
         // Either case is fatal for any operation we might be trying.
         //
         if (0 == size) {
-            debug("group (un)serialize failed, buffer too small or not supported");
+            serialDebug("group (un)serialize failed, buffer too small or not supported");
             return 0;
         }
 
@@ -704,7 +767,7 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali
         // and the calling function will have to treat it as an error.
         //
         total_size += size;
-        debug("used %u", total_size);
+        serialDebug("used %u", total_size);
         if (size <= buf_size) {
             // there was space for this one, account for it
             buf_size -= size;

libraries/AP_Common/AP_Var.h

@@ -128,6 +128,10 @@ public:
     ///
     static const size_t k_size_max = 64;
 
+    /// The maximum number of keys
+    ///
+    static const size_t k_num_max = 255;
+
     /// Optional flags affecting the behavior and usage of the variable.
     ///
     typedef uint8_t Flags;

libraries/AP_Common/Arduino.mk

@@ -305,7 +305,7 @@ LIBOBJS			:=	$(SKETCHLIBOBJS) $(ARDUINOLIBOBJS)
 # Pull the Arduino version from the revisions.txt file
 #
 # XXX can we count on this?  If not, what?
-ARDUINO_VERS		:=	$(shell expr `head -1 $(ARDUINO)/revisions.txt | cut -d ' ' -f 2`)
+ARDUINO_VERS		:=	$(shell expr `head -1 $(ARDUINO)/lib/version.txt | cut -d ' ' -f 2`)
 
 # Find the hardware directory to use
 HARDWARE_DIR		:=	$(firstword $(wildcard $(SKETCHBOOK)/hardware/$(HARDWARE) \

libraries/AP_Common/CMakeLists.txt

@@ -1,33 +1 @@
-set(LIB_NAME AP_Common)
+include(ApmMakeLib)
-	
-set(${LIB_NAME}_SRCS 
-        AP_Common.cpp
-        AP_Loop.cpp
-        AP_MetaClass.cpp
-        AP_Var.cpp
-        AP_Var_menufuncs.cpp
-        c++.cpp
-        menu.cpp
-	)  # Firmware sources
-
-set(${LIB_NAME}_HDRS  
-        AP_Common.h
-        AP_Loop.h
-        AP_MetaClass.h
-        AP_Var.h
-        AP_Test.h
-        c++.h
-        AP_Vector.h
-)
-	
-include_directories(
- 
-  -
-  ${CMAKE_SOURCE_DIR}/libraries/AP_Common
-	${CMAKE_SOURCE_DIR}/libraries/FastSerial
-#	
-	)
-
-set(${LIB_NAME}_BOARD mega2560)
-
-generate_arduino_library(${LIB_NAME})

libraries/AP_Compass/CMakeLists.txt

@@ -1,27 +1 @@
-set(LIB_NAME AP_Compass)
+include(ApmMakeLib)
-	
-set(${LIB_NAME}_SRCS 
-		AP_Compass_HIL.cpp
-		AP_Compass_HMC5843.cpp
-		Compass.cpp		
-	)  # Firmware sources
-
-set(${LIB_NAME}_HDRS  		
-		Compass.h
-		AP_Compass_HIL.h
-		AP_Compass_HMC5843.h
-		AP_Compass.h
-	)
-	
-
-	
-include_directories(
- 
-  -
-  ${ARDUINO_LIBRARIES_PATH}/Wire
-  #${CMAKE_SOURCE_DIR}/libraries/FastSerial
-  #
-)
-set(${LIB_NAME}_BOARD mega2560)
-
-generate_arduino_library(${LIB_NAME})