Added APO branch.

ArduBoat/ArduBoat.pde

@@ -0,0 +1,184 @@
+/*
+ * ardupilotone
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#define ENABLE_FASTSERIAL_DEBUG
+
+// 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>
+
+FastSerialPort0(Serial);
+FastSerialPort1(Serial1);
+FastSerialPort2(Serial2);
+FastSerialPort3(Serial3);
+
+// Vehicle Configuration
+#include "BoatGeneric.h"
+
+/*
+ * Required Global Declarations
+ */
+
+static apo::AP_Autopilot * autoPilot;
+
+void setup() {
+
+	using namespace apo;
+	
+	AP_Var::load_all();
+
+	/*
+	 * Communications
+	 */
+	Serial.begin(DEBUG_BAUD, 128, 128); // debug
+	if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs
+	else Serial3.begin(TELEM_BAUD, 128, 128); // gcs
+
+	// hardware abstraction layer
+	AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer(
+			halMode, board, vehicle, heartBeatTimeout);
+	
+	// debug serial
+	hal->debug = &Serial;
+	hal->debug->println_P(PSTR("initializing debug line"));
+
+	/*
+	 * Initialize Comm Channels
+	 */
+	hal->debug->println_P(PSTR("initializing comm channels"));
+	if (hal->getMode() == MODE_LIVE) {
+		Serial1.begin(GPS_BAUD, 128, 16); // gps
+	} else { // hil
+		Serial1.begin(HIL_BAUD, 128, 128);
+	}
+
+	/*
+	 * Sensor initialization
+	 */
+	if (hal->getMode() == MODE_LIVE) {
+		hal->debug->println_P(PSTR("initializing adc"));
+		hal->adc = new ADC_CLASS;
+		hal->adc->Init();
+
+		if (gpsEnabled) {
+			hal->debug->println_P(PSTR("initializing gps"));
+			AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
+			hal->gps = &gpsDriver;
+			hal->gps->init();
+		}
+
+		if (baroEnabled) {
+			hal->debug->println_P(PSTR("initializing baro"));
+			hal->baro = new BARO_CLASS;
+			hal->baro->Init();
+		}
+
+		if (compassEnabled) {
+			hal->debug->println_P(PSTR("initializing compass"));
+			hal->compass = new COMPASS_CLASS;
+			hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD);
+			hal->compass->init();
+		}
+
+		/**
+		 * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not
+		 * initialize them and NULL will be assigned to those corresponding pointers.
+		 * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code
+		 * will not be executed by the navigator.
+		 * The coordinate system is assigned by the right hand rule with the thumb pointing down.
+		 * In set_orientation, it is defind as (front/back,left/right,down,up)
+		 */
+
+		if (rangeFinderFrontEnabled) {
+			hal->debug->println_P(PSTR("initializing front range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(1);
+			rangeFinder->set_orientation(1, 0, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderBackEnabled) {
+			hal->debug->println_P(PSTR("initializing back range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(2);
+			rangeFinder->set_orientation(-1, 0, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderLeftEnabled) {
+			hal->debug->println_P(PSTR("initializing left range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(3);
+			rangeFinder->set_orientation(0, -1, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderRightEnabled) {
+			hal->debug->println_P(PSTR("initializing right range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(4);
+			rangeFinder->set_orientation(0, 1, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderUpEnabled) {
+			hal->debug->println_P(PSTR("initializing up range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(5);
+			rangeFinder->set_orientation(0, 0, -1);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderDownEnabled) {
+			hal->debug->println_P(PSTR("initializing down range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(6);
+			rangeFinder->set_orientation(0, 0, 1);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+	}
+
+	/*
+	 * Select guidance, navigation, control algorithms
+	 */
+	AP_Navigator * navigator = new NAVIGATOR_CLASS(hal);
+	AP_Guide * guide = new GUIDE_CLASS(navigator, hal);
+	AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal);
+
+	/*
+	 * CommLinks
+	 */
+	if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal);
+	else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal);
+	
+	hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
+
+	/*
+	 * Start the autopilot
+	 */
+	hal->debug->printf_P(PSTR("initializing arduplane\n"));
+	hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
+	autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,
+			loop0Rate, loop1Rate, loop2Rate, loop3Rate);
+}
+
+void loop() {
+	autoPilot->update();
+}

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,80 @@
+#=============================================================================#
+# Author: Sebastian Rohde                                                      #
+# Date:   30.08.2011                                                          #
+#=============================================================================#
+
+
+#====================================================================#
+#  Settings                                                          #
+#====================================================================#
+set(FIRMWARE_NAME ArduBoat)
+
+set(${FIRMWARE_NAME}_BOARD ${BOARD})               # Arduino Target board
+
+set(${FIRMWARE_NAME}_SKETCHES
+	ardupilotone.pde	
+	)  # Firmware sketches
+	
+set(${FIRMWARE_NAME}_SRCS 
+	)  # Firmware sources
+		
+set(${FIRMWARE_NAME}_HDRS 
+        BoatGeneric.h
+        CarStampede.h
+        #CNIRover.h
+        #CNIBoat.h
+        ControllerBoat.h
+        ControllerCar.h
+        ControllerPlane.h
+        ControllerQuad.h
+        PlaneEasystar.h
+        QuadArducopter.h
+        QuadMikrokopter.h
+	)  # Firmware sources
+
+set(${FIRMWARE_NAME}_LIBS
+    m
+    APO
+    FastSerial
+    AP_Common
+    GCS_MAVLink
+    AP_GPS
+    APM_RC
+    AP_DCM
+    AP_ADC
+    AP_Compass
+    AP_IMU
+    AP_RangeFinder
+    APM_BMP085
+	ModeFilter
+)
+	
+
+#${CONSOLE_PORT}
+set(${FIRMWARE_NAME}_PORT COM2)            # Serial upload port
+set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X )  # Serial terminal cmd
+	
+include_directories(
+${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
+)
+#====================================================================#
+#  Target generation                                                 #
+#====================================================================#
+generate_arduino_firmware(${FIRMWARE_NAME})
+
+install(FILES
+	${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_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

ArduRover/ArduRover.pde

@@ -0,0 +1,184 @@
+/*
+ * ardupilotone
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#define ENABLE_FASTSERIAL_DEBUG
+
+// 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>
+
+FastSerialPort0(Serial);
+FastSerialPort1(Serial1);
+FastSerialPort2(Serial2);
+FastSerialPort3(Serial3);
+
+// Vehicle Configuration
+#include "CarStampede.h"
+
+/*
+ * Required Global Declarations
+ */
+
+static apo::AP_Autopilot * autoPilot;
+
+void setup() {
+
+	using namespace apo;
+	
+	AP_Var::load_all();
+
+	/*
+	 * Communications
+	 */
+	Serial.begin(DEBUG_BAUD, 128, 128); // debug
+	if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs
+	else Serial3.begin(TELEM_BAUD, 128, 128); // gcs
+
+	// hardware abstraction layer
+	AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer(
+			halMode, board, vehicle, heartBeatTimeout);
+	
+	// debug serial
+	hal->debug = &Serial;
+	hal->debug->println_P(PSTR("initializing debug line"));
+
+	/*
+	 * Initialize Comm Channels
+	 */
+	hal->debug->println_P(PSTR("initializing comm channels"));
+	if (hal->getMode() == MODE_LIVE) {
+		Serial1.begin(GPS_BAUD, 128, 16); // gps
+	} else { // hil
+		Serial1.begin(HIL_BAUD, 128, 128);
+	}
+
+	/*
+	 * Sensor initialization
+	 */
+	if (hal->getMode() == MODE_LIVE) {
+		hal->debug->println_P(PSTR("initializing adc"));
+		hal->adc = new ADC_CLASS;
+		hal->adc->Init();
+
+		if (gpsEnabled) {
+			hal->debug->println_P(PSTR("initializing gps"));
+			AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
+			hal->gps = &gpsDriver;
+			hal->gps->init();
+		}
+
+		if (baroEnabled) {
+			hal->debug->println_P(PSTR("initializing baro"));
+			hal->baro = new BARO_CLASS;
+			hal->baro->Init();
+		}
+
+		if (compassEnabled) {
+			hal->debug->println_P(PSTR("initializing compass"));
+			hal->compass = new COMPASS_CLASS;
+			hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD);
+			hal->compass->init();
+		}
+
+		/**
+		 * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not
+		 * initialize them and NULL will be assigned to those corresponding pointers.
+		 * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code
+		 * will not be executed by the navigator.
+		 * The coordinate system is assigned by the right hand rule with the thumb pointing down.
+		 * In set_orientation, it is defind as (front/back,left/right,down,up)
+		 */
+
+		if (rangeFinderFrontEnabled) {
+			hal->debug->println_P(PSTR("initializing front range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(1);
+			rangeFinder->set_orientation(1, 0, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderBackEnabled) {
+			hal->debug->println_P(PSTR("initializing back range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(2);
+			rangeFinder->set_orientation(-1, 0, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderLeftEnabled) {
+			hal->debug->println_P(PSTR("initializing left range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(3);
+			rangeFinder->set_orientation(0, -1, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderRightEnabled) {
+			hal->debug->println_P(PSTR("initializing right range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(4);
+			rangeFinder->set_orientation(0, 1, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderUpEnabled) {
+			hal->debug->println_P(PSTR("initializing up range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(5);
+			rangeFinder->set_orientation(0, 0, -1);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderDownEnabled) {
+			hal->debug->println_P(PSTR("initializing down range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(6);
+			rangeFinder->set_orientation(0, 0, 1);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+	}
+
+	/*
+	 * Select guidance, navigation, control algorithms
+	 */
+	AP_Navigator * navigator = new NAVIGATOR_CLASS(hal);
+	AP_Guide * guide = new GUIDE_CLASS(navigator, hal);
+	AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal);
+
+	/*
+	 * CommLinks
+	 */
+	if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal);
+	else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal);
+	
+	hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
+
+	/*
+	 * Start the autopilot
+	 */
+	hal->debug->printf_P(PSTR("initializing arduplane\n"));
+	hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
+	autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,
+			loop0Rate, loop1Rate, loop2Rate, loop3Rate);
+}
+
+void loop() {
+	autoPilot->update();
+}

ArduRover/CMakeLists.txt

@@ -0,0 +1,80 @@
+#=============================================================================#
+# Author: Sebastian Rohde                                                      #
+# Date:   30.08.2011                                                          #
+#=============================================================================#
+
+
+#====================================================================#
+#  Settings                                                          #
+#====================================================================#
+set(FIRMWARE_NAME ArduRover)
+
+set(${FIRMWARE_NAME}_BOARD ${BOARD})               # Arduino Target board
+
+set(${FIRMWARE_NAME}_SKETCHES
+	ardupilotone.pde	
+	)  # Firmware sketches
+	
+set(${FIRMWARE_NAME}_SRCS 
+	)  # Firmware sources
+		
+set(${FIRMWARE_NAME}_HDRS 
+        BoatGeneric.h
+        CarStampede.h
+        #CNIRover.h
+        #CNIBoat.h
+        ControllerBoat.h
+        ControllerCar.h
+        ControllerPlane.h
+        ControllerQuad.h
+        PlaneEasystar.h
+        QuadArducopter.h
+        QuadMikrokopter.h
+	)  # Firmware sources
+
+set(${FIRMWARE_NAME}_LIBS
+    m
+    APO
+    FastSerial
+    AP_Common
+    GCS_MAVLink
+    AP_GPS
+    APM_RC
+    AP_DCM
+    AP_ADC
+    AP_Compass
+    AP_IMU
+    AP_RangeFinder
+    APM_BMP085
+	ModeFilter
+)
+	
+
+#${CONSOLE_PORT}
+set(${FIRMWARE_NAME}_PORT COM2)            # Serial upload port
+set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X )  # Serial terminal cmd
+	
+include_directories(
+${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
+)
+#====================================================================#
+#  Target generation                                                 #
+#====================================================================#
+generate_arduino_firmware(${FIRMWARE_NAME})
+
+install(FILES
+	${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_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/CMakeLists.txt

@@ -0,0 +1,80 @@
+#=============================================================================#
+# Author: Sebastian Rohde                                                      #
+# Date:   30.08.2011                                                          #
+#=============================================================================#
+
+
+#====================================================================#
+#  Settings                                                          #
+#====================================================================#
+set(FIRMWARE_NAME APO)
+
+set(${FIRMWARE_NAME}_BOARD ${BOARD})               # Arduino Target board
+
+set(${FIRMWARE_NAME}_SKETCHES
+	ardupilotone.pde	
+	)  # Firmware sketches
+	
+set(${FIRMWARE_NAME}_SRCS 
+	)  # Firmware sources
+		
+set(${FIRMWARE_NAME}_HDRS 
+        BoatGeneric.h
+        CarStampede.h
+        #CNIRover.h
+        #CNIBoat.h
+        ControllerBoat.h
+        ControllerCar.h
+        ControllerPlane.h
+        ControllerQuad.h
+        PlaneEasystar.h
+        QuadArducopter.h
+        QuadMikrokopter.h
+	)  # Firmware sources
+
+set(${FIRMWARE_NAME}_LIBS
+    m
+    APO
+    FastSerial
+    AP_Common
+    GCS_MAVLink
+    AP_GPS
+    APM_RC
+    AP_DCM
+    AP_ADC
+    AP_Compass
+    AP_IMU
+    AP_RangeFinder
+    APM_BMP085
+	ModeFilter
+)
+	
+
+#${CONSOLE_PORT}
+set(${FIRMWARE_NAME}_PORT COM2)            # Serial upload port
+set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X )  # Serial terminal cmd
+	
+include_directories(
+${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
+)
+#====================================================================#
+#  Target generation                                                 #
+#====================================================================#
+generate_arduino_firmware(${FIRMWARE_NAME})
+
+install(FILES
+	${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_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,184 @@
+/*
+ * ardupilotone
+ *
+ *  Created on: Apr 30, 2011
+ *      Author: jgoppert
+ */
+
+#define ENABLE_FASTSERIAL_DEBUG
+
+// 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>
+
+FastSerialPort0(Serial);
+FastSerialPort1(Serial1);
+FastSerialPort2(Serial2);
+FastSerialPort3(Serial3);
+
+// Vehicle Configuration
+#include "PlaneEasystar.h"
+
+/*
+ * Required Global Declarations
+ */
+
+static apo::AP_Autopilot * autoPilot;
+
+void setup() {
+
+	using namespace apo;
+	
+	AP_Var::load_all();
+
+	/*
+	 * Communications
+	 */
+	Serial.begin(DEBUG_BAUD, 128, 128); // debug
+	if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs
+	else Serial3.begin(TELEM_BAUD, 128, 128); // gcs
+
+	// hardware abstraction layer
+	AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer(
+			halMode, board, vehicle, heartBeatTimeout);
+	
+	// debug serial
+	hal->debug = &Serial;
+	hal->debug->println_P(PSTR("initializing debug line"));
+
+	/*
+	 * Initialize Comm Channels
+	 */
+	hal->debug->println_P(PSTR("initializing comm channels"));
+	if (hal->getMode() == MODE_LIVE) {
+		Serial1.begin(GPS_BAUD, 128, 16); // gps
+	} else { // hil
+		Serial1.begin(HIL_BAUD, 128, 128);
+	}
+
+	/*
+	 * Sensor initialization
+	 */
+	if (hal->getMode() == MODE_LIVE) {
+		hal->debug->println_P(PSTR("initializing adc"));
+		hal->adc = new ADC_CLASS;
+		hal->adc->Init();
+
+		if (gpsEnabled) {
+			hal->debug->println_P(PSTR("initializing gps"));
+			AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
+			hal->gps = &gpsDriver;
+			hal->gps->init();
+		}
+
+		if (baroEnabled) {
+			hal->debug->println_P(PSTR("initializing baro"));
+			hal->baro = new BARO_CLASS;
+			hal->baro->Init();
+		}
+
+		if (compassEnabled) {
+			hal->debug->println_P(PSTR("initializing compass"));
+			hal->compass = new COMPASS_CLASS;
+			hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD);
+			hal->compass->init();
+		}
+
+		/**
+		 * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not
+		 * initialize them and NULL will be assigned to those corresponding pointers.
+		 * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code
+		 * will not be executed by the navigator.
+		 * The coordinate system is assigned by the right hand rule with the thumb pointing down.
+		 * In set_orientation, it is defind as (front/back,left/right,down,up)
+		 */
+
+		if (rangeFinderFrontEnabled) {
+			hal->debug->println_P(PSTR("initializing front range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(1);
+			rangeFinder->set_orientation(1, 0, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderBackEnabled) {
+			hal->debug->println_P(PSTR("initializing back range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(2);
+			rangeFinder->set_orientation(-1, 0, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderLeftEnabled) {
+			hal->debug->println_P(PSTR("initializing left range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(3);
+			rangeFinder->set_orientation(0, -1, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderRightEnabled) {
+			hal->debug->println_P(PSTR("initializing right range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(4);
+			rangeFinder->set_orientation(0, 1, 0);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderUpEnabled) {
+			hal->debug->println_P(PSTR("initializing up range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(5);
+			rangeFinder->set_orientation(0, 0, -1);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+		if (rangeFinderDownEnabled) {
+			hal->debug->println_P(PSTR("initializing down range finder"));
+			RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
+			rangeFinder->set_analog_port(6);
+			rangeFinder->set_orientation(0, 0, 1);
+			hal->rangeFinders.push_back(rangeFinder);
+		}
+
+	}
+
+	/*
+	 * Select guidance, navigation, control algorithms
+	 */
+	AP_Navigator * navigator = new NAVIGATOR_CLASS(hal);
+	AP_Guide * guide = new GUIDE_CLASS(navigator, hal);
+	AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal);
+
+	/*
+	 * CommLinks
+	 */
+	if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal);
+	else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal);
+	
+	hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
+
+	/*
+	 * Start the autopilot
+	 */
+	hal->debug->printf_P(PSTR("initializing arduplane\n"));
+	hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
+	autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,
+			loop0Rate, loop1Rate, loop2Rate, loop3Rate);
+}
+
+void loop() {
+	autoPilot->update();
+}

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/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,39 @@
+set(LIB_NAME APO)
+	
+set(${LIB_NAME}_SRCS 
+        AP_Autopilot.cpp
+        AP_CommLink.cpp
+        AP_Controller.cpp
+        AP_Guide.cpp
+        AP_HardwareAbstractionLayer.cpp
+        AP_MavlinkCommand.cpp
+        AP_Navigator.cpp
+        AP_RcChannel.cpp
+	APO.cpp 
+	)  # Firmware sources
+
+set(${LIB_NAME}_HDRS  
+        AP_Autopilot.h
+        AP_CommLink.h
+        AP_Controller.h
+        AP_Guide.h
+        AP_HardwareAbstractionLayer.h
+        AP_MavlinkCommand.h
+        AP_Navigator.h
+        AP_RcChannel.h
+        AP_Var_keys.h
+	APO.h
+        constants.h
+        template.h
+)
+	
+include_directories(
+#	${CMAKE_SOURCE_DIR}/libraries/AP_Common 
+	${CMAKE_SOURCE_DIR}/libraries/FastSerial
+	${CMAKE_SOURCE_DIR}/libraries/ModeFilter
+#	
+	)
+
+set(${LIB_NAME}_BOARD ${BOARD})
+
+generate_arduino_library(${LIB_NAME})

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_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;
+        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;
+    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/CMakeLists.txt

@@ -28,6 +28,6 @@ include_directories(
 #	
 	)
 
-set(${LIB_NAME}_BOARD mega2560)
+set(${LIB_NAME}_BOARD ${BOARD})
 
 generate_arduino_library(${LIB_NAME})

libraries/FastSerial/CMakeLists.txt

@@ -23,6 +23,6 @@ include_directories(
 #	
 	)
 
-set(${LIB_NAME}_BOARD mega2560)
+set(${LIB_NAME}_BOARD ${BOARD})
 
 generate_arduino_library(${LIB_NAME})

libraries/FastSerial/FastSerial.cpp

@@ -45,6 +45,7 @@
 
 FastSerial::Buffer __FastSerial__rxBuffer[FS_MAX_PORTS];
 FastSerial::Buffer __FastSerial__txBuffer[FS_MAX_PORTS];
+uint8_t FastSerial::_serialInitialized = 0;
 
 // Constructor /////////////////////////////////////////////////////////////////
 
@@ -61,6 +62,8 @@ FastSerial::FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volati
 					   _rxBuffer(&__FastSerial__rxBuffer[portNumber]),
 					   _txBuffer(&__FastSerial__txBuffer[portNumber])
 {
+	setInitialized(portNumber);
+	begin(57600);
 }
 
 // Public Methods //////////////////////////////////////////////////////////////

libraries/FastSerial/FastSerial.h

@@ -55,6 +55,7 @@
 
 #include "BetterStream.h"
 
+
 /// @file	FastSerial.h
 /// @brief	An enhanced version of the Arduino HardwareSerial class
 ///			implementing interrupt-driven transmission and flexible
@@ -101,6 +102,7 @@ extern class FastSerial Serial3;
 ///
 class FastSerial: public BetterStream {
 public:
+
 	/// Constructor
 	FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, volatile uint8_t *ucsra,
 			   volatile uint8_t *ucsrb, const uint8_t u2x, const uint8_t portEnableBits, const uint8_t portTxBits);
@@ -149,7 +151,21 @@ public:
 		uint8_t *bytes;					///< pointer to allocated buffer
 	};
 
+	/// Tell if the serial port has been initialized
+	static bool getInitialized(uint8_t port) {
+		return (1<<port) & _serialInitialized;
+	}
+
 private:
+
+	/// Bit mask for initialized ports
+	static uint8_t _serialInitialized;
+
+	/// Set if the serial port has been initialized
+	static void setInitialized(uint8_t port) {
+		_serialInitialized |= (1<<port);
+	}
+
 	// register accessors
 	volatile uint8_t * const _ubrrh;
 	volatile uint8_t * const _ubrrl;