diff --git a/libraries/APM_Control/APM_Control.h b/libraries/APM_Control/APM_Control.h
index 4de57d002c..3f972f2232 100644
--- a/libraries/APM_Control/APM_Control.h
+++ b/libraries/APM_Control/APM_Control.h
@@ -1,3 +1,4 @@
#include "AP_RollController.h"
#include "AP_PitchController.h"
-#include "AP_YawController.h"
\ No newline at end of file
+#include "AP_YawController.h"
+#include "AP_SteerController.h"
diff --git a/libraries/APM_Control/AP_SteerController.cpp b/libraries/APM_Control/AP_SteerController.cpp
new file mode 100644
index 0000000000..a626f46253
--- /dev/null
+++ b/libraries/APM_Control/AP_SteerController.cpp
@@ -0,0 +1,145 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+// Code by Andrew Tridgell
+// Based upon the roll controller by Paul Riseborough and Jon Challinger
+//
+
+#include
+#include
+#include "AP_SteerController.h"
+
+extern const AP_HAL::HAL& hal;
+
+const AP_Param::GroupInfo AP_SteerController::var_info[] PROGMEM = {
+ // @Param: T_CONST
+ // @DisplayName: Steering Time Constant
+ // @Description: This controls the time constant in seconds from demanded to achieved bank angle. A value of 0.5 is a good default and will work with nearly all models. Advanced users may want to reduce this time to obtain a faster response but there is no point setting a time less than the aircraft can achieve.
+ // @Range: 0.4 1.0
+ // @Units: seconds
+ // @Increment: 0.1
+ // @User: Advanced
+ AP_GROUPINFO("TCONST", 0, AP_SteerController, _tau, 0.5f),
+
+ // @Param: P
+ // @DisplayName: Steering turning gain
+ // @Description: The proportional gain for steering. This should be approximately equal to the diameter of the turning circle of the vehicle at low speed and maximum steering angle
+ // @Range: 0.1 10.0
+ // @Increment: 0.1
+ // @User: User
+ AP_GROUPINFO("P", 1, AP_SteerController, _K_P, 1.8f),
+
+ // @Param: I
+ // @DisplayName: Integrator Gain
+ // @Description: This is the gain from the integral of steering angle. Increasing this gain causes the controller to trim out steady offsets due to an out of trim vehicle.
+ // @Range: 0 1.0
+ // @Increment: 0.05
+ // @User: User
+ AP_GROUPINFO("I", 3, AP_SteerController, _K_I, 0.2f),
+
+ // @Param: D
+ // @DisplayName: Damping Gain
+ // @Description: This adjusts the damping of the steering control loop. This gain helps to reduce steering jitter with vibration. It should be increased in 0.01 increments as too high a value can lead to a high frequency steering oscillation that could overstress the vehicle.
+ // @Range: 0 0.1
+ // @Increment: 0.01
+ // @User: User
+ AP_GROUPINFO("D", 4, AP_SteerController, _K_D, 0.02f),
+
+ // @Param: IMAX
+ // @DisplayName: Integrator limit
+ // @Description: This limits the number of degrees of steering in centi-degrees over which the integrator will operate. At the default setting of 1500 centi-degrees, the integrator will be limited to +- 15 degrees of servo travel. The maximum servo deflection is +- 45 centi-degrees, so the default value represents a 1/3rd of the total control throw which is adequate unless the vehicle is severely out of trim.
+ // @Range: 0 4500
+ // @Increment: 1
+ // @User: Advanced
+ AP_GROUPINFO("IMAX", 5, AP_SteerController, _imax, 1500),
+
+ AP_GROUPEND
+};
+
+
+/*
+ internal rate controller, called by attitude and rate controller
+ public functions
+*/
+int32_t AP_SteerController::get_steering_out(float desired_accel)
+{
+ uint32_t tnow = hal.scheduler->millis();
+ uint32_t dt = tnow - _last_t;
+ if (_last_t == 0 || dt > 1000) {
+ dt = 0;
+ }
+ _last_t = tnow;
+
+ float speed = _ahrs.groundspeed();
+ if (speed < 1.0e-6) {
+ // with no speed all we can do is center the steering
+ return 0;
+ }
+
+ // this is a linear approximation of the inverse steering
+ // equation for a ground vehicle. It returns steering as an angle from -45 to 45
+ float scaler = 1.0f / speed;
+
+ // Calculate the steering rate error (deg/sec) and apply gain scaler
+ float desired_rate = desired_accel / speed;
+ float rate_error = (ToDeg(desired_rate) - ToDeg(_ahrs.get_gyro().z)) * scaler;
+
+ // Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
+ // No conversion is required for K_D
+ float ki_rate = _K_I * _tau * 45.0f;
+ float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0) * 45.0f;
+ float delta_time = (float)dt * 0.001f;
+
+ // Multiply roll rate error by _ki_rate and integrate
+ // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
+ if (ki_rate > 0) {
+ // only integrate if gain and time step are positive.
+ if (dt > 0) {
+ float integrator_delta = rate_error * ki_rate * delta_time;
+ // prevent the integrator from increasing if steering defln demand is above the upper limit
+ if (_last_out < -45) {
+ integrator_delta = max(integrator_delta , 0);
+ } else if (_last_out > 45) {
+ // prevent the integrator from decreasing if steering defln demand is below the lower limit
+ integrator_delta = min(integrator_delta, 0);
+ }
+ _integrator += integrator_delta;
+ }
+ } else {
+ _integrator = 0;
+ }
+
+ // Scale the integration limit
+ float intLimScaled = _imax * 0.01f / scaler;
+
+ // Constrain the integrator state
+ _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
+
+ // Calculate the demanded control surface deflection
+ // Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
+ // path, but want a 1/speed^2 scaler applied to the rate error path.
+ // This is because acceleration scales with speed^2, but rate scales with speed.
+ _last_out = ( (rate_error * _K_D * 45.0f) + _integrator + (desired_rate * kp_ff) ) * scaler;
+
+ // Convert to centi-degrees and constrain
+ return constrain_float(_last_out * 100, -4500, 4500);
+}
+
+void AP_SteerController::reset_I()
+{
+ _integrator = 0;
+}
+
diff --git a/libraries/APM_Control/AP_SteerController.h b/libraries/APM_Control/AP_SteerController.h
new file mode 100644
index 0000000000..59ce32e910
--- /dev/null
+++ b/libraries/APM_Control/AP_SteerController.h
@@ -0,0 +1,43 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_STEER_CONTROLLER_H__
+#define __AP_STEER_CONTROLLER_H__
+
+#include
+#include
+#include
+#include
+
+class AP_SteerController {
+public:
+ AP_SteerController(AP_AHRS &ahrs) :
+ _ahrs(ahrs)
+ {
+ AP_Param::setup_object_defaults(this, var_info);
+ }
+
+ /*
+ return a steering servo output from -4500 to 4500 given a
+ desired lateral acceleration rate in m/s/s.
+ */
+ int32_t get_steering_out(float desired_accel);
+
+ void reset_I();
+
+ static const struct AP_Param::GroupInfo var_info[];
+
+private:
+ AP_Float _tau;
+ AP_Float _K_P;
+ AP_Float _K_I;
+ AP_Float _K_D;
+ AP_Int16 _imax;
+ uint32_t _last_t;
+ float _last_out;
+
+ float _integrator;
+
+ AP_AHRS &_ahrs;
+};
+
+#endif // __AP_STEER_CONTROLLER_H__