diff --git a/libraries/AP_Motors/AP_Motors.cpp b/libraries/AP_Motors/AP_Motors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..02f414601dd64806dd847aad8d600a2838b9751f
--- /dev/null
+++ b/libraries/AP_Motors/AP_Motors.cpp
@@ -0,0 +1,56 @@
+/*
+	AP_Motors.cpp - ArduCopter motors library
+	Code by RandyMackay. 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 "AP_Motors.h"
+
+// parameters for the motor class
+const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
+	AP_GROUPINFO("TB_RATIO", 0, AP_Motors,	top_bottom_ratio),  // not used
+    AP_GROUPEND
+};
+
+// Constructor 
+AP_Motors::AP_Motors( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz ) :
+	top_bottom_ratio(AP_MOTORS_TOP_BOTTOM_RATIO),
+	_rc(rc_out),
+	_rc_roll(rc_roll),
+	_rc_pitch(rc_pitch),
+	_rc_throttle(rc_throttle),
+	_rc_yaw(rc_yaw),
+	_speed_hz(speed_hz),
+	_armed(false),
+	_auto_armed(false),
+	_frame_orientation(0),
+	_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
+	_max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE)
+{
+	uint8_t i;
+	
+	// initialise motor map
+	if( APM_version == AP_MOTORS_APM1 ) {
+		set_motor_to_channel_map(APM1_MOTOR_TO_CHANNEL_MAP);
+	} else {
+		set_motor_to_channel_map(APM2_MOTOR_TO_CHANNEL_MAP);
+	}
+
+	// clear output arrays
+	for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+		motor_out[i] = 0;
+	}
+};
+
+// throttle_pass_through - passes throttle through to motors - dangerous but used for initialising ESCs
+void AP_Motors::throttle_pass_through() {
+	if( armed() ) {
+		for( int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+			_rc->OutputCh(_motor_to_channel_map[i], _rc_throttle->radio_in);
+		}
+	}
+}
diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h
new file mode 100644
index 0000000000000000000000000000000000000000..1d5f9e718466f2795a6baf3e5167ae8c3f7783af
--- /dev/null
+++ b/libraries/AP_Motors/AP_Motors.h
@@ -0,0 +1,137 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AxisController.h
+/// @brief	Generic PID algorithm, with EEPROM-backed storage of constants.
+
+#ifndef AP_MOTORS
+#define AP_MOTORS
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+
+// offsets for motors in motor_out, _motor_filtered and _motor_to_channel_map arrays
+#define AP_MOTORS_MOT_1 0
+#define AP_MOTORS_MOT_2 1
+#define AP_MOTORS_MOT_3 2
+#define AP_MOTORS_MOT_4 3
+#define AP_MOTORS_MOT_5 4
+#define AP_MOTORS_MOT_6 5
+#define AP_MOTORS_MOT_7 6
+#define AP_MOTORS_MOT_8 7
+
+#define APM1_MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_7,CH_8,CH_10,CH_11
+#define APM2_MOTOR_TO_CHANNEL_MAP CH_1,CH_2,CH_3,CH_4,CH_5,CH_6,CH_7,CH_8
+
+#define AP_MOTORS_MAX_NUM_MOTORS 8
+
+#define AP_MOTORS_DEFAULT_MIN_THROTTLE	130
+#define AP_MOTORS_DEFAULT_MAX_THROTTLE	850
+
+// APM board definitions
+#define AP_MOTORS_APM1	1
+#define AP_MOTORS_APM2	2
+
+// frame definitions
+#define AP_MOTORS_PLUS_FRAME 0
+#define AP_MOTORS_X_FRAME 1
+#define AP_MOTORS_V_FRAME 2
+
+// motor update rates
+#define AP_MOTORS_SPEED_DEFAULT 490
+#define AP_MOTORS_SPEED_INSTANT_PWM 0
+
+// top-bottom ratio (for Y6)
+#define AP_MOTORS_TOP_BOTTOM_RATIO	1.0
+
+/// @class      AP_Motors 
+class AP_Motors {
+public: 
+
+	// Constructor 
+	AP_Motors( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
+
+	// init
+	virtual void Init() {};
+
+	// set mapping from motor number to RC channel
+	virtual void set_motor_to_channel_map( uint8_t mot_1, uint8_t mot_2, uint8_t mot_3, uint8_t mot_4, uint8_t mot_5, uint8_t mot_6, uint8_t mot_7, uint8_t mot_8 ) {
+		_motor_to_channel_map[AP_MOTORS_MOT_1] = mot_1;
+		_motor_to_channel_map[AP_MOTORS_MOT_2] = mot_2;
+		_motor_to_channel_map[AP_MOTORS_MOT_3] = mot_3;
+		_motor_to_channel_map[AP_MOTORS_MOT_4] = mot_4;
+		_motor_to_channel_map[AP_MOTORS_MOT_5] = mot_5;
+		_motor_to_channel_map[AP_MOTORS_MOT_6] = mot_6;
+		_motor_to_channel_map[AP_MOTORS_MOT_7] = mot_7;
+		_motor_to_channel_map[AP_MOTORS_MOT_8] = mot_8;
+	}
+
+	// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
+	virtual void set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; };
+
+	// set frame orientation (normally + or X)
+	virtual void set_frame_orientation( uint8_t new_orientation ) { _frame_orientation = new_orientation; };
+
+	// enable - starts allowing signals to be sent to motors
+	virtual void enable() {};
+
+	// arm, disarm or check status status of motors
+	virtual bool armed() { return _armed; };
+	virtual void armed(bool armed) { _armed = armed; };
+
+	// check or set status of auto_armed - controls whether autopilot can take control of throttle
+	// Note: this should probably be moved out of this class as it has little to do with the motors
+	virtual bool auto_armed() { return _auto_armed; };
+	virtual void auto_armed(bool armed) { _auto_armed = armed; };
+
+	// set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
+	virtual void set_min_throttle(uint16_t min_throttle) { _min_throttle = min_throttle; };
+	virtual void set_max_throttle(uint16_t max_throttle) { _max_throttle = max_throttle; };
+
+	// output - sends commands to the motors
+	virtual void output() { if( _armed && _auto_armed ) { output_armed(); }else{ output_disarmed(); } };
+
+	// output_min - sends minimum values out to the motors
+	virtual void output_min() {};
+
+	// get basic information about the platform
+	virtual uint8_t get_num_motors() { return 0; };
+
+	// motor test
+	virtual void output_test() {};
+
+	// throttle_pass_through - passes throttle through to motors - dangerous but required for initialising ESCs
+	virtual void throttle_pass_through();
+ 
+	// 1 if motor is enabled, 0 otherwise
+	AP_Int8		motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];
+
+	// final output values sent to the motors.  public (for now) so that they can be access for logging
+	int16_t 	motor_out[AP_MOTORS_MAX_NUM_MOTORS];
+
+	// power ratio of upper vs lower motors (only used by y6 and octa quad copters)
+	AP_Float	top_bottom_ratio;
+
+	// var_info for holding Parameter information
+	static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+
+	// output functions that should be overloaded by child classes
+	virtual void output_armed() {};
+	virtual void output_disarmed() {};
+	
+	APM_RC_Class*	_rc;				// APM_RC class used to send updates to ESCs/Servos
+	RC_Channel*	_rc_roll, *_rc_pitch, *_rc_throttle, *_rc_yaw;	// input in from users
+	uint8_t		_motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS];	// mapping of motor number (as received from upper APM code) to RC channel output - used to account for differences between APM1 and APM2
+	uint16_t	_speed_hz;			// speed in hz to send updates to motors
+	bool		_armed;				// true if motors are armed
+	bool		_auto_armed;		// true is throttle is above zero, allows auto pilot to take control of throttle
+	uint8_t		_frame_orientation;	// PLUS_FRAME 0, X_FRAME 1, V_FRAME 2
+	int16_t		_min_throttle;		// the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle)
+	int16_t		_max_throttle;		// the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle)
+}; 
+
+#endif  // AP_MOTORS
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..61216dbf7ff7452d1836eb0c96f58a8c82936f57
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsHeli.cpp
@@ -0,0 +1,370 @@
+/*
+	AP_MotorsHeli.cpp - ArduCopter motors library
+	Code by RandyMackay. 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 "AP_MotorsHeli.h"
+
+const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
+	AP_NESTEDGROUPINFO(AP_Motors, 0),
+	AP_GROUPINFO("SV1_POS",	1,	AP_MotorsHeli,	servo1_pos),
+	AP_GROUPINFO("SV2_POS",	2,	AP_MotorsHeli,	servo2_pos),
+	AP_GROUPINFO("SV3_POS",	3,	AP_MotorsHeli,	servo3_pos),
+	AP_GROUPINFO("ROL_MAX",	4,	AP_MotorsHeli,	roll_max),
+	AP_GROUPINFO("PIT_MAX",	5,	AP_MotorsHeli,	pitch_max),
+	AP_GROUPINFO("COL_MIN",	6,	AP_MotorsHeli,	collective_min),
+	AP_GROUPINFO("COL_MAX",	7,	AP_MotorsHeli,	collective_max),
+	AP_GROUPINFO("COL_MID",	8,	AP_MotorsHeli,	collective_mid),
+	AP_GROUPINFO("GYR_ENABLE",	9,	AP_MotorsHeli,	ext_gyro_enabled),
+	AP_GROUPINFO("SWASH_TYPE",	10,	AP_MotorsHeli,	swash_type),				// changed from trunk
+	AP_GROUPINFO("GYR_GAIN",	11,	AP_MotorsHeli,	ext_gyro_gain),
+	AP_GROUPINFO("SV_MAN",		12,	AP_MotorsHeli,	servo_manual),
+	AP_GROUPINFO("PHANG",		13,	AP_MotorsHeli,	phase_angle),				// changed from trunk
+	AP_GROUPINFO("COLYAW",		14,	AP_MotorsHeli,	collective_yaw_effect),	// changed from trunk
+    AP_GROUPEND
+};
+
+// init
+void AP_MotorsHeli::Init()
+{
+	// set update rate
+	set_update_rate(_speed_hz);
+}
+
+// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
+void AP_MotorsHeli::set_update_rate( uint16_t speed_hz )
+{
+	// record requested speed
+	_speed_hz = speed_hz;
+
+	// setup fast channels
+	if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) {
+		_rc->SetFastOutputChannels(_BV(_motor_to_channel_map[AP_MOTORS_MOT_1]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_2]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_3]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_4]), _speed_hz);
+	}
+}
+
+	// enable - starts allowing signals to be sent to motors
+void AP_MotorsHeli::enable()
+{
+	// enable output channels
+	_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_1]);	// swash servo 1
+	_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_2]);	// swash servo 2
+	_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_3]);	// swash servo 3
+	_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_4]);	// yaw
+	_rc->enable_out(AP_MOTORS_HELI_EXT_GYRO);	// for external gyro
+}
+
+// output_min - sends minimum values out to the motors
+void AP_MotorsHeli::output_min()
+{
+	// move swash to mid
+	move_swash(0,0,500,0);
+}
+
+// output_armed - sends commands to the motors
+void AP_MotorsHeli::output_armed()
+{
+    // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
+    if( servo_manual == 1 ) {
+		_rc_roll->servo_out = _rc_roll->control_in;
+		_rc_pitch->servo_out = _rc_pitch->control_in;
+		_rc_throttle->servo_out = _rc_throttle->control_in;
+		_rc_yaw->servo_out = _rc_yaw->control_in;
+	}
+
+    //static int counter = 0;
+	_rc_roll->calc_pwm();
+	_rc_pitch->calc_pwm();
+	_rc_throttle->calc_pwm();
+	_rc_yaw->calc_pwm();
+
+	move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out );
+}
+
+// output_disarmed - sends commands to the motors
+void AP_MotorsHeli::output_disarmed()
+{
+	if(_rc_throttle->control_in > 0){
+		// we have pushed up the throttle
+		// remove safety
+		_auto_armed = true;
+	}
+
+	// for helis - armed or disarmed we allow servos to move
+	output_armed();
+}
+
+// output_disarmed - sends commands to the motors
+void AP_MotorsHeli::output_test()
+{
+	int16_t i;
+	// Send minimum values to all motors
+	output_min();
+
+	// servo 1
+	for( i=0; i<5; i++ ) {
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 100);
+		delay(300);
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim - 100);
+		delay(300);
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_trim + 0);
+		delay(300);
+	}
+
+	// servo 2
+	for( i=0; i<5; i++ ) {
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 100);
+		delay(300);
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim - 100);
+		delay(300);
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_trim + 0);
+		delay(300);
+	}
+
+	// servo 3
+	for( i=0; i<5; i++ ) {
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 100);
+		delay(300);
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim - 100);
+		delay(300);
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_trim + 0);
+		delay(300);
+	}
+
+	// external gyro
+	if( ext_gyro_enabled ) {
+		_rc->OutputCh(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain);
+	}
+
+	// servo 4
+	for( i=0; i<5; i++ ) {
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 100);
+		delay(300);
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim - 100);
+		delay(300);
+		_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_trim + 0);
+		delay(300);
+	}
+
+	// Send minimum values to all motors
+	output_min();
+}
+
+// reset_swash - free up swash for maximum movements. Used for set-up
+void AP_MotorsHeli::reset_swash()
+{
+	// free up servo ranges
+	_servo_1->radio_min = 1000;
+	_servo_1->radio_max = 2000;
+	_servo_2->radio_min = 1000;
+	_servo_2->radio_max = 2000;
+	_servo_3->radio_min = 1000;
+	_servo_3->radio_max = 2000;
+
+	if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) {			//CCPM Swashplate, perform servo control mixing
+		
+		// roll factors
+		_rollFactor[CH_1] = cos(radians(servo1_pos + 90 - phase_angle));
+		_rollFactor[CH_2] = cos(radians(servo2_pos + 90 - phase_angle));
+		_rollFactor[CH_3] = cos(radians(servo3_pos + 90 - phase_angle));
+				
+		// pitch factors
+		_pitchFactor[CH_1] = cos(radians(servo1_pos - phase_angle));
+		_pitchFactor[CH_2] = cos(radians(servo2_pos - phase_angle));
+		_pitchFactor[CH_3] = cos(radians(servo3_pos - phase_angle));
+		
+		// collective factors
+		_collectiveFactor[CH_1] = 1;
+		_collectiveFactor[CH_2] = 1;
+		_collectiveFactor[CH_3] = 1;
+		
+	}else{  								//H1 Swashplate, keep servo outputs seperated
+
+		// roll factors
+		_rollFactor[CH_1] = 1;
+		_rollFactor[CH_2] = 0;
+		_rollFactor[CH_3] = 0;
+	
+		// pitch factors
+		_pitchFactor[CH_1] = 0;
+		_pitchFactor[CH_2] = 1;
+		_pitchFactor[CH_3] = 0;
+		
+		// collective factors
+		_collectiveFactor[CH_1] = 0;
+		_collectiveFactor[CH_2] = 0;
+		_collectiveFactor[CH_3] = 1;
+	}
+
+	// set roll, pitch and throttle scaling
+	_roll_scaler = 1.0;
+	_pitch_scaler = 1.0;
+	_collective_scalar = ((float)(_rc_throttle->radio_max - _rc_throttle->radio_min))/1000.0;
+
+	// we must be in set-up mode so mark swash as uninitialised
+	_swash_initialised = false;
+}
+
+// init_swash - initialise the swash plate
+void AP_MotorsHeli::init_swash()
+{
+
+	// swash servo initialisation
+	_servo_1->set_range(0,1000);
+	_servo_2->set_range(0,1000);
+	_servo_3->set_range(0,1000);
+	_servo_4->set_angle(4500);
+
+	// ensure _coll values are reasonable
+	if( collective_min >= collective_max ) {
+	    collective_min = 1000;
+		collective_max = 2000;
+	}
+	collective_mid = constrain(collective_mid, collective_min, collective_max);
+
+	// calculate throttle mid point
+	throttle_mid = ((float)(collective_mid-collective_min))/((float)(collective_max-collective_min))*1000.0;
+
+	// determine roll, pitch and throttle scaling
+	_roll_scaler = (float)roll_max/4500.0;
+	_pitch_scaler = (float)pitch_max/4500.0;
+	_collective_scalar = ((float)(collective_max-collective_min))/1000.0;
+
+	if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) {			//CCPM Swashplate, perform control mixing
+		
+		// roll factors
+		_rollFactor[CH_1] = cos(radians(servo1_pos + 90 - phase_angle));
+		_rollFactor[CH_2] = cos(radians(servo2_pos + 90 - phase_angle));
+		_rollFactor[CH_3] = cos(radians(servo3_pos + 90 - phase_angle));
+				
+		// pitch factors
+		_pitchFactor[CH_1] = cos(radians(servo1_pos - phase_angle));
+		_pitchFactor[CH_2] = cos(radians(servo2_pos - phase_angle));
+		_pitchFactor[CH_3] = cos(radians(servo3_pos - phase_angle));
+		
+		// collective factors
+		_collectiveFactor[CH_1] = 1;
+		_collectiveFactor[CH_2] = 1;
+		_collectiveFactor[CH_3] = 1;
+		
+	}else{  	//H1 Swashplate, keep servo outputs seperated
+
+		// roll factors
+		_rollFactor[CH_1] = 1;
+		_rollFactor[CH_2] = 0;
+		_rollFactor[CH_3] = 0;
+	
+		// pitch factors
+		_pitchFactor[CH_1] = 0;
+		_pitchFactor[CH_2] = 1;
+		_pitchFactor[CH_3] = 0;
+		
+		// collective factors
+		_collectiveFactor[CH_1] = 0;
+		_collectiveFactor[CH_2] = 0;
+		_collectiveFactor[CH_3] = 1;
+	}
+
+	// servo min/max values
+	_servo_1->radio_min = 1000;
+	_servo_1->radio_max = 2000;
+	_servo_2->radio_min = 1000;
+	_servo_2->radio_max = 2000;
+	_servo_3->radio_min = 1000;
+	_servo_3->radio_max = 2000;
+
+	// mark swash as initialised
+	_swash_initialised = true;
+}
+
+//
+// heli_move_swash - moves swash plate to attitude of parameters passed in
+//                 - expected ranges:
+//                       roll : -4500 ~ 4500
+//                       pitch: -4500 ~ 4500
+//                       collective: 0 ~ 1000
+//                       yaw:   -4500 ~ 4500
+//
+void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_out, int16_t yaw_out)
+{
+	int16_t yaw_offset = 0;
+	int16_t coll_out_scaled;
+
+	if( servo_manual == 1 ) {  // are we in manual servo mode? (i.e. swash set-up mode)?
+		// check if we need to free up the swash
+		if( _swash_initialised ) {
+			reset_swash();
+		}
+		coll_out_scaled = coll_out * _collective_scalar + _rc_throttle->radio_min - 1000;
+	}else{  // regular flight mode
+
+		// check if we need to reinitialise the swash
+		if( !_swash_initialised ) {
+			init_swash();
+		}
+		
+		// rescale roll_out and pitch-out into the min and max ranges to provide linear motion 
+		// across the input range instead of stopping when the input hits the constrain value
+		// these calculations are based on an assumption of the user specified roll_max and pitch_max 
+		// coming into this equation at 4500 or less, and based on the original assumption of the  
+		// total _servo_x.servo_out range being -4500 to 4500.
+		roll_out = roll_out * _roll_scaler;
+		roll_out = constrain(roll_out, (int16_t)-roll_max, (int16_t)roll_max);
+
+		pitch_out = pitch_out * _pitch_scaler;
+		pitch_out = constrain(pitch_out, (int16_t)-pitch_max, (int16_t)pitch_max);
+
+	    // scale collective pitch
+		coll_out = constrain(coll_out, 0, 1000);
+		coll_out_scaled = coll_out * _collective_scalar + collective_min - 1000;
+
+		// rudder feed forward based on collective
+		if( !ext_gyro_enabled ) {
+			yaw_offset = collective_yaw_effect * abs(coll_out_scaled - collective_mid);
+		}
+	}
+
+	// swashplate servos
+	_servo_1->servo_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out)/10 + _collectiveFactor[CH_1] * coll_out_scaled + (_servo_1->radio_trim-1500);
+	_servo_2->servo_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out)/10 + _collectiveFactor[CH_2] * coll_out_scaled + (_servo_2->radio_trim-1500);
+	if( swash_type == AP_MOTORS_HELI_SWASH_H1 ) {
+		_servo_1->servo_out += 500;
+		_servo_2->servo_out += 500;
+	}
+	_servo_3->servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_servo_3->radio_trim-1500);
+	_servo_4->servo_out = yaw_out + yaw_offset;
+
+	// use servo_out to calculate pwm_out and radio_out
+	_servo_1->calc_pwm();
+	_servo_2->calc_pwm();
+	_servo_3->calc_pwm();
+	_servo_4->calc_pwm();
+
+	// actually move the servos
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_out);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_out);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_out);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_out);
+
+	// to be compatible with other frame types
+	motor_out[AP_MOTORS_MOT_1] = _servo_1->radio_out;
+	motor_out[AP_MOTORS_MOT_2] = _servo_2->radio_out;
+	motor_out[AP_MOTORS_MOT_3] = _servo_3->radio_out;
+	motor_out[AP_MOTORS_MOT_4] = _servo_4->radio_out;
+
+	// output gyro value
+	if( ext_gyro_enabled ) {
+		_rc->OutputCh(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain);
+	}
+
+	// InstantPWM
+	if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
+		_rc->Force_Out0_Out1();
+		_rc->Force_Out2_Out3();
+	}
+}
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ad39d9c5924fec8e1c97fcc0e68576a9164dfdd
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsHeli.h
@@ -0,0 +1,140 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AP_MotorsHeli.h
+/// @brief	Motor control class for Traditional Heli
+
+#ifndef AP_MOTORSHELI
+#define AP_MOTORSHELI
+
+#include <inttypes.h>
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+#include <AP_Motors.h>
+
+#define AP_MOTORS_HELI_SPEED_DEFAULT 125	// default servo update rate for helicopters
+#define AP_MOTORS_HELI_SPEED_DIGITAL_SERVOS 125	// update rate for digital servos
+#define AP_MOTORS_HELI_SPEED_ANALOG_SERVOS 125	// update rate for analog servos
+
+#define AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS 3
+
+// tail servo uses channel 7
+#define AP_MOTORS_HELI_EXT_GYRO	CH_7
+
+// frame definitions
+#define AP_MOTORS_HELI_SWASH_CCPM	0
+#define AP_MOTORS_HELI_SWASH_H1		1
+
+/// @class      AP_MotorsHeli 
+class AP_MotorsHeli : public AP_Motors {
+public: 
+
+	/// Constructor 
+	AP_MotorsHeli( uint8_t APM_version, 
+					APM_RC_Class* rc_out,
+					RC_Channel* rc_roll,
+					RC_Channel* rc_pitch,
+					RC_Channel* rc_throttle,
+					RC_Channel* rc_yaw,
+					RC_Channel* swash_servo_1,
+					RC_Channel* swash_servo_2,
+					RC_Channel* swash_servo_3,
+					RC_Channel* yaw_servo,
+					uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : 
+		AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
+		_servo_1(swash_servo_1),
+		_servo_2(swash_servo_2),
+		_servo_3(swash_servo_3),
+		_servo_4(yaw_servo),
+		swash_type(AP_MOTORS_HELI_SWASH_CCPM),
+		servo1_pos				(-60),
+		servo2_pos				(60),
+		servo3_pos				(180),
+		roll_max				(4500),
+		pitch_max				(4500),
+		collective_min			(1250),
+		collective_max			(1750),
+		collective_mid			(1500),
+		ext_gyro_enabled		(0),
+		ext_gyro_gain			(1350),
+		phase_angle				(0),
+		collective_yaw_effect	(0),
+		servo_manual			(0),
+		throttle_mid(0),
+		_roll_scaler(1),
+		_pitch_scaler(1),
+		_collective_scalar(1),
+		_swash_initialised(false)
+		{};
+
+	// init
+	virtual void Init();
+
+	// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
+	// you must have setup_motors before calling this
+	virtual void set_update_rate( uint16_t speed_hz );
+
+	// enable - starts allowing signals to be sent to motors
+	virtual void enable();
+
+	// get basic information about the platform
+	virtual uint8_t get_num_motors() { return 4; };
+
+	// motor test
+	virtual void output_test();
+ 
+ 	// output_min - sends minimum values out to the motors
+	virtual void output_min();
+
+	// reset_swash - free up swash for maximum movements. Used for set-up
+	virtual void reset_swash();
+
+	// init_swash - initialise the swash plate
+	virtual void init_swash();
+
+	// heli_move_swash - moves swash plate to attitude of parameters passed in
+	virtual void move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out);
+
+	// var_info for holding Parameter information
+	static const struct AP_Param::GroupInfo var_info[];
+
+//protected:
+	// output - sends commands to the motors
+	virtual void output_armed();
+	virtual void output_disarmed();
+	
+	float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
+	float _pitchFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
+	float _collectiveFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
+
+	RC_Channel	*_servo_1;
+	RC_Channel	*_servo_2;
+	RC_Channel	*_servo_3;
+	RC_Channel	*_servo_4;
+	AP_Int8		swash_type;
+	AP_Int16	servo1_pos;
+	AP_Int16	servo2_pos;
+	AP_Int16	servo3_pos;
+	AP_Int16	roll_max;
+	AP_Int16	pitch_max;
+	AP_Int16	collective_min;
+	AP_Int16	collective_max;
+	AP_Int16	collective_mid;
+	AP_Int16	ext_gyro_enabled;
+	AP_Int16	ext_gyro_gain;
+	AP_Int16	phase_angle;
+	AP_Int16	collective_yaw_effect;
+	AP_Int8		servo_manual;		// used to trigger swash reset from mission planner
+	
+	// internally used variables
+	int16_t throttle_mid;			// throttle mid point in pwm form (i.e. 0 ~ 1000)
+	float _roll_scaler;				// scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range
+	float _pitch_scaler;			// scaler to convert pitch input from radio (i.e. -4500 ~ 4500) to max pitch range
+	float _collective_scalar;		// throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
+	bool _swash_initialised;		// true if swash has been initialised
+
+}; 
+
+#endif  // AP_MOTORSHELI
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsHexa.cpp b/libraries/AP_Motors/AP_MotorsHexa.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c390298843cc1b3e5701413e1eda50da25f6a22f
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsHexa.cpp
@@ -0,0 +1,37 @@
+/*
+	AP_MotorsHexa.cpp - ArduCopter motors library
+	Code by RandyMackay. 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 "AP_MotorsHexa.h"
+
+// setup_motors - configures the motors for a hexa
+void AP_MotorsHexa::setup_motors()
+{
+	// call parent
+	AP_MotorsMatrix::setup_motors();
+
+	// hard coded config for supported frames
+	if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
+		// plus frame set-up
+		add_motor(AP_MOTORS_MOT_1,   0, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_2, 1);
+		add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 4);
+		add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_4, 5);
+		add_motor(AP_MOTORS_MOT_4,  60, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 2);
+		add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 6);
+		add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_5, 3);
+	}else{
+		// X frame set-up
+		add_motor(AP_MOTORS_MOT_1,  90, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_2, 2);
+		add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 5);
+		add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_4, 6);
+		add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 3);
+		add_motor(AP_MOTORS_MOT_5,  30, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 1);
+		add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_5, 4);
+	}
+}
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsHexa.h b/libraries/AP_Motors/AP_MotorsHexa.h
new file mode 100644
index 0000000000000000000000000000000000000000..ffad0106b123ac65e69da8b8107719e9046976a0
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsHexa.h
@@ -0,0 +1,30 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AP_MotorsHexa.h
+/// @brief	Motor control class for Hexacopters
+
+#ifndef AP_MOTORSHEXA
+#define AP_MOTORSHEXA
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+#include <AP_MotorsMatrix.h>	// Parent Motors Matrix library
+
+/// @class      AP_MotorsHexa 
+class AP_MotorsHexa : public AP_MotorsMatrix {
+public: 
+
+	/// Constructor 
+	AP_MotorsHexa( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
+
+	// setup_motors - configures the motors for a quad
+	virtual void setup_motors();
+
+protected:
+
+}; 
+
+#endif  // AP_MOTORSHEXA
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..851f06365b0ab104b026541706e9a188738db84b
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp
@@ -0,0 +1,330 @@
+/*
+	AP_MotorsMatrix.cpp - ArduCopter motors library
+	Code by RandyMackay. 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 "AP_MotorsMatrix.h"
+
+// Init
+void AP_MotorsMatrix::Init()
+{
+	int8_t i;
+
+	// setup the motors
+	setup_motors();
+
+	// double check that opposite motor definitions are ok
+	for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		if( opposite_motor[i] <= 0 || opposite_motor[i] >= AP_MOTORS_MAX_NUM_MOTORS || !motor_enabled[opposite_motor[i]] )
+			opposite_motor[i] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
+	}
+
+	// enable fast channels or instant pwm
+	set_update_rate(_speed_hz);
+}
+
+// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
+void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
+{
+	uint32_t fast_channel_mask = 0;
+	int8_t i;
+
+	// record requested speed
+	_speed_hz = speed_hz;
+
+	// initialise instant_pwm booleans.  they will be set again below
+	instant_pwm_force01 = false;
+	instant_pwm_force23 = false;
+	instant_pwm_force67 = false;
+
+	// check each enabled motor
+	for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		if( motor_enabled[i] ) {
+			// set-up fast channel mask
+			fast_channel_mask |= _BV(_motor_to_channel_map[i]);	// add to fast channel map
+
+			// and instant pwm
+			if( _motor_to_channel_map[i] == 0 || _motor_to_channel_map[i] == 1 )
+				instant_pwm_force01 = true;
+			if( _motor_to_channel_map[i] == 2 || _motor_to_channel_map[i] == 3 )
+				instant_pwm_force23 = true;
+			if( _motor_to_channel_map[i] == 6 || _motor_to_channel_map[i] == 7 )
+				instant_pwm_force67 = true;
+		}
+	}
+
+	// enable fast channels
+	if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) {
+		_rc->SetFastOutputChannels(fast_channel_mask, _speed_hz);
+	}
+}
+
+// set frame orientation (normally + or X)
+void AP_MotorsMatrix::set_frame_orientation( uint8_t new_orientation )
+{
+	// return if nothing has changed
+	if( new_orientation == _frame_orientation ) {
+		return;
+	}
+
+	// call parent
+	AP_Motors::set_frame_orientation( new_orientation );
+
+	// setup the motors
+	setup_motors();
+
+	// enable fast channels or instant pwm
+	set_update_rate(_speed_hz);
+}
+
+// enable - starts allowing signals to be sent to motors
+void AP_MotorsMatrix::enable()
+{
+	int8_t i;
+
+	// enable output channels
+	for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		if( motor_enabled[i] ) {
+			_rc->enable_out(_motor_to_channel_map[i]);
+		}
+	}
+}
+
+// output_min - sends minimum values out to the motors
+void AP_MotorsMatrix::output_min()
+{
+	int8_t i;
+
+	// fill the motor_out[] array for HIL use and send minimum value to each motor
+	for( int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		if( motor_enabled[i] ) {
+			motor_out[i] = _rc_throttle->radio_min;
+			_rc->OutputCh(_motor_to_channel_map[i], motor_out[i]);
+		}
+	}
+
+	// Force output if instant pwm
+	if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
+		if( instant_pwm_force01 )
+			_rc->Force_Out0_Out1();
+		if( instant_pwm_force23 )
+			_rc->Force_Out2_Out3();
+		if( instant_pwm_force67 )
+			_rc->Force_Out6_Out7();
+	}
+}
+
+// output_armed - sends commands to the motors
+void AP_MotorsMatrix::output_armed()
+{
+	int8_t i;
+	int16_t out_min = _rc_throttle->radio_min;
+	int16_t out_max = _rc_throttle->radio_max;
+
+	// Throttle is 0 to 1000 only
+	_rc_throttle->servo_out = constrain(_rc_throttle->servo_out, 0, _max_throttle);
+
+	if(_rc_throttle->servo_out > 0)
+		out_min = _rc_throttle->radio_min + _min_throttle;
+
+	// capture desired roll, pitch, yaw and throttle from receiver
+	_rc_roll->calc_pwm();
+	_rc_pitch->calc_pwm();
+	_rc_throttle->calc_pwm();
+	_rc_yaw->calc_pwm();
+
+	// mix roll, pitch, yaw, throttle into output for each motor
+	for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		if( motor_enabled[i] ) {
+			motor_out[i] = _rc_throttle->radio_out +
+							_rc_roll->pwm_out * _roll_factor[i] +
+							_rc_pitch->pwm_out * _pitch_factor[i] +
+							_rc_yaw->pwm_out*_yaw_factor[i];
+		}
+		// ensure motor is not below the minimum
+		motor_out[AP_MOTORS_MOT_1]	= max(motor_out[AP_MOTORS_MOT_1], 	out_min);
+	}
+
+	// stability patch
+	for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		if( motor_enabled[i] && opposite_motor[i] != AP_MOTORS_MATRIX_MOTOR_UNDEFINED && motor_out[i] > out_max ) {
+			motor_out[opposite_motor[i]] -= motor_out[i] - out_max;
+			motor_out[i] = out_max;
+		}
+	}
+
+	// ensure motors are not below the minimum
+	for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		if( motor_enabled[i] ) {
+			motor_out[i] = max(motor_out[i], out_min);
+		}
+	}
+
+	#if CUT_MOTORS == ENABLED
+	// if we are not sending a throttle output, we cut the motors
+	if(_rc_throttle->servo_out == 0){
+		for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+			if( motor_enabled[i] ) {
+				motor_out[i]	= _rc_throttle->radio_min;
+			}
+		}
+	}
+	#endif
+
+	// send output to each motor
+	for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		if( motor_enabled[i] ) {
+			_rc->OutputCh(_motor_to_channel_map[i], motor_out[i]);
+		}
+	}
+
+	// InstantPWM
+	if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
+		if( instant_pwm_force01 )
+			_rc->Force_Out0_Out1();
+		if( instant_pwm_force23 )
+			_rc->Force_Out2_Out3();
+		if( instant_pwm_force67 )
+			_rc->Force_Out6_Out7();
+	}
+}
+
+// output_disarmed - sends commands to the motors
+void AP_MotorsMatrix::output_disarmed()
+{
+	if(_rc_throttle->control_in > 0){
+		// we have pushed up the throttle
+		// remove safety for auto pilot
+		_auto_armed = true;
+	}
+
+	// Send minimum values to all motors
+	output_min();
+}
+
+// output_disarmed - sends commands to the motors
+void AP_MotorsMatrix::output_test()
+{
+	int8_t min_order, max_order;
+	int8_t i,j;
+
+	// find min and max orders
+	min_order = test_order[0];
+	max_order = test_order[0];
+	for(i=1; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		if( test_order[i] < min_order )
+			min_order = test_order[i];
+		if( test_order[i] > max_order )
+			max_order = test_order[i];
+	}
+
+	// shut down all motors
+	output_min();
+
+	// first delay is longer
+	delay(4000);
+	
+	// loop through all the possible orders spinning any motors that match that description
+	for( i=min_order; i<=max_order; i++ ) {
+		for( j=0; j<AP_MOTORS_MAX_NUM_MOTORS; j++ ) {
+			if( motor_enabled[j] && test_order[j] == i ) {
+				// turn on this motor and wait 1/3rd of a second
+				_rc->OutputCh(_motor_to_channel_map[j], _rc_throttle->radio_min + 100);
+				delay(300);
+				_rc->OutputCh(_motor_to_channel_map[j], _rc_throttle->radio_min);
+				delay(2000);
+			}
+		}
+	}
+
+	// shut down all motors
+	output_min();
+}
+
+// add_motor
+void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, int8_t opposite_motor_num, int8_t testing_order)
+{
+	// ensure valid motor number is provided
+	if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) {
+	
+		// increment number of motors if this motor is being newly motor_enabled
+		if( !motor_enabled[motor_num] ) {
+			motor_enabled[motor_num] = true;
+			_num_motors++;
+		}
+
+		// set roll, pitch, thottle factors and opposite motor (for stability patch)
+		_roll_factor[motor_num] = roll_fac;
+		_pitch_factor[motor_num] = pitch_fac;
+		_yaw_factor[motor_num] = yaw_fac;
+
+		// set opposite motor after checking it's somewhat valid
+		if( opposite_motor_num == AP_MOTORS_MATRIX_MOTOR_UNDEFINED || (opposite_motor_num >=0 && opposite_motor_num < AP_MOTORS_MAX_NUM_MOTORS) ) {
+			opposite_motor[motor_num] = opposite_motor_num;
+		}else{
+			opposite_motor[motor_num] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
+		}
+
+		// set order that motor appears in test
+		if( testing_order == AP_MOTORS_MATRIX_ORDER_UNDEFINED ) {
+			test_order[motor_num] = motor_num;
+		}else{
+			test_order[motor_num] = testing_order;
+		}
+	}
+}
+
+// add_motor using just position and prop direction
+void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, int8_t direction, int8_t opposite_motor_num, int8_t testing_order)
+{
+	// call raw motor set-up method
+	add_motor_raw(
+		motor_num,
+		cos(radians(angle_degrees + 90)),	// roll factor
+		cos(radians(angle_degrees)),		// pitch factor
+		(float)direction,					// yaw factor
+		opposite_motor_num,
+		testing_order);
+
+}
+
+// remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor
+void AP_MotorsMatrix::remove_motor(int8_t motor_num)
+{
+	int8_t i;
+
+	// ensure valid motor number is provided
+	if( motor_num >= 0 && motor_num <= AP_MOTORS_MAX_NUM_MOTORS ) {
+
+		// if the motor was enabled decrement the number of motors
+		if( motor_enabled[motor_num] )
+			_num_motors--;
+
+		// disable the motor, set all factors to zero
+		motor_enabled[motor_num] = false;
+		_roll_factor[motor_num] = 0;
+		_pitch_factor[motor_num] = 0;
+		_yaw_factor[motor_num] = 0;
+		opposite_motor[motor_num] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
+	}
+
+	// if another motor has referred to this motor as it's opposite, remove that reference 
+	for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		if( opposite_motor[i] == motor_num )
+			opposite_motor[i] = AP_MOTORS_MATRIX_MOTOR_UNDEFINED;
+	}
+}
+
+// remove_all_motors - removes all motor definitions
+void AP_MotorsMatrix::remove_all_motors()
+{
+	for( int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+		remove_motor(i);
+	}
+	_num_motors = 0;
+}
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..758ebff738c2bd932b2399f50cc0b3c3edd8d464
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsMatrix.h
@@ -0,0 +1,93 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AP_MotorsMatrix.h
+/// @brief	Motor control class for Matrixcopters
+
+#ifndef AP_MOTORSMATRIX
+#define AP_MOTORSMATRIX
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+#include <AP_Motors.h>
+
+#define AP_MOTORS_MATRIX_MOTOR_UNDEFINED -1
+#define AP_MOTORS_MATRIX_ORDER_UNDEFINED -1
+
+#define AP_MOTORS_MATRIX_MOTOR_CW -1
+#define AP_MOTORS_MATRIX_MOTOR_CCW 1
+
+/// @class      AP_MotorsMatrix 
+class AP_MotorsMatrix : public AP_Motors { 
+public: 
+
+	/// Constructor 
+	AP_MotorsMatrix( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
+		AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
+		instant_pwm_force01(false),
+		instant_pwm_force23(false),
+		instant_pwm_force67(false),
+		_num_motors(0) {};
+
+	// init
+	virtual void Init();
+
+	// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
+	// you must have setup_motors before calling this
+	virtual void set_update_rate( uint16_t speed_hz );
+
+	// set frame orientation (normally + or X)
+	virtual void set_frame_orientation( uint8_t new_orientation );
+
+	// enable - starts allowing signals to be sent to motors
+	virtual void enable();
+
+	// get basic information about the platform
+	virtual uint8_t get_num_motors() { return _num_motors; };
+
+	// motor test
+	virtual void output_test();
+ 
+ 	// output_min - sends minimum values out to the motors
+	virtual void output_min();
+
+	// add_motor using just position and prop direction
+	virtual void add_motor(int8_t motor_num, float angle_degrees, int8_t direction, int8_t opposite_motor_num = AP_MOTORS_MATRIX_MOTOR_UNDEFINED, int8_t testing_order = AP_MOTORS_MATRIX_ORDER_UNDEFINED);
+
+	// remove_motor
+	virtual void remove_motor(int8_t motor_num);
+
+	// remove_all_motors - removes all motor definitions
+	virtual void remove_all_motors();
+
+	// setup_motors - configures the motors for a given frame type - should be overwritten by child classes
+	virtual void setup_motors() { remove_all_motors(); };
+
+	// matrix
+	AP_Int16	angle[AP_MOTORS_MAX_NUM_MOTORS];			// angle in degrees from the front of the copter
+	AP_Int8		direction[AP_MOTORS_MAX_NUM_MOTORS];		// direction of rotation of the motor (-1 = clockwise, +1 = counter clockwise)
+	AP_Int8		opposite_motor[AP_MOTORS_MAX_NUM_MOTORS];	// motor number of the opposite motor
+	AP_Int8		test_order[AP_MOTORS_MAX_NUM_MOTORS];		// order of the motors in the test sequence
+
+	// used for instant_pwm only
+	bool instant_pwm_force01;
+	bool instant_pwm_force23;
+	bool instant_pwm_force67;
+
+protected:
+	// output - sends commands to the motors
+	virtual void output_armed();
+	virtual void output_disarmed();
+
+	// add_motor using raw roll, pitch, throttle and yaw factors
+	virtual void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, int8_t opposite_motor_num = AP_MOTORS_MATRIX_MOTOR_UNDEFINED, int8_t testing_order = AP_MOTORS_MATRIX_ORDER_UNDEFINED);
+
+	int8_t	_num_motors;	// not a very useful variable as you really need to check the motor_enabled array to see which motors are enabled
+	float	_roll_factor[AP_MOTORS_MAX_NUM_MOTORS];		// each motors contribution to roll
+	float	_pitch_factor[AP_MOTORS_MAX_NUM_MOTORS];	// each motors contribution to pitch
+	float	_yaw_factor[AP_MOTORS_MAX_NUM_MOTORS];		// each motors contribution to yaw (normally 1 or -1)
+}; 
+
+#endif  // AP_MOTORSMATRIX
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsOcta.cpp b/libraries/AP_Motors/AP_MotorsOcta.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..33c16963d3d19d1dd879d32fdd5b06eb8ede031e
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsOcta.cpp
@@ -0,0 +1,53 @@
+/*
+	AP_MotorsOcta.cpp - ArduCopter motors library
+	Code by RandyMackay. 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 "AP_MotorsOcta.h"
+
+// setup_motors - configures the motors for a octa
+void AP_MotorsOcta::setup_motors()
+{
+	// call parent
+	AP_MotorsMatrix::setup_motors();
+
+	// hard coded config for supported frames
+	if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
+		// plus frame set-up
+		add_motor(AP_MOTORS_MOT_1,    0,  AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_2, 1);
+		add_motor(AP_MOTORS_MOT_2,  180,  AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_1, 5);
+		add_motor(AP_MOTORS_MOT_3,   45,  AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 2);
+		add_motor(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
+		add_motor(AP_MOTORS_MOT_5,  -45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_4, 8);
+		add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 6);
+		add_motor(AP_MOTORS_MOT_7,  -90, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_8, 7);
+		add_motor(AP_MOTORS_MOT_8,   90, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_7, 3);
+
+	}else if( _frame_orientation == AP_MOTORS_V_FRAME ) {
+		// V frame set-up
+		add_motor_raw(AP_MOTORS_MOT_1,  1.0,  0.34, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_2, 7);
+		add_motor_raw(AP_MOTORS_MOT_2, -1.0, -0.32, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_1, 3);
+		add_motor_raw(AP_MOTORS_MOT_3,  1.0, -0.32, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 6);
+		add_motor_raw(AP_MOTORS_MOT_4, -0.5,  -1.0, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 8);
+		add_motor_raw(AP_MOTORS_MOT_5,  1.0,   1.0, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_4, 8);
+		add_motor_raw(AP_MOTORS_MOT_6, -1.0,  0.34, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 2);
+		add_motor_raw(AP_MOTORS_MOT_7, -1.0,   1.0, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_8, 1);
+		add_motor_raw(AP_MOTORS_MOT_8,  0.5,  -1.0, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_7, 5);
+
+	}else {
+		// X frame set-up
+		add_motor(AP_MOTORS_MOT_1,   22.5, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_2, 1);
+		add_motor(AP_MOTORS_MOT_2, -157.5, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_1, 5);
+		add_motor(AP_MOTORS_MOT_3,   67.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_6, 2);
+		add_motor(AP_MOTORS_MOT_4,  157.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
+		add_motor(AP_MOTORS_MOT_5,  -22.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_4, 8);
+		add_motor(AP_MOTORS_MOT_6, -112.5, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 6);
+		add_motor(AP_MOTORS_MOT_7,  -67.5, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_8, 7);
+		add_motor(AP_MOTORS_MOT_8,  112.5, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_7, 3);
+	}
+}
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsOcta.h b/libraries/AP_Motors/AP_MotorsOcta.h
new file mode 100644
index 0000000000000000000000000000000000000000..724391802fc02a1daed379ac0364a82f458b445b
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsOcta.h
@@ -0,0 +1,30 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AP_MotorsOcta.h
+/// @brief	Motor control class for Octacopters
+
+#ifndef AP_MOTORSOCTA
+#define AP_MOTORSOCTA
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+#include <AP_MotorsMatrix.h>	// Parent Motors Matrix library
+
+/// @class      AP_MotorsOcta 
+class AP_MotorsOcta : public AP_MotorsMatrix {
+public: 
+
+	/// Constructor 
+	AP_MotorsOcta( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
+
+	// setup_motors - configures the motors for a quad
+	virtual void setup_motors();
+
+protected:
+
+}; 
+
+#endif  // AP_MOTORSOCTA
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.cpp b/libraries/AP_Motors/AP_MotorsOctaQuad.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..55443fc1119f7e4208241fc536239dbba489b5b7
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsOctaQuad.cpp
@@ -0,0 +1,41 @@
+/*
+	AP_MotorsOctaQuad.cpp - ArduCopter motors library
+	Code by RandyMackay. 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 "AP_MotorsOctaQuad.h"
+
+// setup_motors - configures the motors for a octa
+void AP_MotorsOctaQuad::setup_motors()
+{
+	// call parent
+	AP_MotorsMatrix::setup_motors();
+
+	// hard coded config for supported frames
+	if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
+		// plus frame set-up
+		add_motor(AP_MOTORS_MOT_1,    0, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 1);
+		add_motor(AP_MOTORS_MOT_2,  -90, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_4, 7);
+		add_motor(AP_MOTORS_MOT_3,  180, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 5);
+		add_motor(AP_MOTORS_MOT_4,   90, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_2, 4);
+		add_motor(AP_MOTORS_MOT_5,  -90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_7, 8);
+		add_motor(AP_MOTORS_MOT_6,    0, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_8, 2);
+		add_motor(AP_MOTORS_MOT_7,   90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
+		add_motor(AP_MOTORS_MOT_8,  180, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_6, 6);
+	}else{
+		// X frame set-up
+		add_motor(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_3, 1);
+		add_motor(AP_MOTORS_MOT_2,  -45, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_4, 7);
+		add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 5);
+		add_motor(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_2, 4);
+		add_motor(AP_MOTORS_MOT_5,  -45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_7, 8);
+		add_motor(AP_MOTORS_MOT_6,   45, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_8, 2);
+		add_motor(AP_MOTORS_MOT_7,  135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_5, 4);
+		add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_6, 6);
+	}
+}
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.h b/libraries/AP_Motors/AP_MotorsOctaQuad.h
new file mode 100644
index 0000000000000000000000000000000000000000..e7179ebae7a4dbea1fd599896cfc4453e3f66857
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsOctaQuad.h
@@ -0,0 +1,30 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AP_MotorsOctaQuad.h
+/// @brief	Motor control class for OctaQuadcopters
+
+#ifndef AP_MOTORSOCTAQUAD
+#define AP_MOTORSOCTAQUAD
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+#include <AP_MotorsMatrix.h>	// Parent Motors Matrix library
+
+/// @class      AP_MotorsOcta 
+class AP_MotorsOctaQuad : public AP_MotorsMatrix {
+public: 
+
+	/// Constructor 
+	AP_MotorsOctaQuad( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
+
+	// setup_motors - configures the motors for a quad
+	virtual void setup_motors();
+
+protected:
+
+}; 
+
+#endif  // AP_MOTORSOCTAQUAD
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsQuad.cpp b/libraries/AP_Motors/AP_MotorsQuad.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..91ed2a0e3b4c0f0a9b42e3f56f9c46af93a9f618
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsQuad.cpp
@@ -0,0 +1,33 @@
+/*
+	AP_MotorsQuad.cpp - ArduCopter motors library
+	Code by RandyMackay. 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 "AP_MotorsQuad.h"
+
+// setup_motors - configures the motors for a quad
+void AP_MotorsQuad::setup_motors()
+{
+	// call parent
+	AP_MotorsMatrix::setup_motors();
+
+	// hard coded config for supported frames
+	if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
+		// plus frame set-up
+		add_motor(AP_MOTORS_MOT_1,  90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_2, 2);
+		add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 4);
+		add_motor(AP_MOTORS_MOT_3,   0, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_4, 1);
+		add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_3, 3);
+	}else{
+		// X frame set-up
+		add_motor(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_2, 1);
+		add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MOT_1, 3);
+		add_motor(AP_MOTORS_MOT_3,  -45, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_4, 4);
+		add_motor(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MOT_3, 2);
+	}
+}
diff --git a/libraries/AP_Motors/AP_MotorsQuad.h b/libraries/AP_Motors/AP_MotorsQuad.h
new file mode 100644
index 0000000000000000000000000000000000000000..3da0b10332cd936566b95a46e7cd553ef29915d1
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsQuad.h
@@ -0,0 +1,30 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AP_MotorsQuad.h
+/// @brief	Motor control class for Quadcopters
+
+#ifndef AP_MOTORSQUAD
+#define AP_MOTORSQUAD
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+#include <AP_MotorsMatrix.h>	// Parent Motors Matrix library
+
+/// @class      AP_MotorsQuad 
+class AP_MotorsQuad : public AP_MotorsMatrix {
+public: 
+
+	/// Constructor 
+	AP_MotorsQuad( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
+
+	// setup_motors - configures the motors for a quad
+	virtual void setup_motors();
+
+protected:
+
+}; 
+
+#endif  // AP_MOTORSQUAD
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1f973dd808a8510d5737af2f22e6f4f631b03e42
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsTri.cpp
@@ -0,0 +1,182 @@
+/*
+	AP_MotorsTri.cpp - ArduCopter motors library
+	Code by RandyMackay. 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 "AP_MotorsTri.h"
+
+// init
+void AP_MotorsTri::Init()
+{
+	// set update rate for the 3 motors (but not the servo on channel 7)
+	set_update_rate(_speed_hz);
+}
+
+// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
+void AP_MotorsTri::set_update_rate( uint16_t speed_hz )
+{
+	// record requested speed
+	_speed_hz = speed_hz;
+
+	// set update rate for the 3 motors (but not the servo on channel 7)
+	if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) {
+		_rc->SetFastOutputChannels(_BV(_motor_to_channel_map[AP_MOTORS_MOT_1]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_2]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_4]), _speed_hz);
+	}
+}
+
+	// enable - starts allowing signals to be sent to motors
+void AP_MotorsTri::enable()
+{
+	// enable output channels
+	_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_1]);
+	_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_2]);
+	_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_4]);
+	_rc->enable_out(AP_MOTORS_CH_TRI_YAW);
+}
+
+// output_min - sends minimum values out to the motors
+void AP_MotorsTri::output_min()
+{
+	// fill the motor_out[] array for HIL use
+	motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min;
+	motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min;
+	motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min;
+
+	// send minimum value to each motor
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_CH_TRI_YAW], _rc_throttle->radio_trim);
+
+	// InstantPWM
+	if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
+		_rc->Force_Out0_Out1();
+		_rc->Force_Out2_Out3();
+	}
+}
+
+// output_armed - sends commands to the motors
+void AP_MotorsTri::output_armed()
+{
+	int16_t out_min = _rc_throttle->radio_min;
+	int16_t out_max = _rc_throttle->radio_max;
+
+	// Throttle is 0 to 1000 only
+	_rc_throttle->servo_out = constrain(_rc_throttle->servo_out, 0, _max_throttle);
+
+	if(_rc_throttle->servo_out > 0)
+		out_min = _rc_throttle->radio_min + _min_throttle;
+
+	// capture desired roll, pitch, yaw and throttle from receiver
+	_rc_roll->calc_pwm();
+	_rc_pitch->calc_pwm();
+	_rc_throttle->calc_pwm();
+	_rc_yaw->calc_pwm();
+
+	int roll_out 		= (float)_rc_roll->pwm_out * .866;
+	int pitch_out 		= _rc_pitch->pwm_out / 2;
+
+	//left front
+	motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_out + roll_out + pitch_out;
+	//right front
+	motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_out - roll_out + pitch_out;
+	// rear
+	motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_out - _rc_pitch->pwm_out;
+
+	// Tridge's stability patch
+	if(motor_out[AP_MOTORS_MOT_1] > out_max){
+		motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_1] - out_max) >> 1;
+		motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_1] - out_max) >> 1;
+		motor_out[AP_MOTORS_MOT_1] = out_max;
+	}
+
+	if(motor_out[AP_MOTORS_MOT_2] > out_max){
+		motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_2] - out_max) >> 1;
+		motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_2] - out_max) >> 1;
+		motor_out[AP_MOTORS_MOT_2] = out_max;
+	}
+
+	if(motor_out[AP_MOTORS_MOT_4] > out_max){
+		motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_4] - out_max) >> 1;
+		motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_4] - out_max) >> 1;
+		motor_out[AP_MOTORS_MOT_4] = out_max;
+	}
+
+	// ensure motors don't drop below a minimum value and stop
+	motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], 	out_min);
+	motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2], 	out_min);
+	motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4], 	out_min);
+
+	#if CUT_MOTORS == ENABLED
+	// if we are not sending a throttle output, we cut the motors
+	if(_rc_throttle->servo_out == 0){
+		motor_out[AP_MOTORS_MOT_1]	= _rc_throttle->radio_min;
+		motor_out[AP_MOTORS_MOT_2]	= _rc_throttle->radio_min;
+		motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min;
+	}
+	#endif
+
+	// send output to each motor
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
+
+	// also send out to tail command (we rely on any auto pilot to have updated the rc_yaw->radio_out to the correct value)
+	// note we do not save the radio_out to the motor_out array so it may not appear in the ch7out in the status screen of the mission planner
+	_rc->OutputCh(AP_MOTORS_CH_TRI_YAW, _rc_yaw->radio_out);
+		
+	// InstantPWM
+	if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) {
+		_rc->Force_Out0_Out1();
+		_rc->Force_Out2_Out3();
+	}
+}
+
+// output_disarmed - sends commands to the motors
+void AP_MotorsTri::output_disarmed()
+{
+	if(_rc_throttle->control_in > 0){
+		// we have pushed up the throttle
+		// remove safety
+		_auto_armed = true;
+	}
+
+	// fill the motor_out[] array for HIL use
+	for (unsigned char i = AP_MOTORS_MOT_1; i < AP_MOTORS_MOT_4; i++){
+		motor_out[i] = _rc_throttle->radio_min;
+	}
+
+	// Send minimum values to all motors
+	output_min();
+}
+
+// output_disarmed - sends commands to the motors
+void AP_MotorsTri::output_test()
+{
+	// Send minimum values to all motors
+	output_min();
+
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min);
+	delay(4000);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min + 100);
+	delay(300);
+
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], _rc_throttle->radio_min);
+	delay(2000);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min + 100);
+	delay(300);
+
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min);
+	delay(2000);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min + 100);
+	delay(300);
+
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], motor_out[AP_MOTORS_MOT_2]);
+	_rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], motor_out[AP_MOTORS_MOT_4]);
+}
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h
new file mode 100644
index 0000000000000000000000000000000000000000..14616ec7b144fecc968807136cf81e30f566f3dc
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsTri.h
@@ -0,0 +1,51 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AP_MotorsTri.h
+/// @brief	Motor control class for Tricopters
+
+#ifndef AP_MOTORSTRI
+#define AP_MOTORSTRI
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+#include <AP_Motors.h>
+
+// tail servo uses channel 7
+#define AP_MOTORS_CH_TRI_YAW	CH_7
+
+/// @class      AP_MotorsTri 
+class AP_MotorsTri : public AP_Motors { 
+public: 
+
+	/// Constructor 
+	AP_MotorsTri( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
+
+	// init
+	virtual void Init();
+
+	// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
+	void set_update_rate( uint16_t speed_hz );
+
+	// enable - starts allowing signals to be sent to motors
+	virtual void enable();
+
+	// get basic information about the platform
+	virtual uint8_t get_num_motors() { return 4; };	// 3 motors + 1 tail servo
+
+	// motor test
+	virtual void output_test();
+ 
+ 	// output_min - sends minimum values out to the motors
+	virtual void output_min();
+
+protected:
+	// output - sends commands to the motors
+	virtual void output_armed();
+	virtual void output_disarmed();
+
+}; 
+
+#endif  // AP_MOTORSTRI
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsY6.cpp b/libraries/AP_Motors/AP_MotorsY6.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..00702574b021642689e65e00ee70b466ae6706d9
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsY6.cpp
@@ -0,0 +1,26 @@
+/*
+	AP_MotorsY6.cpp - ArduCopter motors library
+	Code by RandyMackay. 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 "AP_MotorsY6.h"
+
+// setup_motors - configures the motors for a hexa
+void AP_MotorsY6::setup_motors()
+{
+	// call parent
+	AP_MotorsMatrix::setup_motors();
+
+	// MultiWii set-up
+	add_motor_raw(AP_MOTORS_MOT_1, -1.0,  0.666, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 2);
+	add_motor_raw(AP_MOTORS_MOT_2,  1.0,  0.666, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 5);
+	add_motor_raw(AP_MOTORS_MOT_3,  1.0,  0.666, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 6);
+	add_motor_raw(AP_MOTORS_MOT_4,  0.0, -1.333, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 4);
+	add_motor_raw(AP_MOTORS_MOT_5, -1.0,  0.666, AP_MOTORS_MATRIX_MOTOR_CW,  AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 1);
+	add_motor_raw(AP_MOTORS_MOT_6,  0.0, -1.333, AP_MOTORS_MATRIX_MOTOR_CCW, AP_MOTORS_MATRIX_MOTOR_UNDEFINED, 3);
+}
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsY6.h b/libraries/AP_Motors/AP_MotorsY6.h
new file mode 100644
index 0000000000000000000000000000000000000000..712056cce4c7ff8abf0b100172377e5b42bd6a6e
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsY6.h
@@ -0,0 +1,32 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	AP_MotorsY6.h
+/// @brief	Motor control class for Y6 frames
+
+#ifndef AP_MOTORSY6
+#define AP_MOTORSY6
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+#include <AP_MotorsMatrix.h>	// Parent Motors Matrix library
+
+#define AP_MOTORS_Y6_YAW_DIRECTION 1	// this really should be a user selectable parameter
+
+/// @class      AP_MotorsY6 
+class AP_MotorsY6 : public AP_MotorsMatrix {
+public: 
+
+	/// Constructor 
+	AP_MotorsY6( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {};
+
+	// setup_motors - configures the motors for a quad
+	virtual void setup_motors();
+
+protected:
+
+}; 
+
+#endif  // AP_MOTORSY6
\ No newline at end of file
diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde
new file mode 100644
index 0000000000000000000000000000000000000000..e9148cf3030d07f95007b9aea05fcab89134659d
--- /dev/null
+++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde
@@ -0,0 +1,114 @@
+/*
+  Example of AP_Motors library.
+  Code by Randy Mackay. DIYDrones.com
+*/
+
+// AVR runtime
+#include <avr/io.h>
+#include <avr/eeprom.h>
+#include <avr/pgmspace.h>
+#include <math.h>
+
+// Libraries
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+#include <AP_Motors.h>
+#include <AP_MotorsTri.h>
+#include <AP_MotorsQuad.h>
+#include <AP_MotorsHexa.h>
+#include <AP_MotorsY6.h>
+#include <AP_MotorsOcta.h>
+#include <AP_MotorsOctaQuad.h>
+#include <AP_MotorsHeli.h>
+#include <AP_MotorsMatrix.h>
+
+////////////////////////////////////////////////////////////////////////////////
+// Serial ports
+////////////////////////////////////////////////////////////////////////////////
+//
+// Note that FastSerial port buffers are allocated at ::begin time,
+// so there is not much of a penalty to defining ports that we don't
+// use.
+//
+FastSerialPort0(Serial);        // FTDI/console
+
+Arduino_Mega_ISR_Registry isr_registry;
+
+// uncomment one row below depending upon whether you have an APM1 or APM2
+//APM_RC_APM1 APM_RC;
+APM_RC_APM2 APM_RC;
+
+RC_Channel rc1, rc2, rc3, rc4;
+
+// uncomment the row below depending upon what frame you are using
+//AP_MotorsTri	motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
+AP_MotorsQuad	motors(AP_MOTORS_APM2, &APM_RC, &rc1, &rc2, &rc3, &rc4);
+//AP_MotorsHexa	motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
+//AP_MotorsY6	motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
+//AP_MotorsOcta	motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
+//AP_MotorsOctaQuad	motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
+//AP_MotorsHeli	motors(AP_MOTORS_APM1, &APM_RC, &rc1, &rc2, &rc3, &rc4);
+
+
+// setup
+void setup()
+{  
+	Serial.begin(115200);
+	Serial.println("AP_Motors library test ver 1.0");
+
+	RC_Channel::set_apm_rc(&APM_RC);
+	APM_RC.Init( &isr_registry );		// APM Radio initialization
+
+	// motor initialisation
+	motors.set_update_rate(490);
+	motors.set_frame_orientation(AP_MOTORS_X_FRAME);
+	motors.set_min_throttle(130);
+	motors.set_max_throttle(850);
+	motors.Init();  // initialise motors
+
+	motors.enable();
+	motors.output_min();
+
+	delay(1000);
+}
+
+// loop
+void loop()
+{
+	int value;
+
+	// display help
+	Serial.println("Press 't' to test motors.  Becareful they will spin!");
+
+	// wait for user to enter something
+	while( !Serial.available() ) {
+	    delay(20);
+	}
+	
+	// get character from user
+	value = Serial.read();
+
+	// test motors
+	if( value == 't' || value == 'T' ) {
+		Serial.println("testing motors...");
+		motors.armed(true);
+		motors.output_test();
+		motors.armed(false);
+		Serial.println("finished test.");
+	}
+}
+
+// print motor output
+void print_motor_output()
+{
+	int8_t i;
+	for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+		if( motors.motor_enabled[i] ) {
+			Serial.printf("\t%d %d",i,motors.motor_out[i]);
+		}
+	}
+}
\ No newline at end of file