Go to the documentation of this file.00001 #ifndef HECTOR_QUADROTOR_CONTROLLER_PID_H
00002 #define HECTOR_QUADROTOR_CONTROLLER_PID_H
00003
00004 #include <ros/node_handle.h>
00005
00006 namespace hector_quadrotor_controller {
00007
00008 class PID
00009 {
00010 public:
00011 struct parameters {
00012 parameters();
00013 bool enabled;
00014 double time_constant;
00015 double k_p;
00016 double k_i;
00017 double k_d;
00018 double limit_i;
00019 double limit_output;
00020 } parameters_;
00021
00022 struct state {
00023 state();
00024 double p, i, d;
00025 double input, dinput;
00026 double dx;
00027 } state_;
00028
00029 public:
00030 PID();
00031 PID(const parameters& parameters);
00032 ~PID();
00033
00034 void init(const ros::NodeHandle ¶m_nh);
00035 void reset();
00036
00037 double update(double input, double x, double dx, const ros::Duration& dt);
00038 double update(double error, double dx, const ros::Duration& dt);
00039
00040 double getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt);
00041 };
00042
00043 }
00044
00045 #endif // HECTOR_QUADROTOR_CONTROLLER_PID_H