pid_controller.cpp
Go to the documentation of this file.
00001 
00006 /*
00007  * Copyright 2015 Nuno Marques.
00008  *
00009  * This file is part of the mavros package and subject to the license terms
00010  * in the top-level LICENSE file of the mavros repository.
00011  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00012  */
00013 
00014 #include <test_mavros/utils/pid_controller.h>
00015 
00016 using namespace pidcontroller;
00017 
00018 PIDController::PIDController()
00019 {};
00020 
00021 void PIDController::setup_linvel_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min, const ros::NodeHandle &node){
00022         // PID values
00023         std::array<double, 3> linvel_pid = { {p_gain, i_gain, d_gain} };
00024 
00025         // Min/max bounds for the integral windup
00026         double linvel_imin = i_min;
00027         double linvel_imax = i_max;
00028 
00029 #ifdef CONTROL_TOOLBOX_PRE_1_14
00030         pid_linvel_x.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, node);
00031         pid_linvel_y.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, node);
00032         pid_linvel_z.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, node);
00033 #else
00034         pid_linvel_x.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, false, node);
00035         pid_linvel_y.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, false, node);
00036         pid_linvel_z.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, false, node);
00037 #endif
00038 }
00039 
00040 void PIDController::setup_yawrate_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min, const ros::NodeHandle &node){
00041         // PID values
00042         std::array<double, 3> yawrate_pid = { {p_gain, i_gain, d_gain} };
00043 
00044         // Min/max bounds for the integral windup
00045         double yawrate_imin = i_min;
00046         double yawrate_imax = i_max;
00047 
00048 #ifdef CONTROL_TOOLBOX_PRE_1_14
00049         pid_yaw_rate.initPid(yawrate_pid[0], yawrate_pid[1], yawrate_pid[2], yawrate_imax, yawrate_imin, node);
00050 #else
00051         pid_yaw_rate.initPid(yawrate_pid[0], yawrate_pid[1], yawrate_pid[2], yawrate_imax, yawrate_imin, false, node);
00052 #endif
00053 }
00054 
00055 Eigen::Vector3d PIDController::compute_linvel_effort(Eigen::Vector3d goal, Eigen::Vector3d current, ros::Time last_time){
00056         double lin_vel_x = pid_linvel_x.computeCommand(goal.x() - current.x(), ros::Time::now() - last_time);
00057         double lin_vel_y = pid_linvel_y.computeCommand(goal.y() - current.y(), ros::Time::now() - last_time);
00058         double lin_vel_z = pid_linvel_z.computeCommand(goal.z() - current.z(), ros::Time::now() - last_time);
00059 
00060         return Eigen::Vector3d(lin_vel_x, lin_vel_y, lin_vel_z);
00061 }
00062 
00063 double PIDController::compute_yawrate_effort(double goal, double current, ros::Time last_time){
00064         double yaw_rate = pid_yaw_rate.computeCommand(goal - current, ros::Time::now() - last_time);
00065 
00066         return yaw_rate;
00067 }


test_mavros
Author(s): Nuno Marques , Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:26