From 8fae75ee87ead8232bb744217db58f72d1659798 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 14 Feb 2014 15:53:23 +0900
Subject: [PATCH] AC_PID: add AC_P controller

Used for stabilize angle controllers, alt hold and loiter
---
 libraries/AC_PID/AC_P.cpp | 30 +++++++++++++++++
 libraries/AC_PID/AC_P.h   | 71 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 101 insertions(+)
 create mode 100644 libraries/AC_PID/AC_P.cpp
 create mode 100644 libraries/AC_PID/AC_P.h

diff --git a/libraries/AC_PID/AC_P.cpp b/libraries/AC_PID/AC_P.cpp
new file mode 100644
index 0000000000..a8d3054d69
--- /dev/null
+++ b/libraries/AC_PID/AC_P.cpp
@@ -0,0 +1,30 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file	AC_P.cpp
+/// @brief	Generic P algorithm
+
+#include <AP_Math.h>
+#include "AC_P.h"
+
+const AP_Param::GroupInfo AC_P::var_info[] PROGMEM = {
+    // @Param: P
+    // @DisplayName: PI Proportional Gain
+    // @Description: P Gain which produces an output value that is proportional to the current error value
+    AP_GROUPINFO("P",    0, AC_P, _kp, 0),
+    AP_GROUPEND
+};
+
+float AC_P::get_p(float error) const
+{
+    return (float)error * _kp;
+}
+
+void AC_P::load_gains()
+{
+    _kp.load();
+}
+
+void AC_P::save_gains()
+{
+    _kp.save();
+}
diff --git a/libraries/AC_PID/AC_P.h b/libraries/AC_PID/AC_P.h
new file mode 100644
index 0000000000..be310b7380
--- /dev/null
+++ b/libraries/AC_PID/AC_P.h
@@ -0,0 +1,71 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file	AC_PD.h
+/// @brief	Generic PID algorithm, with EEPROM-backed storage of constants.
+
+#ifndef __AC_P_H__
+#define __AC_P_H__
+
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <stdlib.h>
+#include <math.h>
+
+/// @class	AC_P
+/// @brief	Object managing one P controller
+class AC_P {
+public:
+
+    /// Constructor for P that saves its settings to EEPROM
+    ///
+    /// @note	PIs must be named to avoid either multiple parameters with the
+    ///			same name, or an overly complex constructor.
+    ///
+    /// @param  initial_p       Initial value for the P term.
+    ///
+    AC_P(
+        const float &   initial_p = 0.0)
+    {
+		AP_Param::setup_object_defaults(this, var_info);
+        _kp = initial_p;
+    }
+
+    /// Iterate the P controller, return the new control value
+    ///
+    /// Positive error produces positive output.
+    ///
+    /// @param error	The measured error value
+    /// @param dt		The time delta in milliseconds (note
+    ///					that update interval cannot be more
+    ///					than 65.535 seconds due to limited range
+    ///					of the data type).
+    ///
+    /// @returns		The updated control output.
+    ///
+    float       get_p(float error) const;
+
+    /// Load gain properties
+    ///
+    void        load_gains();
+
+    /// Save gain properties
+    ///
+    void        save_gains();
+
+    /// @name	parameter accessors
+    //@{
+
+    /// Overload the function call operator to permit relatively easy initialisation
+    void operator() (const float p) { _kp = p; }
+
+    // accessors
+    float       kP() const { return _kp.get(); }
+    void        kP(const float v) { _kp.set(v); }
+
+    static const struct AP_Param::GroupInfo        var_info[];
+
+private:
+    AP_Float        _kp;
+};
+
+#endif // __AC_P_H__