.gitignore

+.metadata/
+Tools/ArdupilotMegaPlanner/bin/Release/logs/
+ArduCopter/Debug/
+config.mk
+serialsent.raw
+CMakeFiles
+CMakeCache.txt
+cmake_install.cmake
+build
+/Tools/ArdupilotMegaPlanner/bin/Release/gmapcache
+/Tools/ArdupilotMegaPlanner/APMPlannerXplanes/APMPlannerXplanes/Debug
+/Tools/ArdupilotMegaPlanner/CustomImages
+/Tools/ArdupilotMegaPlanner/Updater/obj
+/Tools/ArdupilotMegaPlanner/Updater/bin
+/Tools/ArdupilotMegaPlanner/bin/Debug
+/Tools/ArdupilotMegaPlanner/bin/APM Planner.app
+/Tools/ArdupilotMegaPlanner/obj
+/Tools/ArdupilotMegaPlanner/resedit/bin
+/Tools/ArdupilotMegaPlanner/resedit/obj
+/Tools/ArdupilotMegaPlanner/wix/bin
+/Tools/ArdupilotMegaPlanner/wix/obj
+tags
+*.o
+.*.swp
+.*.swo
+mav.log
+mav.log.raw
+status.txt
+
+/Tools/ArdupilotMegaPlanner/Msi/upload.bat
\ No newline at end of file

.project

+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>ArduPilotMega-Source@ardupilotone</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+	</buildSpec>
+	<natures>
+	</natures>
+</projectDescription>

ArduBoat/ArduBoat.pde

+// Libraries
+#include <Wire.h>
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <APM_RC.h>
+#include <AP_RangeFinder.h>
+#include <GCS_MAVLink.h>
+#include <AP_ADC.h>
+#include <AP_DCM.h>
+#include <AP_Compass.h>
+#include <Wire.h>
+#include <AP_GPS.h>
+#include <AP_IMU.h>
+#include <APM_BMP085.h>
+#include <ModeFilter.h>
+#include <APO.h>
+#include <AP_AnalogSource.h>
+#include <AP_InertialSensor.h>
+
+// Vehicle Configuration
+//#include "BoatGeneric.h"
+#include "SailboatLaser.h"
+
+// ArduPilotOne Default Setup
+#include "APO_DefaultSetup.h"

ArduBoat/BoatGeneric.h

+/*
+ * BoatGeneric.h
+ *
+ *  Created on: May 1, 2011
+ *      Author: jgoppert
+ *
+ */
+
+#ifndef BOATGENERIC_H_
+#define BOATGENERIC_H_
+
+using namespace apo;
+
+// vehicle options
+static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass;
+static const MAV_TYPE vehicle = UGV_SURFACE_SHIP; 
+static const apo::halMode_t halMode = apo::MODE_LIVE;
+//static const apo::AP_Board::mode_e = apo::AP_Board::MODE_HIL_CNTL;
+static const apo::AP_Board::mode_e = apo::AP_Board::MODE_LIVE;
+static const uint8_t heartBeatTimeout = 0;
+
+// algorithm selection
+#define CONTROLLER_CLASS ControllerBoat
+#define GUIDE_CLASS MavlinkGuide
+#define NAVIGATOR_CLASS Navigator_Dcm
+
+// hardware selection
+//#define BOARD_TYPE Board_APM1
+//#define BOARD_TYPE Board_APM1_2560
+#define BOARD_TYPE Board_APM2
+
+// loop rates
+static const float loopRate = 150; // attitude nav
+static const float loop0Rate = 50; // controller
+static const float loop1Rate = 5; 	// pos nav/ gcs fast
+static const float loop2Rate = 1; 	// gcs slow
+static const float loop3Rate = 0.1;
+
+// controller
+static const bool useForwardReverseSwitch = true;
+
+// gains
+static const float steeringP = 0.1;
+static const float steeringI = 0.0;
+static const float steeringD = 0.1;
+static const float steeringIMax = 0.0;
+static const float steeringYMax = 1;
+static const float steeringDFCut = 25.0;
+
+static const float throttleP = 0.1;
+static const float throttleI = 0.0;
+static const float throttleD = 0.0;
+static const float throttleIMax = 0.0;
+static const float throttleYMax = 1;
+static const float throttleDFCut = 0.0;
+
+// guidance
+static const float velCmd = 5;
+static const float xt = 10;
+static const float xtLim = 90;
+
+#include "ControllerBoat.h"
+
+#endif /* BOATGENERIC_H_ */
+// vim:ts=4:sw=4:expandtab

ArduBoat/ControllerBoat.h

+/*
+ * ControllerBoat.h
+ *
+ *  Created on: Jun 30, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef CONTROLLERBOAT_H_
+#define CONTROLLERBOAT_H_
+
+#include "../APO/AP_Controller.h"
+
+namespace apo {
+
+class ControllerBoat: public AP_Controller {
+public:
+    ControllerBoat(AP_Navigator * nav, AP_Guide * guide,
+                  AP_Board * board) :
+        AP_Controller(nav, guide, board,new AP_ArmingMechanism(board,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl),
+        pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
+               steeringI, steeringD, steeringIMax, steeringYMax),
+        pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
+                  throttleI, throttleD, throttleIMax, throttleYMax,
+                  throttleDFCut), _strCmd(0), _thrustCmd(0),
+      _rangeFinderFront()
+    {
+        _board->debug->println_P(PSTR("initializing boat controller"));
+
+        _board->rc.push_back(
+            new AP_RcChannel(k_chMode, PSTR("MODE_"), board->radio, 5, 1100,
+                             1500, 1900, RC_MODE_IN, false));
+        _board->rc.push_back(
+            new AP_RcChannel(k_chStr, PSTR("STR_"), board->radio, 3, 1100, 1500,
+                             1900, RC_MODE_INOUT, false));
+        _board->rc.push_back(
+            new AP_RcChannel(k_chThrust, PSTR("THR_"), board->radio, 2, 1100, 1500,
+                             1900, RC_MODE_INOUT, false));
+        _board->rc.push_back(
+            new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), board->radio, 4, 1100, 1500,
+                             1900, RC_MODE_IN, false));
+
+        for (uint8_t i = 0; i < _board->rangeFinders.getSize(); i++) {
+            RangeFinder * rF = _board->rangeFinders[i];
+            if (rF == NULL)
+                continue;
+            if (rF->orientation_x == 1 && rF->orientation_y == 0
+                    && rF->orientation_z == 0)
+                _rangeFinderFront = rF;
+        }
+    }
+
+private:
+    // methods
+    void manualLoop(const float dt) {
+        _strCmd = _board->rc[ch_str]->getRadioPosition();
+        _thrustCmd = _board->rc[ch_thrust]->getRadioPosition();
+        if (useForwardReverseSwitch && _board->rc[ch_FwdRev]->getRadioPosition() < 0.0)
+            _thrustCmd = -_thrustCmd;
+    }
+    void autoLoop(const float dt) {
+        //_board->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_board->rc[ch_thrust]->getRadioPosition(), _board->rc[ch_str]->getRadioPosition());
+        // neglects heading command derivative
+        float steering = pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt);
+        float thrust = pidThrust.update(
+                         _guide->getGroundSpeedCommand()
+                         - _nav->getGroundSpeed(), dt);
+
+        // obstacle avoidance overrides
+        // try to drive around the obstacle in front. if this fails, stop
+        if (_rangeFinderFront) {
+            _rangeFinderFront->read();
+
+            int distanceToObstacle = _rangeFinderFront->distance;
+            if (distanceToObstacle < 100) {
+                thrust = 0;   // Avoidance didn't work out. Stopping
+            }
+            else if (distanceToObstacle < 650) {
+                // Deviating from the course. Deviation angle is inverse proportional
+                // to the distance to the obstacle, with 15 degrees min angle, 180 - max
+                steering += (15 + 165 *
+                    (1 - ((float)(distanceToObstacle - 100)) / 550)) * deg2Rad;
+            }
+        }
+
+        _strCmd = steering;
+        _thrustCmd = thrust;
+    }
+    void setMotors() {
+        _board->rc[ch_str]->setPosition(_strCmd);
+        _board->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd);
+    }
+    void handleFailsafe() {
+        // turn off
+        setMode(MAV_MODE_LOCKED);
+    }
+
+    // attributes
+    enum {
+        ch_mode = 0, ch_str, ch_thrust, ch_FwdRev
+    };
+    enum {
+        k_chMode = k_radioChannelsStart, k_chStr, k_chThrust, k_chFwdRev
+    };
+    enum {
+        k_pidStr = k_controllersStart, k_pidThrust
+    };
+    BlockPIDDfb pidStr;
+    BlockPID pidThrust;
+    float _strCmd, _thrustCmd;
+
+    RangeFinder * _rangeFinderFront;
+};
+
+} // namespace apo
+
+#endif /* CONTROLLERBOAT_H_ */
+// vim:ts=4:sw=4:expandtab

ArduBoat/ControllerSailboat.h

+/*
+ * ControllerSailboat.h
+ *
+ *  Created on: Jun 30, 2011
+ *      Author: jgoppert
+ */
+
+#ifndef CONTROLLERSAILBOAT_H_
+#define CONTROLLERSAILBOAT_H_
+
+#include "../APO/AP_Controller.h"
+
+namespace apo {
+
+class ControllerSailboat: public AP_Controller {
+public:
+    ControllerSailboat(AP_Navigator * nav, AP_Guide * guide,
+                  AP_Board * board) :
+        AP_Controller(nav, guide, board,new AP_ArmingMechanism(board,this,ch_sail,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl),
+        pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
+               steeringI, steeringD, steeringIMax, steeringYMax),
+        pidSail(new AP_Var_group(k_pidSail, PSTR("SAIL_")), 1, throttleP,
+                  throttleI, throttleD, throttleIMax, throttleYMax,
+                  throttleDFCut), _strCmd(0), _sailCmd(0)
+    {
+        _board->debug->println_P(PSTR("initializing sailboat controller"));
+
+        _board->rc.push_back(
+            new AP_RcChannel(k_chMode, PSTR("MODE_"), board->radio, 5, 1100,
+                             1500, 1900, RC_MODE_IN, false));
+        _board->rc.push_back(
+            new AP_RcChannel(k_chStr, PSTR("STR_"), board->radio, 3, 1100, 1500,
+                             1900, RC_MODE_INOUT, false));
+        _board->rc.push_back(
+            new AP_RcChannel(k_chSail, PSTR("SAIL_"), board->radio, 2, 1100, 1500,
+                             1900, RC_MODE_INOUT, false));
+    }
+
+private:
+    // methods
+    void manualLoop(const float dt) {
+        _strCmd = -_board->rc[ch_str]->getRadioPosition();
+        _sailCmd = _board->rc[ch_sail]->getRadioPosition();
+        _board->debug->printf_P(PSTR("sail: %f, steering: %f\n"),_sailCmd,_strCmd);
+    }
+    void autoLoop(const float dt) {
+        //_board->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_board->rc[ch_sail]->getRadioPosition(), _board->rc[ch_str]->getRadioPosition());
+        float windDir = -.339373*analogRead(1)+175.999;       
+
+
+        // neglects heading command derivative
+        float steering = -pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt);
+        float sail = 0.00587302*fabs(windDir) - 0.05;
+        if (sail < 0.0) sail = 0.0;
+
+        //_board->debug->printf_P(PSTR("heading: %f\n"),heading);       //Print Heading 
+     
+        //if(fabs(psi)<45)                                    //Tacking Logic
+        //{
+            //if(psi<-10)
+                //alpha = -45;
+            //else if(psi>10)
+                //alpha = 45;
+            //else
+            //{
+                //if(psi==10)
+                    //alpha = 45;
+                //else if(psi==-10)
+                    //alpha = -45;
+                //else 
+                    //alpha = alpha;
+            //}
+        //}*/
+        _strCmd = steering;
+        _sailCmd = sail;
+    }
+    void setMotors() {
+        _board->rc[ch_str]->setPosition(_strCmd);
+        _board->rc[ch_sail]->setPosition(_sailCmd);
+    }
+    void handleFailsafe() {
+        // turn off
+        setMode(MAV_MODE_LOCKED);
+    }
+
+    // attributes
+    enum {
+        ch_mode = 0, ch_str, ch_sail
+    };
+    enum {
+        k_chMode = k_radioChannelsStart, k_chStr, k_chSail
+    };
+    enum {
+        k_pidStr = k_controllersStart, k_pidSail
+    };
+    BlockPIDDfb pidStr;
+    BlockPID pidSail;
+    float _strCmd, _sailCmd;
+};
+
+} // namespace apo
+
+#endif /* CONTROLLERSAILBOAT_H_ */
+// vim:ts=4:sw=4:expandtab

ArduBoat/Makefile

+include ../libraries/AP_Common/Arduino.mk

ArduBoat/SailboatLaser.h

+/*
+ * SailboatLaser.h
+ *
+ *  Created on: May 1, 2011
+ *      Author: jgoppert
+ *
+ */
+
+#ifndef SAILBOATLASER_H_
+#define SAILBOATLASER_H_
+
+using namespace apo;
+
+// vehicle options
+static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass;
+static const MAV_TYPE vehicle = UGV_SURFACE_SHIP; 
+static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_HIL_CNTL;
+//static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_LIVE;
+static const uint8_t heartBeatTimeout = 0;
+
+// algorithm selection
+#define CONTROLLER_CLASS ControllerSailboat
+#define GUIDE_CLASS MavlinkGuide
+#define NAVIGATOR_CLASS Navigator_Dcm
+
+// hardware selection
+#define BOARD_TYPE Board_APM1
+//#define BOARD_TYPE Board_APM1_2560
+//#define BOARD_TYPE Board_APM2
+
+// loop rates
+static const float loopRate = 150; // attitude nav
+static const float loop0Rate = 50; // controller
+static const float loop1Rate = 5; 	// pos nav/ gcs fast
+static const float loop2Rate = 1; 	// gcs slow
+static const float loop3Rate = 0.1;
+
+// gains
+static const float steeringP = 0.1;
+static const float steeringI = 0.0;
+static const float steeringD = 0.1;
+static const float steeringIMax = 0.0;
+static const float steeringYMax = 1;
+static const float steeringDFCut = 25.0;
+
+static const float throttleP = 0.1;
+static const float throttleI = 0.0;
+static const float throttleD = 0.2;
+static const float throttleIMax = 0.0;
+static const float throttleYMax = 1;
+static const float throttleDFCut = 0.0;
+
+// guidance
+static const float velCmd = 2;
+static const float xt = 10;
+static const float xtLim = 90;
+
+#include "ControllerSailboat.h"
+
+#endif /* SAILBOATLASER_H_ */
+// vim:ts=4:sw=4:expandtab

ArduCopter/.gitignore

+arducopter.cpp

ArduCopter/APM_Config.h

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// Example config file. Take a look at config.h. Any term define there can be overridden by defining it here.
+
+// # define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
+// # define APM2_BETA_HARDWARE
+
+// GPS is auto-selected
+
+//#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
+//#define HIL_MODE				HIL_MODE_ATTITUDE
+
+//#define FRAME_CONFIG QUAD_FRAME
+	/*
+	options:
+	QUAD_FRAME
+	TRI_FRAME
+	HEXA_FRAME
+	Y6_FRAME
+	OCTA_FRAME
+	OCTA_QUAD_FRAME
+	HELI_FRAME
+	*/
+
+//#define FRAME_ORIENTATION X_FRAME
+	/*
+	PLUS_FRAME
+	X_FRAME
+	V_FRAME
+	*/
+
+//# define CH7_OPTION		CH7_SAVE_WP
+	/*
+	CH7_DO_NOTHING
+	CH7_SET_HOVER
+	CH7_FLIP
+	CH7_SIMPLE_MODE
+	CH7_RTL
+	CH7_AUTO_TRIM
+	CH7_ADC_FILTER (experimental)
+	CH7_SAVE_WP
+	*/
+
+#define ACCEL_ALT_HOLD 0		// disabled by default, work in progress
+
+
+//#define RATE_ROLL_I 	0.18
+//#define RATE_PITCH_I	0.18
+//#define MOTORS_JD880
+//#define MOTORS_JD850
+
+
+// agmatthews USERHOOKS
+// the choice of function names is up to the user and does not have to match these
+// uncomment these hooks and ensure there is a matching function un your "UserCode.pde" file
+//#define USERHOOK_FASTLOOP userhook_FastLoop();
+#define USERHOOK_50HZLOOP userhook_50Hz();
+//#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();
+//#define USERHOOK_SLOWLOOP userhook_SlowLoop();
+//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();
+#define USERHOOK_INIT userhook_init();
+
+// the choice of includeed variables file (*.h) is up to the user and does not have to match this one
+// Ensure the defined file exists and is in the arducopter directory
+#define USERHOOK_VARIABLES "UserVariables.h"
+
+// to enable, set to 1
+// to disable, set to 0
+#define AUTO_THROTTLE_HOLD 1
+
+//# define LOGGING_ENABLED		DISABLED
+
+
+// Custom channel config - Expert Use Only.
+// this for defining your own MOT_n to CH_n mapping.
+// Overrides defaults (for APM1 or APM2) found in config_channels.h
+// MOT_n variables are used by the Frame mixing code. You must define
+// MOT_1 through MOT_m where m is the number of motors on your frame.
+// CH_n variables are used for RC output. These can be CH_1 through CH_8,
+// and CH_10 or CH_12. 
+// Sample channel config. Must define all MOT_ chanels used by
+// your FRAME_TYPE.
+// #define CONFIG_CHANNELS CHANNEL_CONFIG_CUSTOM
+// #define MOT_1 CH_6
+// #define MOT_2 CH_3
+// #define MOT_3 CH_2
+// #define MOT_4 CH_5
+// #define MOT_5 CH_1
+// #define MOT_6 CH_4
+// #define MOT_7 CH_7
+// #define MOT_8 CH_8

ArduCopter/APM_Config_mavlink_hil.h

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// HIL_MODE SELECTION
+//
+// Mavlink supports
+// 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude
+// 2. HIL_MODE_SENSORS: full sensor simulation
+#define HIL_MODE            HIL_MODE_ATTITUDE
+
+// HIL_PORT SELCTION
+//
+// PORT 1
+// If you would like to run telemetry communications for a groundstation
+// while you are running hardware in the loop it is necessary to set
+// HIL_PORT to 1. This uses the port that would have been used for the gps
+// as the hardware in the loop port. You will have to solder
+// headers onto the gps port connection on the apm
+// and connect via an ftdi cable.
+//
+// The baud rate is set to 115200 in this mode.
+//
+// PORT 3
+// If you don't require telemetry communication with a gcs while running
+// hardware in the loop you may use the telemetry port as the hardware in
+// the loop port.  Alternatively, use a telemetry/HIL shim like FGShim
+// https://ardupilot-mega.googlecode.com/svn/Tools/trunk/FlightGear
+//
+// The buad rate is controlled by SERIAL3_BAUD in this mode.
+
+#define HIL_PORT            3
+
+// You can set your gps protocol here for your actual
+// hardware and leave it without affecting the hardware
+// in the loop simulation
+#define GPS_PROTOCOL 		GPS_PROTOCOL_MTK
+
+// Sensors
+// All sensors are supported in all modes.
+// The magnetometer is not used in
+// HIL_MODE_ATTITUDE but you may leave it
+// enabled if you wish.
+#define MAGNETOMETER        ENABLED

ArduCopter/Attitude.pde

+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+static int16_t
+get_stabilize_roll(int32_t target_angle)
+{
+	// angle error
+	target_angle 		= wrap_180(target_angle - ahrs.roll_sensor);
+
+#if FRAME_CONFIG == HELI_FRAME
+
+	// limit the error we're feeding to the PID
+	target_angle 		= constrain(target_angle, -4500, 4500);
+
+	// convert to desired Rate:
+	target_angle 		= g.pi_stabilize_roll.get_pi(target_angle, G_Dt);
+
+	// output control:
+	return constrain(target_angle, -4500, 4500);
+#else
+
+	// limit the error we're feeding to the PID
+	target_angle 		= constrain(target_angle, -2500, 2500);
+
+	// convert to desired Rate:
+	int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle);
+	int16_t iterm 		= g.pi_stabilize_roll.get_i(target_angle, G_Dt);
+
+	return get_rate_roll(target_rate) + iterm;
+#endif
+}
+
+static int16_t
+get_stabilize_pitch(int32_t target_angle)
+{
+	// angle error
+	target_angle 		= wrap_180(target_angle - ahrs.pitch_sensor);
+
+#if FRAME_CONFIG == HELI_FRAME
+	// limit the error we're feeding to the PID
+	target_angle 		= constrain(target_angle, -4500, 4500);
+
+	// convert to desired Rate:
+	target_angle 		= g.pi_stabilize_pitch.get_pi(target_angle, G_Dt);
+
+	// output control:
+	return constrain(target_angle, -4500, 4500);
+#else
+	// limit the error we're feeding to the PID
+	target_angle 		= constrain(target_angle, -2500, 2500);
+
+	// conver to desired Rate:
+	int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle);
+	int16_t iterm 		= g.pi_stabilize_pitch.get_i(target_angle, G_Dt);
+
+	return get_rate_pitch(target_rate) + iterm;
+#endif
+}
+
+static int16_t
+get_stabilize_yaw(int32_t target_angle)
+{
+	static int8_t log_counter = 0;		// used to slow down logging of PID values to dataflash
+	int32_t target_rate,i_term;
+	int32_t angle_error;
+	int32_t output;
+
+	// angle error
+	angle_error	 	= wrap_180(target_angle - ahrs.yaw_sensor);
+
+	// limit the error we're feeding to the PID
+#if FRAME_CONFIG == HELI_FRAME
+	angle_error 		= constrain(angle_error, -4500, 4500);
+#else
+	angle_error 		= constrain(angle_error, -2000, 2000);
+#endif
+
+	// convert angle error to desired Rate:
+	target_rate = g.pi_stabilize_yaw.get_p(angle_error);
+	i_term = g.pi_stabilize_yaw.get_i(angle_error, G_Dt);
+
+	// do not use rate controllers for helicotpers with external gyros
+#if FRAME_CONFIG == HELI_FRAME
+	if(!motors.ext_gyro_enabled){
+		output = get_rate_yaw(target_rate) + i_term;
+	}else{
+		output = constrain((target_rate + i_term), -4500, 4500);
+	}
+#else
+	output = get_rate_yaw(target_rate) + i_term;
+#endif
+
+#if LOGGING_ENABLED == ENABLED
+	// log output if PID logging is on and we are tuning the yaw
+	if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) {
+		log_counter++;
+		if( log_counter >= 10 ) {	// (update rate / desired output rate) = (100hz / 10hz) = 10
+			log_counter = 0;
+			Log_Write_PID(CH6_YAW_KP, angle_error, target_rate, i_term, 0, output, tuning_value);
+		}
+	}
+#endif
+
+	// ensure output does not go beyond barries of what an int16_t can hold
+	return constrain(output,-32000,32000);
+}
+
+static int16_t
+get_acro_roll(int32_t target_rate)
+{
+	target_rate = target_rate * g.acro_p;
+	target_rate = constrain(target_rate, -10000, 10000);
+	return get_rate_roll(target_rate);
+}
+
+static int16_t
+get_acro_pitch(int32_t target_rate)
+{
+	target_rate = target_rate * g.acro_p;
+	target_rate = constrain(target_rate, -10000, 10000);
+	return get_rate_pitch(target_rate);
+}
+
+static int16_t
+get_acro_yaw(int32_t target_rate)
+{
+	target_rate = g.pi_stabilize_yaw.get_p(target_rate);
+	target_rate = constrain(target_rate, -15000, 15000);
+	return get_rate_yaw(target_rate);
+}
+
+static int16_t
+get_rate_roll(int32_t target_rate)
+{
+	static int8_t log_counter = 0;					// used to slow down logging of PID values to dataflash
+	static int32_t last_rate = 0;					// previous iterations rate
+	int32_t p,i,d;									// used to capture pid values for logging
+	int32_t current_rate;							// this iteration's rate
+	int32_t rate_error;								// simply target_rate - current_rate
+	int32_t rate_d;  								// roll's acceleration
+	int32_t output;									// output from pid controller
+	int32_t rate_d_dampener;						// value to dampen output based on acceleration
+
+	// get current rate
+	current_rate 	= (omega.x * DEGX100);
+
+	// calculate and filter the acceleration
+	rate_d 			= roll_rate_d_filter.apply(current_rate - last_rate);
+
+	// store rate for next iteration
+	last_rate 		= current_rate;
+
+	// call pid controller
+	rate_error	= target_rate - current_rate;
+	p 			= g.pid_rate_roll.get_p(rate_error);
+	i			= g.pid_rate_roll.get_i(rate_error, G_Dt);
+	d			= g.pid_rate_roll.get_d(rate_error, G_Dt);
+	output		= p + i + d;
+
+	// Dampening output with D term
+	rate_d_dampener = rate_d * roll_scale_d;
+	rate_d_dampener = constrain(rate_d_dampener, -400, 400);
+	output -= rate_d_dampener;
+
+	// constrain output
+	output = constrain(output, -2500, 2500);
+
+#if LOGGING_ENABLED == ENABLED
+	// log output if PID logging is on and we are tuning the rate P, I or D gains
+	if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) {
+		log_counter++;
+		if( log_counter >= 10 ) {	// (update rate / desired output rate) = (100hz / 10hz) = 10
+			log_counter = 0;
+			Log_Write_PID(CH6_RATE_KP, rate_error, p, i, d-rate_d_dampener, output, tuning_value);
+		}
+	}
+#endif
+
+	// output control
+	return output;
+}
+
+static int16_t
+get_rate_pitch(int32_t target_rate)
+{
+	static int8_t log_counter = 0;					// used to slow down logging of PID values to dataflash
+	static int32_t last_rate = 0;					// previous iterations rate
+	int32_t p,i,d;									// used to capture pid values for logging
+	int32_t current_rate;							// this iteration's rate
+	int32_t rate_error;								// simply target_rate - current_rate
+	int32_t rate_d;  								// roll's acceleration
+	int32_t output;									// output from pid controller
+	int32_t rate_d_dampener;						// value to dampen output based on acceleration
+
+	// get current rate
+	current_rate 	= (omega.y * DEGX100);
+
+	// calculate and filter the acceleration
+	rate_d 			= pitch_rate_d_filter.apply(current_rate - last_rate);
+
+	// store rate for next iteration
+	last_rate 		= current_rate;
+
+	// call pid controller
+	rate_error	= target_rate - current_rate;
+	p 			= g.pid_rate_pitch.get_p(rate_error);
+	i 			= g.pid_rate_pitch.get_i(rate_error, G_Dt);
+	d 			= g.pid_rate_pitch.get_d(rate_error, G_Dt);
+	output		= p + i + d;
+
+	// Dampening output with D term
+	rate_d_dampener = rate_d * pitch_scale_d;
+	rate_d_dampener = constrain(rate_d_dampener, -400, 400);
+	output -= rate_d_dampener;
+
+	// constrain output
+	output = constrain(output, -2500, 2500);
+
+#if LOGGING_ENABLED == ENABLED
+	// log output if PID logging is on and we are tuning the rate P, I or D gains
+	if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) {
+		log_counter++;
+		if( log_counter >= 10 ) {	// (update rate / desired output rate) = (100hz / 10hz) = 10
+			log_counter = 0;
+			Log_Write_PID(CH6_RATE_KP+100, rate_error, p, i, d-rate_d_dampener, output, tuning_value);
+		}
+	}
+#endif
+
+	// output control
+	return output;
+}
+
+static int16_t
+get_rate_yaw(int32_t target_rate)
+{
+	static int8_t log_counter = 0;					// used to slow down logging of PID values to dataflash
+	int32_t p,i,d;									// used to capture pid values for logging
+	int32_t rate_error;
+	int32_t output;
+
+	// rate control
+	rate_error	 	= target_rate - (omega.z * DEGX100);
+
+	// separately calculate p, i, d values for logging
+	p = g.pid_rate_yaw.get_p(rate_error);
+	i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
+	d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
+
+	output 	= p+i+d;
+
+	// output control:
+	int16_t yaw_limit = 1400 + abs(g.rc_4.control_in);
+
+	// constrain output
+	output = constrain(output, -yaw_limit, yaw_limit);
+
+#if LOGGING_ENABLED == ENABLED
+	// log output if PID loggins is on and we are tuning the yaw
+	if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) {
+		log_counter++;
+		if( log_counter >= 10 ) {	// (update rate / desired output rate) = (100hz / 10hz) = 10
+			log_counter = 0;
+			Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value);
+		}
+	}
+#endif
+
+	// constrain output
+	return output;
+}
+
+static int16_t
+get_nav_throttle(int32_t z_error)
+{
+	//static int16_t old_output = 0;
+	int16_t rate_error = 0;
+	int16_t output = 0;
+
+	// convert to desired Rate:
+	rate_error 		= g.pi_alt_hold.get_p(z_error);
+	rate_error 		= constrain(rate_error, -250, 250);
+
+	// limit error to prevent I term wind up
+	z_error 		= constrain(z_error, -400, 400);
+
+	// compensates throttle setpoint error for hovering
+	int16_t iterm = g.pi_alt_hold.get_i(z_error, .02);
+
+	// calculate rate error
+	rate_error 		= rate_error - climb_rate;
+
+	// hack to see if we can smooth out oscillations
+	//if(rate_error < 0)
+	//	rate_error = rate_error >> 1;
+
+	// limit the rate
+	output =  constrain(g.pid_throttle.get_pid(rate_error, .02), -80, 120);
+
+	// light filter of output
+	//output = (old_output + output) / 2;
+
+	// save our output
+	//old_output  = output;
+
+	// output control:
+	return output + iterm;
+}
+
+// Keeps old data out of our calculation / logs
+static void reset_nav_params(void)
+{
+	nav_throttle 			= 0;
+
+	// always start Circle mode at same angle
+	circle_angle			= 0;
+
+	// We must be heading to a new WP, so XTrack must be 0
+	crosstrack_error 		= 0;
+
+	// Will be set by new command
+	target_bearing 			= 0;
+
+	// Will be set by new command
+	wp_distance 			= 0;
+
+	// Will be set by new command, used by loiter
+	long_error 				= 0;
+	lat_error  				= 0;
+
+	// Will be set by new command, used by loiter
+	next_WP.alt				= 0;
+
+	// We want to by default pass WPs
+	slow_wp = false;
+}
+
+/*
+  reset all I integrators
+ */
+static void reset_I_all(void)
+{
+	reset_rate_I();
+	reset_stability_I();
+	reset_wind_I();
+	reset_throttle_I();
+	reset_optflow_I();
+
+	// This is the only place we reset Yaw
+	g.pi_stabilize_yaw.reset_I();
+}
+
+static void reset_rate_I()
+{
+	g.pid_rate_roll.reset_I();
+	g.pid_rate_pitch.reset_I();
+	g.pid_rate_yaw.reset_I();
+}
+
+static void reset_optflow_I(void)
+{
+	g.pid_optflow_roll.reset_I();
+	g.pid_optflow_pitch.reset_I();
+	of_roll = 0;
+	of_pitch = 0;
+}
+
+static void reset_wind_I(void)
+{
+	// Wind Compensation
+	// this i is not currently being used, but we reset it anyway
+	// because someone may modify it and not realize it, causing a bug
+	g.pi_loiter_lat.reset_I();
+	g.pi_loiter_lon.reset_I();
+
+	g.pid_loiter_rate_lat.reset_I();
+	g.pid_loiter_rate_lon.reset_I();
+
+	g.pid_nav_lat.reset_I();
+	g.pid_nav_lon.reset_I();
+}
+
+static void reset_throttle_I(void)
+{
+	// For Altitude Hold
+	g.pi_alt_hold.reset_I();
+	g.pid_throttle.reset_I();
+}
+
+static void reset_stability_I(void)
+{
+	// Used to balance a quad
+	// This only needs to be reset during Auto-leveling in flight
+	g.pi_stabilize_roll.reset_I();
+	g.pi_stabilize_pitch.reset_I();
+}
+
+
+/*************************************************************
+throttle control
+****************************************************************/
+
+static long
+get_nav_yaw_offset(int yaw_input, int reset)
+{
+	int32_t _yaw;
+
+	if(reset == 0){
+		// we are on the ground
+		return ahrs.yaw_sensor;
+
+	}else{
+		// re-define nav_yaw if we have stick input
+		if(yaw_input != 0){
+			// set nav_yaw + or - the current location
+			_yaw = yaw_input + ahrs.yaw_sensor;
+			// we need to wrap our value so we can be 0 to 360 (*100)
+			return wrap_360(_yaw);
+
+		}else{
+			// no stick input, lets not change nav_yaw
+			return nav_yaw;
+		}
+	}
+}
+
+static int16_t get_angle_boost(int16_t value)
+{
+	float temp = cos_pitch_x * cos_roll_x;
+	temp = 1.0 - constrain(temp, .5, 1.0);
+	int16_t output = temp * value;
+	return constrain(output, 0, 200);
+//	return (int)(temp * value);
+}
+
+#if FRAME_CONFIG == HELI_FRAME
+// heli_angle_boost - adds a boost depending on roll/pitch values
+// equivalent of quad's angle_boost function
+// throttle value should be 0 ~ 1000
+static int16_t heli_get_angle_boost(int16_t throttle)
+{
+    float angle_boost_factor = cos_pitch_x * cos_roll_x;
+	angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);
+	int throttle_above_mid = max(throttle - motors.throttle_mid,0);
+	return throttle + throttle_above_mid*angle_boost_factor;
+
+}
+#endif // HELI_FRAME
+
+#define NUM_G_SAMPLES 40
+
+#if ACCEL_ALT_HOLD == 2
+// z -14.4306 = going up
+// z -6.4306 = going down
+static int get_z_damping()
+{
+	int output;
+
+	Z_integrator 	+= get_world_Z_accel() - Z_offset;
+	output 			= Z_integrator * 3;
+	Z_integrator 	= Z_integrator * .8;
+	output = constrain(output, -100, 100);
+	return output;
+}
+
+float get_world_Z_accel()
+{
+	accels_rot = ahrs.get_dcm_matrix() * imu.get_accel();
+	//Serial.printf("z %1.4f\n", accels_rot.z);
+	return accels_rot.z;
+}
+
+static void init_z_damper()
+{
+	Z_offset = 0;
+	for (int i = 0; i < NUM_G_SAMPLES; i++){
+		delay(5);
+		read_AHRS();
+		Z_offset += get_world_Z_accel();
+	}
+	Z_offset /= (float)NUM_G_SAMPLES;
+}
+
+
+
+
+// Accelerometer Z dampening by Aurelio R. Ramos
+// ---------------------------------------------
+#elif ACCEL_ALT_HOLD == 1
+
+// contains G and any other DC offset
+static float estimatedAccelOffset = 0;
+
+// state
+static float synVelo = 0;
+static float synPos = 0;
+static float synPosFiltered = 0;
+static float posError = 0;
+static float prevSensedPos = 0;
+
+// tuning for dead reckoning
+static const float dt_50hz = 0.02;
+static float synPosP = 10 * dt_50hz;
+static float synPosI = 15 * dt_50hz;
+static float synVeloP = 1.5 * dt_50hz;
+static float maxVeloCorrection = 5 * dt_50hz;
+static float maxSensedVelo = 1;
+static float synPosFilter = 0.5;
+
+
+// Z damping term.
+static float fullDampP = 0.100;
+
+float get_world_Z_accel()
+{
+	accels_rot = ahrs.get_dcm_matrix() * imu.get_accel();
+	return accels_rot.z;
+}
+
+static void init_z_damper()
+{
+	estimatedAccelOffset = 0;
+	for (int i = 0; i < NUM_G_SAMPLES; i++){
+		delay(5);
+		read_AHRS();
+		estimatedAccelOffset += get_world_Z_accel();
+	}
+	estimatedAccelOffset /= (float)NUM_G_SAMPLES;
+}
+
+float dead_reckon_Z(float sensedPos, float sensedAccel)
+{
+	// the following algorithm synthesizes position and velocity from
+	// a noisy altitude and accelerometer data.
+
+	// synthesize uncorrected velocity by integrating acceleration
+	synVelo += (sensedAccel - estimatedAccelOffset) * dt_50hz;
+
+	// synthesize uncorrected position by integrating uncorrected velocity
+	synPos += synVelo * dt_50hz;
+
+	// filter synPos, the better this filter matches the filtering and dead time
+	// of the sensed position, the less the position estimate will lag.
+	synPosFiltered = synPosFiltered * (1 - synPosFilter) + synPos * synPosFilter;
+
+	// calculate error against sensor position
+	posError = sensedPos - synPosFiltered;
+
+	// correct altitude
+	synPos += synPosP * posError;
+
+	// correct integrated velocity by posError
+	synVelo = synVelo + constrain(posError, -maxVeloCorrection, maxVeloCorrection) * synPosI;
+
+	// correct integrated velocity by the sensed position delta in a small proportion
+	// (i.e., the low frequency of the delta)
+	float sensedVelo = (sensedPos - prevSensedPos) / dt_50hz;
+	synVelo += constrain(sensedVelo - synVelo, -maxSensedVelo, maxSensedVelo) * synVeloP;
+
+	prevSensedPos = sensedPos;
+	return synVelo;
+}
+
+static int get_z_damping()
+{
+	float sensedAccel = get_world_Z_accel();
+	float sensedPos = current_loc.alt / 100.0;
+
+	float synVelo = dead_reckon_Z(sensedPos, sensedAccel);
+	return constrain(fullDampP * synVelo * (-1), -300, 300);
+}
+
+#else
+
+static int get_z_damping()
+{
+	return 0;
+}
+
+static void init_z_damper()
+{
+}
+#endif
+
+// calculate modified roll/pitch depending upon optical flow calculated position
+static int32_t
+get_of_roll(int32_t control_roll)
+{
+#ifdef OPTFLOW_ENABLED
+	static float tot_x_cm = 0;  // total distance from target
+    static uint32_t last_of_roll_update = 0;
+	int32_t new_roll = 0;
+
+	// check if new optflow data available
+	if( optflow.last_update != last_of_roll_update) {
+	    last_of_roll_update = optflow.last_update;
+
+		// add new distance moved
+		tot_x_cm += optflow.x_cm;
+
+		// only stop roll if caller isn't modifying roll
+		if( control_roll == 0 && current_loc.alt < 1500) {
+			new_roll = g.pid_optflow_roll.get_pid(-tot_x_cm, 1.0);  // we could use the last update time to calculate the time change
+		}else{
+		    g.pid_optflow_roll.reset_I();
+			tot_x_cm = 0;
+		}
+		// limit amount of change and maximum angle
+		of_roll = constrain(new_roll, (of_roll-20), (of_roll+20));
+	}
+
+	// limit max angle
+    of_roll = constrain(of_roll, -1000, 1000);
+    return control_roll+of_roll;
+#else
+    return control_roll;
+#endif
+}
+
+static int32_t
+get_of_pitch(int32_t control_pitch)
+{
+#ifdef OPTFLOW_ENABLED
+    static float tot_y_cm = 0;  // total distance from target
+    static uint32_t last_of_pitch_update = 0;
+	int32_t new_pitch = 0;
+
+	// check if new optflow data available
+	if( optflow.last_update != last_of_pitch_update ) {
+	    last_of_pitch_update = optflow.last_update;
+
+		// add new distance moved
+		tot_y_cm += optflow.y_cm;
+
+		// only stop roll if caller isn't modifying pitch
+		if( control_pitch == 0 && current_loc.alt < 1500 ) {
+			new_pitch = g.pid_optflow_pitch.get_pid(tot_y_cm, 1.0);  // we could use the last update time to calculate the time change
+		}else{
+		    tot_y_cm = 0;
+		    g.pid_optflow_pitch.reset_I();
+		}
+
+		// limit amount of change
+		of_pitch = constrain(new_pitch, (of_pitch-20), (of_pitch+20));
+	}
+
+	// limit max angle
+	of_pitch = constrain(of_pitch, -1000, 1000);
+    return control_pitch+of_pitch;
+#else
+    return control_pitch;
+#endif
+}

ArduCopter/CMakeLists.txt

+set(CMAKE_TOOLCHAIN_FILE ../cmake/ArduinoToolchain.cmake) # Arduino Toolchain
+
+cmake_minimum_required(VERSION 2.8)
+
+project(ArduCopter C CXX)
+
+set(PROJECT_VERSION_MAJOR "2")
+set(PROJECT_VERSION_MINOR "6")
+set(PROJECT_VERSION_PATCH "0")
+
+set(PROJECT_DESCRIPTION "ArduPilotMega based Rotor-craft Autopilot.")
+
+# macro path
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../cmake/modules")
+include(CMakeParseArguments)
+include(APMOption)
+
+# options
+add_definitions(-DUSE_CMAKE_APM_CONFIG)
+include(options.cmake)
+include_directories(${CMAKE_BINARY_DIR})
+apm_option_generate_config(FILE "APM_Config_cmake.h" OPTIONS ${APM_OPTIONS})
+
+# disallow in-source build
+include(MacroEnsureOutOfSourceBuild)
+macro_ensure_out_of_source_build("${PROJECT_NAME} requires an out of source build.
+Please create a separate build directory and run 'cmake /path/to/${PROJECT_NAME} [options]' there.")
+
+# built variables
+set(PROJECT_VERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}.${PROJECT_VERSION_PATCH}")
+set(FIRMWARE_NAME "${PROJECT_NAME}-${APM_HARDWARE}-${APM_PROCESSOR}-${HIL_MODE}")
+
+# modify flags from default toolchain flags
+set(APM_OPT_FLAGS "-Wformat -Wall -Wshadow -Wpointer-arith -Wcast-align -Wwrite-strings -Wformat=2")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${APM_OPT_FLAGS}")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${APM_OPT_FLAGS} -Wno-reorder")
+set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${APM_OPT_FLAGS} -Wl,--relax")
+
+# build apm project
+set(ARDUINO_EXTRA_LIBRARIES_PATH ${CMAKE_SOURCE_DIR}/../libraries)
+set(${FIRMWARE_NAME}_SKETCH ${CMAKE_SOURCE_DIR}/../${PROJECT_NAME})
+set(${FIRMWARE_NAME}_BOARD ${APM_PROCESSOR})
+set(${FIRMWARE_NAME}_PORT ${APM_PROGRAMMING_PORT})
+generate_arduino_firmware(${FIRMWARE_NAME})
+install(FILES ${CMAKE_BINARY_DIR}/${FIRMWARE_NAME}.hex DESTINATION "/")
+
+# cpack
+include(APMCPackConfig)

ArduCopter/Camera.pde

+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+//#if CAMERA_STABILIZER == ENABLED
+
+static void init_camera()
+{
+	APM_RC.enable_out(CH_CAM_PITCH);
+	APM_RC.enable_out(CH_CAM_ROLL);
+
+	// ch 6 high(right) is down.
+	g.rc_camera_pitch.set_angle(4500);
+	g.rc_camera_roll.set_angle(4500);
+
+	g.rc_camera_roll.set_type(RC_CHANNEL_ANGLE_RAW);
+	g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW);
+}
+
+static void
+camera_stabilization()
+{
+	// PITCH
+	// -----
+	// Allow user to control camera pitch with channel 6 (mixed with pitch DCM)
+	if(g.radio_tuning == 0) {
+		g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6));
+		g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(ahrs.pitch_sensor);
+	}else{
+		// unless channel 6 is already being used for tuning
+	    g.rc_camera_pitch.servo_out = ahrs.pitch_sensor  * -1;
+	}
+	g.rc_camera_pitch.servo_out	= (float)g.rc_camera_pitch.servo_out * g.camera_pitch_gain;
+
+	// limit
+	//g.rc_camera_pitch.servo_out = constrain(g.rc_camera_pitch.servo_out, -4500, 4500);
+
+
+	// ROLL
+	// -----
+	// allow control mixing
+	/*
+	g.rc_camera_roll.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here.
+	g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-ahrs.roll_sensor);
+	*/
+
+	// dont allow control mixing
+	g.rc_camera_roll.servo_out	= (float)-ahrs.roll_sensor * g.camera_roll_gain;
+
+	// limit
+	//g.rc_camera_roll.servo_out = constrain(-ahrs.roll_sensor, -4500, 4500);
+
+	// Output
+	// ------
+	g.rc_camera_pitch.calc_pwm();
+	g.rc_camera_roll.calc_pwm();
+
+	APM_RC.OutputCh(CH_CAM_PITCH, g.rc_camera_pitch.radio_out);
+	APM_RC.OutputCh(CH_CAM_ROLL , g.rc_camera_roll.radio_out);
+	//Serial.printf("c:%d\n",  g.rc_camera_pitch.radio_out);
+}
+
+//#endif