pid_controller.cpp
Go to the documentation of this file.
1 
6 /*
7  * Copyright 2015 Nuno Marques.
8  *
9  * This file is part of the mavros package and subject to the license terms
10  * in the top-level LICENSE file of the mavros repository.
11  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
12  */
13 
15 
16 using namespace pidcontroller;
17 
19 {};
20 
21 void PIDController::setup_linvel_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min, const ros::NodeHandle &node){
22  // PID values
23  std::array<double, 3> linvel_pid = { {p_gain, i_gain, d_gain} };
24 
25  // Min/max bounds for the integral windup
26  double linvel_imin = i_min;
27  double linvel_imax = i_max;
28 
29 #ifdef CONTROL_TOOLBOX_PRE_1_14
30  pid_linvel_x.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, node);
31  pid_linvel_y.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, node);
32  pid_linvel_z.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, node);
33 #else
34  pid_linvel_x.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, false, node);
35  pid_linvel_y.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, false, node);
36  pid_linvel_z.initPid(linvel_pid[0], linvel_pid[1], linvel_pid[2], linvel_imax, linvel_imin, false, node);
37 #endif
38 }
39 
40 void PIDController::setup_yawrate_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min, const ros::NodeHandle &node){
41  // PID values
42  std::array<double, 3> yawrate_pid = { {p_gain, i_gain, d_gain} };
43 
44  // Min/max bounds for the integral windup
45  double yawrate_imin = i_min;
46  double yawrate_imax = i_max;
47 
48 #ifdef CONTROL_TOOLBOX_PRE_1_14
49  pid_yaw_rate.initPid(yawrate_pid[0], yawrate_pid[1], yawrate_pid[2], yawrate_imax, yawrate_imin, node);
50 #else
51  pid_yaw_rate.initPid(yawrate_pid[0], yawrate_pid[1], yawrate_pid[2], yawrate_imax, yawrate_imin, false, node);
52 #endif
53 }
54 
55 Eigen::Vector3d PIDController::compute_linvel_effort(Eigen::Vector3d goal, Eigen::Vector3d current, ros::Time last_time){
56  double lin_vel_x = pid_linvel_x.computeCommand(goal.x() - current.x(), ros::Time::now() - last_time);
57  double lin_vel_y = pid_linvel_y.computeCommand(goal.y() - current.y(), ros::Time::now() - last_time);
58  double lin_vel_z = pid_linvel_z.computeCommand(goal.z() - current.z(), ros::Time::now() - last_time);
59 
60  return Eigen::Vector3d(lin_vel_x, lin_vel_y, lin_vel_z);
61 }
62 
63 double PIDController::compute_yawrate_effort(double goal, double current, ros::Time last_time){
64  double yaw_rate = pid_yaw_rate.computeCommand(goal - current, ros::Time::now() - last_time);
65 
66  return yaw_rate;
67 }
PID controller header.
double compute_yawrate_effort(double goal, double current, ros::Time last_time)
Computes the yaw rate effort to apply.
void setup_yawrate_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min, const ros::NodeHandle &node)
Sets up the PID values for computation of the yaw rate effort.
control_toolbox::Pid pid_linvel_y
void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup=false)
void setup_linvel_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min, const ros::NodeHandle &node)
Sets up the PID values for computation of the linear velocities effort.
double computeCommand(double error, ros::Duration dt)
control_toolbox::Pid pid_yaw_rate
static Time now()
control_toolbox::Pid pid_linvel_z
float current
Eigen::Vector3d compute_linvel_effort(Eigen::Vector3d goal, Eigen::Vector3d current, ros::Time last_time)
Computes the linear velocity effort to apply to each axis.
control_toolbox::Pid pid_linvel_x


test_mavros
Author(s): Nuno Marques , Vladimir Ermakov
autogenerated on Mon Jul 8 2019 03:20:22