From bd848a6a7fd3aec52411ac30cbd2e65e87bd5ea7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 3 Oct 2013 21:54:11 +1000
Subject: [PATCH] APM_Control: added rate and angle steering controllers

---
 libraries/APM_Control/AP_SteerController.cpp | 45 +++++++++++++++++---
 libraries/APM_Control/AP_SteerController.h   | 17 +++++++-
 2 files changed, 54 insertions(+), 8 deletions(-)

diff --git a/libraries/APM_Control/AP_SteerController.cpp b/libraries/APM_Control/AP_SteerController.cpp
index b5010dc1ce..6d2ecfa485 100644
--- a/libraries/APM_Control/AP_SteerController.cpp
+++ b/libraries/APM_Control/AP_SteerController.cpp
@@ -80,10 +80,10 @@ const AP_Param::GroupInfo AP_SteerController::var_info[] PROGMEM = {
 
 
 /*
-  internal rate controller, called by attitude and rate controller
-  public functions
+  steering rate controller. Returns servo out -4500 to 4500 given
+  desired yaw rate in degrees/sec. Positive yaw rate means clockwise yaw.
 */
-int32_t AP_SteerController::get_steering_out(float desired_accel)
+int32_t AP_SteerController::get_steering_out_rate(float desired_rate)
 {
 	uint32_t tnow = hal.scheduler->millis();
 	uint32_t dt = tnow - _last_t;
@@ -103,8 +103,7 @@ int32_t AP_SteerController::get_steering_out(float desired_accel)
     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;
+	float rate_error = (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
@@ -138,12 +137,46 @@ int32_t AP_SteerController::get_steering_out(float desired_accel)
     _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
 	
 	// Calculate the demanded control surface deflection
-	_last_out = (rate_error * _K_D * 4.0f) + (desired_rate * kp_ff) * scaler + _integrator;
+	_last_out = (rate_error * _K_D * 4.0f) + (ToRad(desired_rate) * kp_ff) * scaler + _integrator;
 	
 	// Convert to centi-degrees and constrain
 	return constrain_float(_last_out * 100, -4500, 4500);
 }
 
+
+/*
+  lateral acceleration controller. Returns servo value -4500 to 4500
+  given a desired lateral acceleration
+*/
+int32_t AP_SteerController::get_steering_out_lat_accel(float desired_accel)
+{
+    float speed = _ahrs.groundspeed();
+    if (speed < _minspeed) {
+        // assume a minimum speed. This reduces osciallations when first starting to move
+        speed = _minspeed;
+    }
+
+	// Calculate the desired steering rate given desired_accel and speed
+    float desired_rate = ToDeg(desired_accel / speed);
+    return get_steering_out_rate(desired_rate);
+}
+
+/*
+  return a steering servo value from -4500 to 4500 given an angular
+  steering error in centidegrees.
+*/
+int32_t AP_SteerController::get_steering_out_angle_error(int32_t angle_err)
+{
+    if (_tau < 0.1) {
+        _tau = 0.1;
+    }
+	
+	// Calculate the desired steering rate (deg/sec) from the angle error
+	float desired_rate = angle_err * 0.01f / _tau;
+
+    return get_steering_out_rate(desired_rate);
+}
+
 void AP_SteerController::reset_I()
 {
 	_integrator = 0;
diff --git a/libraries/APM_Control/AP_SteerController.h b/libraries/APM_Control/AP_SteerController.h
index 729937a916..0320c3f609 100644
--- a/libraries/APM_Control/AP_SteerController.h
+++ b/libraries/APM_Control/AP_SteerController.h
@@ -18,9 +18,22 @@ public:
 
     /*
       return a steering servo output from -4500 to 4500 given a
-      desired lateral acceleration rate in m/s/s.
+      desired lateral acceleration rate in m/s/s. Positive lateral
+      acceleration is to the right.
      */
-	int32_t get_steering_out(float desired_accel);
+	int32_t get_steering_out_lat_accel(float desired_accel);
+
+    /*
+      return a steering servo output from -4500 to 4500 given a
+      desired yaw rate in degrees/sec. Positive yaw is to the right.
+     */
+	int32_t get_steering_out_rate(float desired_rate);
+
+    /*
+      return a steering servo output from -4500 to 4500 given a
+      yaw error in centi-degrees
+     */
+	int32_t get_steering_out_angle_error(int32_t angle_err);
 
     /*
       return the steering radius (half diameter). Assumed to be half