APO merge.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1935 f9c3cf11-9bcb-44bc-f272-b75c42450872

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,21 @@
+/*
+ * 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"
+//#include "APO_Config.h"
+
+#endif /* APO_H_ */

libraries/APO/APO_Config.h

@@ -0,0 +1,50 @@
+#ifndef APO_Config_H
+#define APO_Config_H
+
+#include "AP_HardwareAbstractionLayer.h"
+#include "mikrokopter.h"
+#include "constants.h"
+
+// Serial 0: debug      /dev/ttyUSB0
+// Serial 1: gps/hil    /dev/ttyUSB1
+// Serial 2: gcs        /dev/ttyUSB2
+
+// select hardware absraction mode from
+// 	MODE_LIVE, actual flight
+// 	TODO: IMPLEMENT --> MODE_HIL_NAV, hardware in the loop with sensors running, tests navigation system and control
+// 	MODE_HIL_CNTL, hardware in the loop with only controller running, just tests controller
+extern apo::halMode_t halMode;
+
+// select from, BOARD_ARDUPILOTMEGA
+extern apo::board_t board;
+
+// select from, VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE
+extern apo::vehicle_t vehicle;
+
+//---------ADVANCED SECTION ----------------//
+
+// loop rates
+extern const float loop0Rate;
+extern const float loop1Rate;
+extern const float loop2Rate;
+extern const float loop3Rate;
+extern const float loop4Rate;
+
+// max time in seconds to allow flight without ground station comms
+// zero will ignore timeout
+extern const uint8_t heartbeatTimeout;
+
+//---------HARDWARE CONFIG ----------------//
+
+//Hardware Parameters
+#define SLIDE_SWITCH_PIN 40
+#define PUSHBUTTON_PIN 41
+#define A_LED_PIN 37 //36 = B,3637 = A,363735 = C
+#define B_LED_PIN 36
+#define C_LED_PIN 35
+#define EEPROM_MAX_ADDR	2048
+#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarLV
+
+
+#endif
+// vim:ts=4:sw=4:expandtab

libraries/APO/AP_Autopilot.cpp

@@ -0,0 +1,216 @@
+/*
+ * 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) :
+	Loop(loop0Rate, callback0, this),
+			_navigator(navigator), _guide(guide), _controller(controller), _hal(hal) {
+
+	/*
+	 * Calibration
+	 */
+	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->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
+		delay(1000);
+		if (hal->mode() == 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->mode() == 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(0);
+	home.setAlt(_navigator->getAlt());
+	home.setLat(_navigator->getLat());
+	home.setLon(_navigator->getLon());
+	home.save();
+	_hal->debug->printf_P(PSTR("home before load lat: %f deg, lon: %f deg\n"), home.getLat()*rad2Deg,home.getLon()*rad2Deg);
+	home.load();
+	_hal->debug->printf_P(PSTR("home after load lat: %f deg, lon: %f deg\n"), home.getLat()*rad2Deg,home.getLon()*rad2Deg);
+
+	/*
+	 * Attach loops
+	 */
+	hal->debug->println_P(PSTR("attaching loops"));
+	subLoops().push_back(new Loop(loop1Rate, callback1, this));
+	subLoops().push_back(new Loop(loop2Rate, callback2, this));
+	subLoops().push_back(new Loop(loop3Rate, callback3, this));
+	subLoops().push_back(new Loop(loop4Rate, callback4, this));
+
+	hal->debug->println_P(PSTR("running"));
+	hal->gcs->sendText(SEVERITY_LOW,PSTR("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->navigator())
+		apo->navigator()->updateFast(1.0/loop0Rate);
+}
+
+void AP_Autopilot::callback1(void * data) {
+	AP_Autopilot * apo = (AP_Autopilot *) data;
+	//apo->hal()->debug->println_P(PSTR("callback 1"));
+
+	/*
+	 * hardware in the loop
+	 */
+	if (apo->hal()->hil && apo->hal()->mode()!=MODE_LIVE)
+	{
+		apo->hal()->hil->receive();
+		apo->hal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
+	}
+
+	/*
+     * update control laws
+	 */
+	if (apo->guide())
+		apo->guide()->update();
+
+	/*
+	 * update control laws
+	 */
+	if (apo->controller())
+	{
+		//apo->hal()->debug->println_P(PSTR("updating controller"));
+		apo->controller()->update(1./loop1Rate);
+	}
+	/*
+	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->hal()->debug->println_P(PSTR("callback 2"));
+
+	/*
+	 * send telemetry
+	 */
+	if (apo->hal()->gcs) {
+		// send messages
+		apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW);
+		apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE);
+		//apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
+		//apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION);
+		apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
+		//apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
+	}
+
+	/*
+	 * slow navigation loop update
+	 */
+	if (apo->navigator()) {
+		apo->navigator()->updateSlow(1.0/loop2Rate);
+	}
+
+	/*
+	 * handle ground control station communication
+	 */
+	if (apo->hal()->gcs) {
+		// send messages
+		apo->hal()->gcs->requestCmds();
+		apo->hal()->gcs->sendParameters();
+
+		// receive messages
+		apo->hal()->gcs->receive();
+	}
+
+	/*
+	 * navigator debug
+	 */
+	/*
+	 if (apo->navigator()) {
+		 apo->hal()->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->hal()->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->hal()->debug->println_P(PSTR("callback 3"));
+
+	/*
+	 * send heartbeat
+	 */
+	apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
+
+	/*
+	 * load/loop rate/ram debug
+	 */
+
+	apo->hal()->load = apo->load();
+	apo->hal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"),
+			apo->load(),1.0/apo->dt(),freeMemory());
+
+	apo->hal()->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->hal()->debug->println_P(PSTR("callback 4"));
+}
+
+} // apo

libraries/APO/AP_Autopilot.h

@@ -0,0 +1,153 @@
+/*
+ * 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"
+#include "controllers.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 the serial streams available
+ * and attaches them to the appropriate devices.
+ * Also, since APM_RC is globally instantiated this
+ * is also passed to the constructor so that we
+ * can avoid and global instance calls for maximum
+ * clarity.
+ *
+ * It inherits from loop to manage
+ * the subloops and sets the overal
+ * 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);
+
+	/**
+	 * Accessors
+	 */
+	AP_Navigator * navigator() {
+		return _navigator;
+	}
+	AP_Guide * guide() {
+		return _guide;
+	}
+	AP_Controller * controller() {
+		return _controller;
+	}
+	AP_HardwareAbstractionLayer * hal() {
+		return _hal;
+	}
+
+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);
+
+	/**
+	 * Loop 1 Callbacks
+	 * - control
+	 * - compass reading
+	 * @see callback0
+	 */
+	static void callback1(void * data);
+
+	/**
+	 * Loop 2 Callbacks
+	 * - gps sensor fusion
+	 * - compass sensor fusion
+	 * @see callback0
+	 */
+	static void callback2(void * data);
+
+	/**
+	 * Loop 3 Callbacks
+	 * - slow messages
+	 * @see callback0
+	 */
+	static void callback3(void * data);
+
+	/**
+	 * Loop 4 Callbacks
+	 * - super slow mesages
+	 * - log writing
+	 * @see callback0
+	 */
+	static void callback4(void * data);
+
+	/**
+	 * 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,775 @@
+/*
+ * 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),
+
+				// 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_NB_HIGH;
+			break;
+		}
+	}
+
+	virtual void send() {
+		// if number of channels exceeded return
+		if (_channel == MAVLINK_COMM_NB_HIGH)
+			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_NB_HIGH)
+			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 (int 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 (int 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, prev_cell, temp;
+	    	  temp = analogRead(0);
+	    	  batteryVoltage = ((temp*5/1023)/0.28);
+
+				mavlink_msg_sys_status_send(_channel, _controller->getMode(),
+								_guide->getMode(), _hal->state(), _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_NB_HIGH)
+			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 <= _guide->getNumberOfCommands()) {
+			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;
+	Vector<mavlink_command_t *> _cmdList;
+
+	// parameters
+	static uint8_t _paramNameLengthMax;
+	uint16_t _parameterCount;
+	AP_Var * _queuedParameter;
+	uint16_t _queuedParameterIndex;
+	uint16_t _cmdMax;
+
+	// 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->setHeading(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);
+
+		mavlink_waypoint_t msg = cmd.convert(_guide->getCurrentIndex());
+		mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId,
+				msg.seq, msg.frame, msg.command, msg.current, msg.autocontinue,
+				msg.param1, msg.param2, msg.param3, msg.param4, msg.x, msg.y,
+				msg.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);
+		if (_checkTarget(packet.target_system, packet.target_component))
+			break;
+
+		// set current waypoint
+		_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;
+		}
+		_guide->setNumberOfCommands(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 msg[50];
+			sprintf(msg,
+					"waypoint request out of sequence: (packet) %d / %d (ap)",
+					packet.seq, _cmdRequestIndex);
+			sendText(SEVERITY_HIGH, msg);
+			*/
+			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 >= _guide->getNumberOfCommands()) {
+			sendMessage( MAVLINK_MSG_ID_WAYPOINT_ACK);
+			//sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
+		}
+		_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,167 @@
+/*
+ * 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) {
+	}
+
+	bool commLost() {
+		if (_hal->getTimeSinceLastHeartBeat() > heartbeatTimeout) return true;
+		else return false;
+	}
+
+	virtual void update(const float & dt) = 0;
+
+	MAV_MODE getMode() { return _mode; }
+
+protected:
+	MAV_MODE _mode;
+	AP_Navigator * _nav;
+	AP_Guide * _guide;
+	AP_HardwareAbstractionLayer * _hal;
+};
+
+class Pid2 {
+public:
+	Pid2(AP_Var::Key key, const prog_char_t * name, float kP = 0.0,
+			float kI = 0.0, float kD = 0.0, float iMax = 0.0, float yMax = 0.0,
+			uint8_t dFcut = 20.0) :
+		_group(key, name), _eP(0), _eI(0), _eD(0),
+				_kP(&_group, 1, kP, PSTR("P")),
+				_kI(&_group, 2, kI, PSTR("I")),
+				_kD(&_group, 3, kD, PSTR("D")),
+				_iMax(&_group, 4, iMax, PSTR("IMAX")),
+				_yMax(&_group, 5, yMax, PSTR("YMAX")),
+				_fCut(&_group, 6, dFcut, PSTR("FCUT")) {
+	}
+
+	float update(const float & input, const float & dt) {
+
+		// derivative with low pass
+		float RC = 1 / (2 * M_PI * _fCut); // low pass filter
+		_eD = _eD + ((_eP - input) / dt - _eD) * (dt / (dt + RC));
+
+		// proportional, note must come after derivative
+		// because derivatve uses _eP as previous value
+		_eP = input;
+
+		// integral
+		_eI += _eP * dt;
+
+		// wind up guard
+		if (_eI > _iMax)
+			_eI = _iMax;
+		if (_eI < -_iMax)
+			_eI = -_iMax;
+
+		// pid sum
+		float y = _kP * _eP + _kI * _eI + _kD * _eD;
+
+		// saturation
+		if (y > _yMax)
+			y = _yMax;
+		if (y < -_yMax)
+			y = -_yMax;
+
+		return y;
+	}
+protected:
+	AP_Var_group _group; /// helps with parameter management
+	float _eP; /// input
+	float _eI; /// integral of input
+	float _eD; /// derivative of input
+	AP_Float _kP; /// proportional gain
+	AP_Float _kI; /// integral gain
+	AP_Float _kD; /// derivative gain
+	AP_Float _iMax; /// integrator saturation
+	AP_Float _yMax; /// output saturation
+	AP_Uint8 _fCut; /// derivative low-pass cut freq (Hz)
+};
+
+
+/// PID(DFB) block
+class PidDFB2 {
+public:
+	PidDFB2(AP_Var::Key key, const prog_char_t * name,
+			float kP = 0.0, float kI = 0.0, float kD = 0.0, float iMax = 0.0,
+			float yMax = 0.0) :
+		_group(key, name), _eP(0), _eI(0), _eD(0),
+				_kP(&_group, 1, kP, PSTR("P")),
+				_kI(&_group, 2, kI, PSTR("I")),
+				_kD(&_group, 3, kD, PSTR("D")),
+				_iMax(&_group, 4, iMax, PSTR("IMAX")),
+				_yMax(&_group, 5, yMax, PSTR("YMAX")) {
+	}
+
+	float update(const float & input, const float & derivative, const float & dt) {
+
+		// proportional, note must come after derivative
+		// because derivative uses _eP as previous value
+		_eP = input;
+
+		// integral
+		_eI += _eP * dt;
+
+		// wind up guard
+		if (_eI > _iMax)
+			_eI = _iMax;
+		if (_eI < -_iMax)
+			_eI = -_iMax;
+
+		// pid sum
+		float y = _kP * _eP + _kI * _eI - _kD * derivative;
+
+		// saturation
+		if (y > _yMax)
+			y = _yMax;
+		if (y < -_yMax)
+			y = -_yMax;
+
+		return y;
+	}
+protected:
+	AP_Var_group _group; /// helps with parameter management
+	float _eP; /// input
+	float _eI; /// integral of input
+	float _eD; /// derivative of input
+	AP_Float _kP; /// proportional gain
+	AP_Float _kI; /// integral gain
+	AP_Float _kD; /// derivative gain
+	AP_Float _iMax; /// integrator saturation
+	AP_Float _yMax; /// integrator saturation
+};
+
+} // 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,298 @@
+/*
+ * 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), headingCommand(0), airSpeedCommand(0),
+				groundSpeedCommand(0), altitudeCommand(0), pNCmd(0), pECmd(0),
+				pDCmd(0), _hal(hal), _home(0), _command(0), _previousCommand(0),
+				_mode(MAV_NAV_LOST), _numberOfCommands(1), _cmdIndex(0)
+	{
+	}
+
+	virtual void update() = 0;
+	virtual void nextCommand() = 0;
+	float headingCommand;
+	float airSpeedCommand;
+	float groundSpeedCommand;
+	float altitudeCommand;
+	float pNCmd;
+	float pECmd;
+	float pDCmd;
+	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());
+		_previousCommand = AP_MavlinkCommand(getPreviousIndex());
+		//_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;
+	}
+
+protected:
+
+	AP_MavlinkCommand _home, _command, _previousCommand;
+	MAV_NAV _mode;
+	AP_Uint8 _numberOfCommands;
+	AP_Uint8 _cmdIndex;
+	AP_Navigator * _navigator;
+	AP_HardwareAbstractionLayer * _hal;
+};
+
+class MavlinkGuide: public AP_Guide {
+public:
+	MavlinkGuide(AP_Var::Key key, AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) :
+		AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),
+				_rangeFinderLeft(), _rangeFinderRight(),
+				_group(key,PSTR("GUIDE_")),
+				_velocityCommand(&_group, 1, 1, PSTR("VELCMD")),
+				_crossTrackGain(&_group, 2, 2, PSTR("XTK")),
+				_crossTrackLim(&_group, 3, 10, PSTR("XTLIM"))
+		{
+
+		for (int 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) {
+			headingCommand = _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,home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt()));
+					*/
+		} else {
+			// TODO wrong behavior if 0 selected as waypoint, says previous 0
+			float dXt = AP_MavlinkCommand::crossTrack(_previousCommand,
+					_command, _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 = AP_MavlinkCommand::alongTrack(_previousCommand,_command,
+					_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) 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 = 0;
+		pECmd = 0;
+		pDCmd = 0;
+
+		// process mavlink commands
+		//handleCommand();
+
+		// obstacle avoidance overrides
+		// stop if your going to drive into something in front of you
+		for(int 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) {
+			//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() {
+		_cmdIndex = getNextIndex();
+		_command = AP_MavlinkCommand(getCurrentIndex());
+		_previousCommand = AP_MavlinkCommand(getPreviousIndex());
+		//_hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT);
+	}
+
+	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();
+			}
+			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
+			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,99 @@
+/*
+ * 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};
+enum vehicle_t {VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE};
+
+class AP_HardwareAbstractionLayer {
+
+public:
+
+	AP_HardwareAbstractionLayer(halMode_t mode, board_t board, vehicle_t vehicle) :
+		_mode(mode), _board(board), _vehicle(vehicle), adc(),
+		gps(), baro(), compass(), rangeFinders(),
+		imu(), rc(), gcs(), hil(), debug(), load()
+	{
+	}
+	/**
+	 * 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;
+
+	// accessors
+	halMode_t mode() { return _mode; }
+	board_t board() { return _board; }
+	vehicle_t vehicle() { return _vehicle; }
+	MAV_STATE state(){ return _state; }
+
+	float getTimeSinceLastHeartBeat() {
+		return (micros() - lastHeartBeat)/1e6;
+	}
+
+	uint8_t load;
+	uint32_t lastHeartBeat;
+
+private:
+
+	// enumerations
+	halMode_t _mode;
+	board_t _board;
+	vehicle_t _vehicle;
+	MAV_STATE _state;
+};
+
+}
+
+#endif /* AP_HARDWAREABSTRACTIONLAYER_H_ */

libraries/APO/AP_MavlinkCommand.cpp

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

libraries/APO/AP_MavlinkCommand.h

@@ -0,0 +1,456 @@
+/*
+ * 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"
+
+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;
+	//static AP_Var_group _group;
+public:
+	/**
+	 * Constructor for loading from memory.
+	 * @param index Start at zero.
+	 */
+	AP_MavlinkCommand(uint16_t index) :
+		_seq(index), _data(k_commands+index) {
+		_data.load();
+		//Serial.print("x: "); Serial.println(_data.get().x);
+	}
+
+	/**
+	 * Constructor for saving from command a mavlink waypoint.
+	 * @param cmd The mavlink_waopint_t structure for the command.
+	 */
+	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);
+		_data.save();
+		/*
+		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);
+		*/
+		_data.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);
+		*/
+	}
+	bool save() {
+		return _data.save();
+	}
+	bool load() {
+		return _data.load();
+	}
+	uint8_t getSeq() {
+		return _seq;
+	}
+	bool getAutocontinue() {
+		return _data.get().autocontinue;
+	}
+	void setAutocontinue(bool val) {
+		_data.get().autocontinue = val;
+	}
+	void setSeq(uint8_t val) {
+		_seq = val;
+	}
+	MAV_CMD getCommand() {
+		return _data.get().command;
+	}
+	void setCommand(MAV_CMD val) {
+		_data.get().command = val;
+	}
+	MAV_FRAME getFrame() {
+		return _data.get().frame;
+	}
+	void setFrame(MAV_FRAME val) {
+		_data.get().frame = val;
+	}
+	float getParam1() {
+		return _data.get().param1;
+	}
+	void setParam1(float val) {
+		_data.get().param1 = val;
+	}
+	float getParam2() {
+		return _data.get().param2;
+	}
+	void setParam2(float val) {
+		_data.get().param2 = val;
+	}
+	float getParam3() {
+		return _data.get().param3;
+	}
+	void setParam3(float val) {
+		_data.get().param3 = val;
+	}
+	float getParam4() {
+		return _data.get().param4;
+	}
+	void setParam4(float val) {
+		_data.get().param4 = val;
+	}
+	float getX() {
+		return _data.get().x;
+	}
+	void setX(float val) {
+		_data.get().x = val;
+	}
+	float getY() {
+		return _data.get().y;
+	}
+	void setY(float val) {
+		_data.get().y = val;
+	}
+	float getZ() {
+		return _data.get().z;
+	}
+	void setZ(float val) {
+		_data.get().z = val;
+	}
+	float getLatDeg() {
+		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() {
+		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() {
+		return getLonDeg()*1e7;
+	}
+	int32_t getLat_degInt() {
+		return getLatDeg()*1e7;
+	}
+	float getLon() {
+		return getLonDeg()*deg2Rad;
+	}
+	float getLat() {
+		return getLatDeg()*deg2Rad;
+	}
+	void setLat(float val) {
+		setLatDeg(val*rad2Deg);
+	}
+	float getAlt() {
+		switch (getFrame()) {
+		case MAV_FRAME_GLOBAL:
+			return getZ();
+			break;
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+		case MAV_FRAME_LOCAL:
+			return getZ() + AP_MavlinkCommand(0).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 - AP_MavlinkCommand(0).getLonDeg());
+			break;
+		case MAV_FRAME_MISSION:
+		default:
+			break;
+		}
+	}
+	/**
+	 * Get the relative altitud to home
+	 * @return relative altitude in meters
+	 */
+	float getRelAlt() {
+		switch (getFrame()) {
+		case MAV_FRAME_GLOBAL:
+			return getZ() - AP_MavlinkCommand(0).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 + AP_MavlinkCommand(0).getAlt());
+			break;
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+		case MAV_FRAME_LOCAL:
+			setZ(val);
+			break;
+		case MAV_FRAME_MISSION:
+			break;
+		}
+	}
+
+	float getRadius() {
+		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) {
+		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;
+	}
+	/**
+	 * 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) {
+		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;
+	}
+
+	/**
+	 * 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) {
+		// 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;
+	}
+
+	/**
+	 * Distance to another command
+	 * @param next The command to measure to.
+	 * @return The distance in meters.
+	 */
+	float distanceTo(AP_MavlinkCommand next) {
+		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;
+	}
+
+	/**
+	 * 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) {
+		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;
+	}
+
+	float getPN(int32_t lat_degInt, int32_t lon_degInt) {
+		// 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) {
+		// local tangent approximation at this waypoint
+		float deltaLon = (lon_degInt - getLon_degInt())*degInt2Rad;
+		return cos(getLat())*deltaLon*rEarth;
+	}
+
+	float getPD(int32_t alt_intM) {
+		return -(alt_intM/scale_m - getAlt());
+	}
+
+	float getLat(float pN) {
+
+		return pN/rEarth + getLat();
+	}
+
+	float getLon(float pE) {
+
+		return pE/rEarth/cos(getLat()) + getLon();
+	}
+
+	/**
+	 * Gets altitude in meters
+	 * @param pD alt in meters
+	 * @return
+	 */
+	float getAlt(float pD) {
+
+		return getAlt() - pD;
+	}
+
+	//calculates cross track of a current location
+	static float crossTrack(AP_MavlinkCommand previous, AP_MavlinkCommand current, 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(current);
+		return asin(sin(d/rEarth) * sin(bCurrent - bNext)) * rEarth;
+	}
+
+	// calculates along  track distance of a current location
+	static float alongTrack(AP_MavlinkCommand previous, AP_MavlinkCommand current, int32_t lat_degInt, int32_t lon_degInt) {
+		// ignores lat/lon since single prec.
+		float dXt = crossTrack(previous,current,lat_degInt,lon_degInt);
+		float d = previous.distanceTo(lat_degInt,lon_degInt);
+		return dXt/tan(asin(dXt/d));
+	}
+};
+
+
+#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,445 @@
+/*
+ * 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 "APO_Config.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), _heading(0),
+		_lon_degInt(0), _alt_intM(0)
+	{
+	}
+	virtual void calibrate() = 0;
+	virtual void updateFast(float dt) = 0;
+	virtual void updateSlow(float dt) = 0;
+    float getPD() const
+    {
+    	AP_MavlinkCommand home = AP_MavlinkCommand(0);
+        return home.getPD(getAlt_intM());
+    }
+
+    float getPE() const
+    {
+    	AP_MavlinkCommand home = AP_MavlinkCommand(0);
+        return home.getPE(getLat_degInt(),getLon_degInt());
+    }
+
+    float getPN() const
+    {
+    	AP_MavlinkCommand home = AP_MavlinkCommand(0);
+        return home.getPN(getLat_degInt(),getLon_degInt());
+    }
+
+    void setPD(float _pD)
+    {
+        AP_MavlinkCommand home = AP_MavlinkCommand(0);
+        setAlt(home.getAlt(_pD));
+    }
+
+    void setPE(float _pE)
+    {
+    	AP_MavlinkCommand home = AP_MavlinkCommand(0);
+    	setLat(home.getLat(_pE));
+    }
+
+    void setPN(float _pN)
+    {
+    	AP_MavlinkCommand home = AP_MavlinkCommand(0);
+    	setLon(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
+    {
+        return _lat_degInt * degInt2Rad;
+    }
+
+    void setLat(float _lat)
+    {
+        this->_lat_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 getHeading() const
+    {
+    	return _heading;
+    }
+
+    float getVE() const
+    {
+        return sin(getHeading())*getGroundSpeed();
+    }
+
+    float getGroundSpeed() const
+    {
+        return _groundSpeed;
+    }
+
+    int32_t getLat_degInt() const
+    {
+        return _lat_degInt;
+    }
+
+    int32_t getLon_degInt() const
+    {
+        return _lon_degInt;
+    }
+
+    float getVN() const
+    {
+        return cos(getHeading())*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)
+    {
+        this->_airSpeed = _airSpeed;
+    }
+
+    void setHeading(float _heading)
+    {
+    	this->_heading = _heading;
+    }
+
+    void setAlt_intM(int32_t _alt_intM)
+    {
+        this->_alt_intM = _alt_intM;
+    }
+
+    void setVD(float _vD)
+    {
+        this->_vD = _vD;
+    }
+
+    void setGroundSpeed(float _groundSpeed)
+    {
+        this->_groundSpeed = _groundSpeed;
+    }
+
+    void setLat_degInt(int32_t _lat_degInt)
+    {
+        this->_lat_degInt = _lat_degInt;
+    }
+
+    void setLon_degInt(int32_t _lon_degInt)
+    {
+        this->_lon_degInt = _lon_degInt;
+    }
+
+    void setPitch(float _pitch)
+    {
+        this->_pitch = _pitch;
+    }
+
+    void setPitchRate(float _pitchRate)
+    {
+        this->_pitchRate = _pitchRate;
+    }
+
+    void setRoll(float _roll)
+    {
+        this->_roll = _roll;
+    }
+
+    void setRollRate(float _rollRate)
+    {
+        this->_rollRate = _rollRate;
+    }
+
+    void setYaw(float _yaw)
+    {
+        this->_yaw = _yaw;
+    }
+
+    void setYawRate(float _yawRate)
+    {
+        this->_yawRate = _yawRate;
+    }
+    void setTimeStamp(int32_t _timeStamp)
+    {
+        this->_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 _heading; // rad
+	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 (int 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->mode() == 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() {
+		// TODO: handle cold restart
+		if (_hal->imu) {
+			/*
+			 * Gyro has built in warm up cycle and should
+			 * run first */
+			_hal->imu->init_gyro();
+			_hal->imu->init_accel();
+		}
+	}
+	virtual void updateFast(float dt) {
+		if (_hal->mode() != 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(dt);
+			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->mode() != 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());
+		}
+
+		// need to use lat/lon and convert
+		// TODO make a local copy of home location, EEPROM takes way too long to read
+//		AP_MavlinkCommand home(0);
+//		setPN((getLat() - home.getLat())/rEarth);
+//		setPE((getLon() - home.getLon())*cos(home.getLat())/rEarth);
+//		setPD(-(getAlt() - home.getAlt()));
+	}
+	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(C_LED_PIN, LOW);
+				} else {
+					digitalWrite(C_LED_PIN, HIGH);
+				}
+				_hal->gps->valid_read = false;
+			}
+			break;
+
+		default:
+			digitalWrite(C_LED_PIN, LOW);
+			break;
+		}
+	}
+
+
+};
+
+} // namespace apo
+
+#endif // AP_Navigator_H
+// vim:ts=4:sw=4:expandtab

libraries/APO/AP_RcChannel.cpp

@@ -0,0 +1,109 @@
+/*
+	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 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, const bool & reverse) :
+		AP_Var_group(key,name),
+		_rc(rc),
+		_ch(this,0,ch,PSTR("CH")),
+		_pwmMin(this,1,pwmMin,PSTR("PMIN")),
+		_pwmMax(this,2,pwmMax,PSTR("PMAX")),
+		_pwmNeutral(this,3,pwmNeutral,PSTR("PNTRL")),
+		_rcMode(rcMode),
+		_reverse(this,4,reverse,PSTR("REV")),
+		_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::readRadio() {
+	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::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 > _pwmMax) _pwm = _pwmMax;
+	if (_pwm < _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 > _pwmMax) pwm = _pwmMax;
+	if (pwm < _pwmMin) pwm = _pwmMin;
+	return pwm;
+}
+
+float
+AP_RcChannel::_pwmToPosition(const uint16_t & pwm)
+{
+	float position;
+	if(pwm < _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

@@ -3,13 +3,21 @@
 /// @file	AP_RcChannel.h
 /// @brief	AP_RcChannel manager
 
-#ifndef AP_RcChannel_h
-#define AP_RcChannel_h
+#ifndef AP_RCCHANNEL_H
+#define AP_RCCHANNEL_H
 
 #include <stdint.h>
-#include <APM_RC.h>
-#include <AP_Common.h>
-#include <AP_Var.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
@@ -17,39 +25,32 @@ 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 float & scale=45.0, const float & center=0.0, 
-			const uint16_t & pwmMin=1200, 
-			const uint16_t & pwmNeutral=1500, const uint16_t & pwmMax=1800,
-			const uint16_t & pwmDeadZone=10,
-			const bool & filter=false, const bool & reverse=false);
+			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_Float scale;
-	AP_Float center;
-	AP_Uint16 pwmMin;
-	AP_Uint16 pwmNeutral;
-	AP_Uint16 pwmMax;
-	AP_Uint16 pwmDeadZone;
-	AP_Bool filter;
-	AP_Bool reverse;
+	AP_Uint8 _ch;
+	AP_Uint16 _pwmMin;
+	AP_Uint16 _pwmNeutral;
+	AP_Uint16 _pwmMax;
+	rcMode_t _rcMode;
+	AP_Bool _reverse;
+
 
 	// set
 	uint16_t readRadio();
 	void setPwm(uint16_t pwm);
 	void setPosition(float position);
-	void setNormalized(float normPosition);
-	void mixRadio(uint16_t infStart);
 
 	// get
 	uint16_t getPwm() { return _pwm; }
 	float getPosition() { return _pwmToPosition(_pwm); }
-	float getNormalized() { return getPosition()/scale; }
 
 	// did our read come in 50µs below the min?
-	bool failSafe() { _pwm < (pwmMin - 50); }
+	bool failSafe() { _pwm < (_pwmMin - 50); }
 
 private:
 
@@ -65,4 +66,6 @@ private:
 	float _pwmToPosition(const uint16_t & pwm);
 };
 
-#endif	
+} // apo
+
+#endif	// AP_RCCHANNEL_H

libraries/APO/AP_Var_keys.h

@@ -0,0 +1,41 @@
+#ifndef AP_Var_keys_H
+#define AP_Var_keys_H
+
+enum keys {
+
+	// general
+	k_config = 0,
+	k_cntrl,
+	k_guide,
+	k_sensorCalib,
+
+	// radio channels
+	k_chMode = 20,
+	k_chLeft,
+	k_chRight,
+	k_chFront,
+	k_chBack,
+	k_chRoll,
+	k_chPitch,
+	k_chYaw,
+	k_chThr,
+	k_chStr,
+
+	// pids
+	k_pidPN = 40,
+	k_pidPE,
+	k_pidPD,
+	k_pidThr,
+	k_pidStr,
+	k_pidRoll,
+	k_pidPitch,
+	k_pidYawRate,
+	k_pidYaw,
+
+	// 200-256 reserved for commands
+	k_commands = 200
+};
+
+// max 256 keys
+
+#endif

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/controllers.h

@@ -0,0 +1,277 @@
+#ifndef defaultControllers_H
+#define defaultControllers_H
+
+#include "AP_Controller.h"
+#include "AP_HardwareAbstractionLayer.h"
+#include "../AP_Common/AP_Var.h"
+#include <avr/pgmspace.h>
+#include "AP_Navigator.h"
+
+namespace apo {
+
+class CarController: public AP_Controller {
+private:
+	// control mode
+	AP_Var_group _group;AP_Uint8 _mode;
+	enum {
+		CH_MODE = 0, CH_STR, CH_THR
+	};
+	PidDFB2 pidStr;
+	Pid2 pidThr;
+public:
+	CarController(AP_Var::Key cntrlKey, AP_Var::Key pidStrKey,
+			AP_Var::Key pidThrKey, AP_Navigator * nav, AP_Guide * guide,
+			AP_HardwareAbstractionLayer * hal) :
+		AP_Controller(nav, guide, hal), _group(cntrlKey, PSTR("CNTRL_")),
+				_mode(&_group, 1, 0, PSTR("MODE")),
+				pidStr(pidStrKey, PSTR("STR_"), 1.0, 0, 0, 0, 3),
+				pidThr(pidThrKey, PSTR("THR_"), 0.6, 0.5, 0, 1, 3) {
+		_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 void update(const float & dt) {
+		// read mode switch
+		_hal->rc[CH_MODE]->setPwm(_hal->rc[CH_MODE]->readRadio());
+		// manual
+		if (_hal->rc[CH_MODE]->getPosition() > 0) {
+			_hal->rc[CH_STR]->setPwm(_hal->rc[CH_STR]->readRadio());
+			_hal->rc[CH_THR]->setPwm(_hal->rc[CH_THR]->readRadio());
+			//_hal->debug->println("manual");
+
+		} else { // auto
+			float headingError = _guide->headingCommand - _nav->getHeading();
+			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->groundSpeedCommand - _nav->getGroundSpeed(),dt));
+			//_hal->debug->println("automode");
+		}
+	}
+};
+
+#if CUSTOM_INCLUDES == CUSTOM_MIKROKOPTER
+#include "mikrokopter.h"
+#endif
+
+class QuadController: 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 autoChannel_t {
+		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
+	};
+
+	QuadController(AP_Navigator * nav, AP_Guide * guide,
+			AP_HardwareAbstractionLayer * hal) :
+				AP_Controller(nav, guide, hal),
+				pidRoll(k_pidRoll, PSTR("ROLL_"), PID_ATT_P, PID_ATT_I,
+						PID_ATT_D, PID_ATT_AWU, PID_ATT_LIM),
+				pidPitch(k_pidPitch, PSTR("PITCH_"), PID_ATT_P, PID_ATT_I,
+						PID_ATT_D, PID_ATT_AWU, PID_ATT_LIM),
+				pidYaw(k_pidYaw, PSTR("YAW_"), PID_YAWPOS_P, PID_YAWPOS_I,
+						PID_YAWPOS_D, PID_YAWPOS_AWU, PID_YAWPOS_LIM),
+				pidYawRate(k_pidYawRate, PSTR("YAWRATE_"), PID_YAWSPEED_P,
+						PID_YAWSPEED_I, PID_YAWSPEED_D, PID_YAWSPEED_AWU,
+						PID_YAWSPEED_LIM),
+				pidPN(k_pidPN, PSTR("NORTH_"), PID_POS_P,
+						PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM),
+				pidPE(k_pidPE, PSTR("EAST_"), PID_POS_P, PID_POS_I,
+						PID_POS_D, PID_POS_AWU, PID_POS_LIM),
+				pidPD(k_pidPD, PSTR("DOWN_"), 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, 7, 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( // -1 -> 0 maps to 1200, linear 0-1 -> 1200-1800
+				new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 3, 1100, 1100, 1900, RC_MODE_IN));
+	}
+
+	virtual void update(const float & dt) {
+
+			if (commLost()) {
+				_mode = MAV_MODE_FAILSAFE;
+				_hal->rc[CH_LEFT]->setPosition(0);
+				_hal->rc[CH_RIGHT]->setPosition(0);
+				_hal->rc[CH_FRONT]->setPosition(0);
+				_hal->rc[CH_BACK]->setPosition(0);
+			}
+
+			// read and set pwm so they can be read as positions later
+			_hal->rc[CH_MODE]->setPwm(_hal->rc[CH_MODE]->readRadio());
+			_hal->rc[CH_ROLL]->setPwm(_hal->rc[CH_ROLL]->readRadio());
+			_hal->rc[CH_PITCH]->setPwm(_hal->rc[CH_PITCH]->readRadio());
+			_hal->rc[CH_YAW]->setPwm(_hal->rc[CH_YAW]->readRadio());
+			_hal->rc[CH_THRUST]->setPwm(_hal->rc[CH_THRUST]->readRadio());
+
+			// manual mode
+			float mixRemoteWeight = 0;
+			if (_hal->rc[CH_MODE]->getPwm() > 1350) mixRemoteWeight = 1;
+
+			// "mix manual"
+			float cmdRoll = _hal->rc[CH_ROLL]->getPosition() * mixRemoteWeight;
+			float cmdPitch = _hal->rc[CH_PITCH]->getPosition() * mixRemoteWeight;
+			float cmdYawRate = _hal->rc[CH_YAW]->getPosition() * mixRemoteWeight;
+			float thrustMix = _hal->rc[CH_THRUST]->getPosition() * mixRemoteWeight;
+
+			// 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
+			float rollMix = pidRoll.update(cmdRoll - _nav->getRoll(),_nav->getRollRate(),dt);
+			float pitchMix = pidPitch.update(cmdPitch - _nav->getPitch(),_nav->getPitchRate(),dt);
+			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("thrust pwm: %d\n",_hal->rc[CH_THRUST]->readRadio());
+		}
+
+private:
+	PidDFB2 pidRoll, pidPitch, pidYaw, pidPN, pidPE, pidPD;
+	Pid2 pidYawRate;
+};
+
+/*
+ class PlaneController : public AP_Controller
+ {
+ private:
+ // state
+ AP_Float roll;
+ AP_Float airspeed;
+ AP_Float velocity;
+ AP_Float heading;
+
+ // servo positions
+ AP_Float steering;
+ AP_Float throttle;
+
+ // control variables
+ AP_Float headingCommand;
+ AP_Float airspeedCommand;
+ AP_Float rollCommand;
+
+ // channels
+ static const uint8_t chRoll = 0;
+ static const uint8_t chPitch = 1;
+ static const uint8_t chYaw = 2;
+
+ public:
+ PlaneController(AP_Var::Key chRollKey, AP_Var::Key chPitchKey, AP_Var::Key chYawKey,
+ AP_Var::Key pidRollKey, AP_Var::Key pidPitchKey, AP_Var::Key pidYawKey) : {
+ // rc channels
+ addCh(new AP_RcChannelSimple(chRollKey,PSTR("ROLL"),APM_RC,chRoll,45));
+ addCh(new AP_RcChannelSimple(chPitchKey,PSTR("PTCH"),APM_RC,chPitch,45));
+ addCh(new AP_RcChannelSimple(chYawKey,PSTR("YAW"),APM_RC,chYaw,45));
+
+ // pitch control loop
+ #if AIRSPEED_SENSOR == ENABLED
+ // pitch control loop w/ airspeed
+ addBlock(new SumGain(airspeedCommand,AP_Float_unity,airspeed,AP_Float_negative_unity));
+ #else
+ // cross feed variables
+ addBlock(new SumGain(roll,kffPitchCompk,throttleServo,kffT2P));
+ #endif
+ addBlock(new Pid(pidPitchKey,PSTR("PTCH"),0.1,0,0,1,20));
+ addBlock(new ToServo(getRc(chPitch)));
+
+ // roll control loop
+ addBlock(new SumGain(headingCommand,one,heading,negOne));
+ addBlock(new Pid(headingkP,headingKI,headingKD));
+ addBlock(new Sink(rollCommand));
+ addBlock(new SumGain(rollCommand,one,roll,negOne));
+ addBlock(new Pid(rollKP,rollKI,rollKD));
+ addBlock(new ToServo(getRc(chRoll)));
+
+ // throttle control loop
+ addBlock(new SumGain(airspeedCommand,one,airspeed,negOne));
+ addBlock(new Pid(throttleKP,throttleKI,throttleKD));
+ addBlock(new ToServo(getRc(chThr)));
+ }
+ };
+ */
+
+} // namespace apo
+
+#endif // defaultControllers_H
+// vim:ts=4:sw=4:expandtab

libraries/APO/examples/Manual/Manual.pde

@@ -7,6 +7,7 @@
 
 #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>
@@ -15,6 +16,8 @@ FastSerialPort0(Serial); // make sure this proceeds variable declarations
 
 // test settings
 
+using namespace apo;
+
 class RadioTest
 {
 private:
@@ -36,14 +39,14 @@ private:
 public:
 	RadioTest() :  testPosition(2), testSign(1)
 	{
-		ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,45));
-		ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,45));
-		ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,100));
-		ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,45));
-		ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1));
-		ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1));
-		ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1));
-		ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1));
+		ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,1100,1500,1900));
+		ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,1100,1500,1900));
+		ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,1100,1500,1900));
+		ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,1100,1500,1900));
+		ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1100,1500,1900));
+		ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1100,1500,1900));
+		ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1100,1500,1900));
+		ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1100,1500,1900));
 
 		Serial.begin(115200);
 		delay(2000);

libraries/APO/examples/ServoSweep/ServoSweep.pde

@@ -7,6 +7,7 @@
 
 #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>
@@ -15,6 +16,8 @@ FastSerialPort0(Serial); // make sure this proceeds variable declarations
 
 // test settings
 
+using namespace apo;
+
 class RadioTest
 {
 private:
@@ -36,14 +39,14 @@ private:
 public:
     RadioTest() :  testPosition(2), testSign(1)
     {
-        ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,45));
-        ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,45));
-        ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,100));
-        ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,45));
-        ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1));
-        ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1));
-        ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1));
-        ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1));
+        ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,1100,1500,1900));
+        ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,1100,1500,1900));
+        ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,1100,1500,1900));
+        ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,1100,1500,1900));
+        ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1100,1500,1900));
+        ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1100,1500,1900));
+        ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1100,1500,1900));
+        ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1100,1500,1900));
 
         Serial.begin(115200);
         delay(2000);
@@ -69,7 +72,7 @@ public:
         }
 
         // set channel positions
-        for (int i=0;i<ch.getSize();i++) ch[i]->setNormalized(testPosition);
+        for (int i=0;i<ch.getSize();i++) ch[i]->setPosition(testPosition);
 
         // print test position
         Serial.printf("\nnormalized position (%f)\n",testPosition);

libraries/APO/mikrokopter.h

@@ -0,0 +1,56 @@
+/*
+ * mikrokopter.h
+ *
+ *  Created on: Apr 6, 2011
+ *      Author: nkgentry
+ */
+
+#ifndef MIKROKOPTER_H_
+#define MIKROKOPTER_H_
+
+#define VEHICLE_MIKROKOPTER
+
+//motor parameters
+#define MOTOR_MAX  1
+#define MOTOR_MIN  0.1
+
+// position control loop
+#define PID_POS_INTERVAL  1/5 // 5 hz
+#define PID_POS_P 0.02
+#define PID_POS_I 0
+#define PID_POS_D 0.1
+#define PID_POS_LIM 0.1 // about 5 deg
+#define PID_POS_AWU 0.0 // about 5 deg
+#define PID_POS_Z_P 0.5
+#define PID_POS_Z_I 0.2
+#define PID_POS_Z_D 0.5
+#define PID_POS_Z_LIM 0.5
+#define PID_POS_Z_AWU 0.1
+
+// attitude control loop
+#define PID_ATT_INTERVAL  1/20 // 20 hz
+#define PID_ATT_P 0.03 // 0.1
+#define PID_ATT_I 0.03 // 0.0
+#define PID_ATT_D 0.03 // 0.1
+#define PID_ATT_LIM 0.1 // 0.01 // 10 % #define MOTORs
+#define PID_ATT_AWU 0.1 // 0.0
+#define PID_YAWPOS_P 1
+#define PID_YAWPOS_I 0.1
+#define PID_YAWPOS_D 0
+#define PID_YAWPOS_LIM 1 // 1 rad/s
+#define PID_YAWPOS_AWU 1 // 1 rad/s
+#define PID_YAWSPEED_P 1
+#define PID_YAWSPEED_I 0
+#define PID_YAWSPEED_D 0.5
+#define PID_YAWSPEED_LIM 0.1 // 0.01 // 10 % #define MOTORs
+#define PID_YAWSPEED_AWU 0.0
+
+// mixing
+#define MIX_REMOTE_WEIGHT  1
+#define MIX_POSITION_WEIGHT 1
+#define MIX_POSITION_Z_WEIGHT 1
+#define MIX_POSITION_YAW_WEIGHT  1
+
+#define THRUST_HOVER_OFFSET  0.475
+
+#endif /* MIKROKOPTER_H_ */

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_Compass/AP_Compass_HMC5843.h

@@ -1,8 +1,8 @@
 #ifndef AP_Compass_HMC5843_H
 #define AP_Compass_HMC5843_H
 
-#include <AP_Common.h>
-#include <AP_Math.h>
+#include "../AP_Common/AP_Common.h"
+#include "../AP_Math/AP_Math.h"
 
 #include "Compass.h"
 

libraries/AP_Compass/Compass.h

@@ -2,8 +2,8 @@
 #define Compass_h
 
 #include <inttypes.h>
-#include <AP_Common.h>
-#include <AP_Math.h>
+#include "../AP_Common/AP_Common.h"
+#include "../AP_Math/AP_Math.h"
 
 // standard rotation matrices
 #define ROTATION_NONE               Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)

libraries/AP_DCM/AP_DCM.h

@@ -7,14 +7,14 @@
 // header file
 #include "AP_DCM_HIL.h"
 
-#include <FastSerial.h>
-#include <AP_Math.h>
+#include "../FastSerial/FastSerial.h"
+#include "../AP_Math/AP_Math.h"
 #include <inttypes.h>
 #include "WProgram.h"
-#include <AP_Compass.h>
-#include <AP_ADC.h>
-#include <AP_GPS.h>
-#include <AP_IMU.h>
+#include "../AP_Compass/AP_Compass.h"
+#include "../AP_ADC/AP_ADC.h"
+#include "../AP_GPS/AP_GPS.h"
+#include "../AP_IMU/AP_IMU.h"
 
 
 class AP_DCM

libraries/AP_DCM/AP_DCM_HIL.h

@@ -1,14 +1,14 @@
 #ifndef AP_DCM_HIL_H
 #define AP_DCM_HIL_H
 
-#include <FastSerial.h>
-#include <AP_Math.h>
+#include "../FastSerial/FastSerial.h"
+#include "../AP_Math/AP_Math.h"
 #include <inttypes.h>
 #include "WProgram.h"
-#include <AP_Compass.h>
-#include <AP_ADC.h>
-#include <AP_GPS.h>
-#include <AP_IMU.h>
+#include "../AP_Compass/AP_Compass.h"
+#include "../AP_ADC/AP_ADC.h"
+#include "../AP_GPS/AP_GPS.h"
+#include "../AP_IMU/AP_IMU.h"
 
 
 class AP_DCM_HIL

libraries/AP_GPS/AP_GPS_Auto.h

@@ -6,8 +6,8 @@
 #ifndef AP_GPS_Auto_h
 #define AP_GPS_Auto_h
 
-#include <FastSerial.h>
-#include <GPS.h>
+#include "../FastSerial/FastSerial.h"
+#include "GPS.h"
 
 class AP_GPS_Auto : public GPS
 {

libraries/AP_GPS/AP_GPS_HIL.h

@@ -12,7 +12,7 @@
 #ifndef AP_GPS_HIL_h
 #define AP_GPS_HIL_h
 
-#include <GPS.h>
+#include "GPS.h"
 
 class AP_GPS_HIL : public GPS {
 public:

libraries/AP_GPS/AP_GPS_IMU.h

@@ -3,7 +3,7 @@
 #ifndef AP_GPS_IMU_h
 #define AP_GPS_IMU_h
 
-#include <GPS.h>
+#include "GPS.h"
 #define MAXPAYLOAD 32
 
 class AP_GPS_IMU : public GPS {

libraries/AP_GPS/AP_GPS_MTK.h

@@ -15,7 +15,7 @@
 #ifndef AP_GPS_MTK_h
 #define AP_GPS_MTK_h
 
-#include <GPS.h>
+#include "GPS.h"
 #include "AP_GPS_MTK_Common.h"
 
 class AP_GPS_MTK : public GPS {

libraries/AP_GPS/AP_GPS_MTK16.h

@@ -13,7 +13,7 @@
 #ifndef AP_GPS_MTK16_h
 #define AP_GPS_MTK16_h
 
-#include <GPS.h>
+#include "GPS.h"
 #include "AP_GPS_MTK_Common.h"
 
 class AP_GPS_MTK16 : public GPS {

libraries/AP_GPS/AP_GPS_NMEA.h

@@ -42,7 +42,7 @@
 #ifndef AP_GPS_NMEA_h
 #define AP_GPS_NMEA_h
 
-#include <GPS.h>
+#include "GPS.h"
 #include <avr/pgmspace.h>
 
 

libraries/AP_GPS/AP_GPS_None.h

@@ -3,7 +3,7 @@
 #ifndef AP_GPS_None_h
 #define AP_GPS_None_h
 
-#include <GPS.h>
+#include "GPS.h"
 
 class AP_GPS_None : public GPS
 {

libraries/AP_GPS/AP_GPS_SIRF.h

@@ -11,7 +11,7 @@
 #ifndef AP_GPS_SIRF_h
 #define AP_GPS_SIRF_h
 
-#include <GPS.h>
+#include "GPS.h"
 
 #define SIRF_SET_BINARY	"$PSRF100,0,38400,8,1,0*3C"
 

libraries/AP_GPS/AP_GPS_Shim.h

@@ -12,7 +12,7 @@
 #ifndef AP_GPS_Shim_h
 #define AP_GPS_Shim_h
 
-#include <GPS.h>
+#include "GPS.h"
 
 class AP_GPS_Shim : public GPS
 {

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -11,7 +11,7 @@
 #ifndef AP_GPS_UBLOX_h
 #define AP_GPS_UBLOX_h
 
-#include <GPS.h>
+#include "GPS.h"
 
 #define UBLOX_SET_BINARY	"$PUBX,41,1,0003,0001,38400,0*26"
 

libraries/AP_IMU/AP_IMU_Oilpan.h

@@ -7,9 +7,9 @@
 #define AP_IMU_Oilpan_h
 
 
-#include <AP_Common.h>
-#include <AP_Math.h>
-#include <AP_ADC.h>
+#include "../AP_Common/AP_Common.h"
+#include "../AP_Math/AP_Math.h"
+#include "../AP_ADC/AP_ADC.h"
 #include <inttypes.h>
 
 #include "IMU.h"

libraries/AP_IMU/IMU.h

@@ -7,7 +7,7 @@
 #ifndef IMU_h
 #define IMU_h
 
-#include <AP_Math.h>
+#include "../AP_Math/AP_Math.h"
 #include <inttypes.h>
 
 class IMU

libraries/AP_RcChannel/AP_RcChannel.cpp

@@ -1,128 +0,0 @@
-/*
-	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.h>
-
-AP_RcChannel::AP_RcChannel(AP_Var::Key key, const prog_char_t * name, APM_RC_Class & rc, const uint8_t & ch,
-			const float & scale, const float & center, 
-			const uint16_t & pwmMin, 
-			const uint16_t & pwmNeutral, const uint16_t & pwmMax,
-			const uint16_t & pwmDeadZone,
-			const bool & filter, const bool & reverse) :
-		AP_Var_group(key,name),
-		_rc(rc),
-		ch(this,0,ch,PSTR("CH")),
-		scale(this,1,scale,PSTR("SCALE")),
-		center(this,2,center,PSTR("CENTER")),
-		pwmMin(this,3,pwmMin,PSTR("PMIN")),
-		pwmMax(this,4,pwmMax,PSTR("PMAX")),
-		pwmNeutral(this,5,pwmNeutral,PSTR("PNTRL")),
-		pwmDeadZone(this,6,pwmDeadZone,PSTR("PDEAD")),
-		filter(this,7,filter,PSTR("FLTR")),
-		reverse(this,8,reverse,PSTR("REV")),
-		_pwm(0)
-	{
-		setNormalized(0.0);
-	}
-
-
-uint16_t AP_RcChannel::readRadio() {
-	return _rc.InputCh(ch);
-}
-
-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 filter
-	if(filter){
-		if(_pwm == 0)
-			_pwm = pwm;
-		else
-			_pwm = ((pwm + _pwm) >> 1);		// Small filtering
-	}else{
-		_pwm = pwm;
-	}
-
-	//Serial.printf("pwm after filter: %d\n", _pwm);
-
-	// apply deadzone
-	_pwm = (abs(_pwm - pwmNeutral) < pwmDeadZone) ? uint16_t(pwmNeutral) : _pwm;
-
-	//Serial.printf("pwm after deadzone: %d\n", _pwm);
-	_rc.OutputCh(ch,_pwm);
-}
-
-void
-AP_RcChannel::setPosition(float position)
-{
-	if (position > scale) position = scale;
-	else if (position < -scale) position = -scale;
-	setPwm(_positionToPwm(position));
-}
-
-void
-AP_RcChannel::setNormalized(float normPosition)
-{
-	setPosition(normPosition*scale);
-}
-
-void
-AP_RcChannel::mixRadio(uint16_t infStart)
-{
-	uint16_t pwmRadio = _rc.InputCh(ch);
-	float inf = abs( int16_t(pwmRadio - pwmNeutral) );
-	inf = min(inf, infStart);
-	inf = ((infStart - inf) /infStart);
-	setPwm(_pwm*inf + pwmRadio); 
-}
-
-uint16_t
-AP_RcChannel::_positionToPwm(const float & position)
-{
-	uint16_t pwm;
-	//Serial.printf("position: %f\n", position);
-	float p = position - center;	
-	if(p < 0)
-		pwm = p * int16_t(pwmNeutral - pwmMin) / 
-			scale + pwmNeutral;
-	else
-		pwm = p * int16_t(pwmMax - pwmNeutral) / 
-			scale + pwmNeutral;
-	constrain(pwm,uint16_t(pwmMin),uint16_t(pwmMax));
-	return pwm;
-}
-
-float
-AP_RcChannel::_pwmToPosition(const uint16_t & pwm)
-{
-	float position;
-	if(pwm < pwmNeutral)
-		position = scale * int16_t(pwm - pwmNeutral)/ 
-			int16_t(pwmNeutral - pwmMin) + center;
-	else
-		position = scale * int16_t(pwm - pwmNeutral)/ 
-			int16_t(pwmMax - pwmNeutral) + center;
-	constrain(position,center-scale,center+scale);
-	return position;
-}
-
-// ------------------------------------------

libraries/FastSerial/BetterStream.h

@@ -13,7 +13,7 @@
 
 #include <Stream.h>
 #include <avr/pgmspace.h>
-#include <AP_Common.h>
+#include "../AP_Common/AP_Common.h"
 
 class BetterStream : public Stream {
 public: