pid.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <hector_quadrotor_controller/pid.h>
00030 #include <limits>
00031 
00032 namespace hector_quadrotor_controller {
00033 
00034 PID::state::state()
00035   : p(std::numeric_limits<double>::quiet_NaN())
00036   , i(0.0)
00037   , d(std::numeric_limits<double>::quiet_NaN())
00038   , input(std::numeric_limits<double>::quiet_NaN())
00039   , dinput(0.0)
00040   , dx(std::numeric_limits<double>::quiet_NaN())
00041 {
00042 }
00043 
00044 PID::parameters::parameters()
00045   : enabled(true)
00046   , time_constant(0.0)
00047   , k_p(0.0)
00048   , k_i(0.0)
00049   , k_d(0.0)
00050   , limit_i(std::numeric_limits<double>::quiet_NaN())
00051   , limit_output(std::numeric_limits<double>::quiet_NaN())
00052 {
00053 }
00054 
00055 PID::PID()
00056 {}
00057 
00058 PID::PID(const parameters& params)
00059   : parameters_(params)
00060 {}
00061 
00062 PID::~PID()
00063 {}
00064 
00065 void PID::init(const ros::NodeHandle &param_nh)
00066 {
00067   param_nh.getParam("enabled", parameters_.enabled);
00068   param_nh.getParam("k_p", parameters_.k_p);
00069   param_nh.getParam("k_i", parameters_.k_i);
00070   param_nh.getParam("k_d", parameters_.k_d);
00071   param_nh.getParam("limit_i", parameters_.limit_i);
00072   param_nh.getParam("limit_output", parameters_.limit_output);
00073   param_nh.getParam("time_constant", parameters_.time_constant);
00074 }
00075 
00076 void PID::reset()
00077 {
00078   state_ = state();
00079 }
00080 
00081 template <typename T> static inline T& checknan(T& value)
00082 {
00083   if (std::isnan(value)) value = T();
00084   return value;
00085 }
00086 
00087 double PID::update(double input, double x, double dx, const ros::Duration& dt)
00088 {
00089   if (!parameters_.enabled) return 0.0;
00090   double dt_sec = dt.toSec();
00091 
00092   // low-pass filter input
00093   if (std::isnan(state_.input)) state_.input = input;
00094   if (dt_sec + parameters_.time_constant > 0.0) {
00095     state_.dinput = (input - state_.input) / (dt_sec + parameters_.time_constant);
00096     state_.input  = (dt_sec * input + parameters_.time_constant * state_.input) / (dt_sec + parameters_.time_constant);
00097   }
00098 
00099   return update(state_.input - x, dx, dt);
00100 }
00101 
00102 double PID::update(double error, double dx, const ros::Duration& dt)
00103 {
00104   if (!parameters_.enabled) return 0.0;
00105   if (std::isnan(error)) return 0.0;
00106   double dt_sec = dt.toSec();
00107 
00108   // integral error
00109   state_.i += error * dt_sec;
00110   if (parameters_.limit_i > 0.0)
00111   {
00112     if (state_.i >  parameters_.limit_i) state_.i =  parameters_.limit_i;
00113     if (state_.i < -parameters_.limit_i) state_.i = -parameters_.limit_i;
00114   }
00115 
00116   // differential error
00117   if (dt_sec > 0.0 && !std::isnan(state_.p) && !std::isnan(state_.dx)) {
00118     state_.d = (error - state_.p) / dt_sec + state_.dx - dx;
00119   } else {
00120     state_.d = -dx;
00121   }
00122   state_.dx = dx;
00123 
00124   // proportional error
00125   state_.p = error;
00126 
00127   // calculate output...
00128   double output = parameters_.k_p * state_.p + parameters_.k_i * state_.i + parameters_.k_d * state_.d;
00129   int antiwindup = 0;
00130   if (parameters_.limit_output > 0.0)
00131   {
00132     if (output >  parameters_.limit_output) { output =  parameters_.limit_output; antiwindup =  1; }
00133     if (output < -parameters_.limit_output) { output = -parameters_.limit_output; antiwindup = -1; }
00134   }
00135   if (antiwindup && (error * dt_sec * antiwindup > 0.0)) state_.i -= error * dt_sec;
00136 
00137   checknan(output);
00138   return output;
00139 }
00140 
00141 double PID::getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt)
00142 {
00143   double dt_sec = dt.toSec();
00144   filtered_error = checknan(filtered_error);
00145   if (dt_sec + time_constant > 0.0) {
00146     filtered_error = (time_constant * filtered_error + dt_sec * state_.p) / (dt_sec + time_constant);
00147   }
00148   return filtered_error;
00149 }
00150 
00151 } // namespace hector_quadrotor_controller
00152 


hector_quadrotor_controller
Author(s): Johannes Meyer
autogenerated on Sun Jul 10 2016 04:49:53