PX4 Firmware
PX4 Autopilot Software http://px4.io
RateControl.hpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file RateControl.hpp
36  *
37  * PID 3 axis angular rate / angular velocity control.
38  */
39 
40 #pragma once
41 
42 #include <matrix/matrix/math.hpp>
44 
47 
49 {
50 public:
51  RateControl() = default;
52  ~RateControl() = default;
53 
54  /**
55  * Set the rate control gains
56  * @param P 3D vector of proportional gains for body x,y,z axis
57  * @param I 3D vector of integral gains
58  * @param D 3D vector of derivative gains
59  */
60  void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
61 
62  /**
63  * Set the mximum absolute value of the integrator for all axes
64  * @param integrator_limit limit value for all axes x, y, z
65  */
66  void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
67 
68  /**
69  * Set update frequency and low-pass filter cutoff that is applied to the derivative term
70  * @param loop_rate [Hz] rate with which update function is called
71  * @param cutoff [Hz] cutoff frequency for the low-pass filter on the dervative term
72  * @param force flag to force an expensive update even if the cutoff didn't change
73  */
74  void setDTermCutoff(const float loop_rate, const float cutoff, const bool force);
75 
76  /**
77  * Set direct rate to torque feed forward gain
78  * @see _gain_ff
79  * @param FF 3D vector of feed forward gains for body x,y,z axis
80  */
81  void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
82 
83  /**
84  * Set saturation status
85  * @param status message from mixer reporting about saturation
86  */
88 
89  /**
90  * Run one control loop cycle calculation
91  * @param rate estimation of the current vehicle angular rate
92  * @param rate_sp desired vehicle angular rate setpoint
93  * @param dt desired vehicle angular rate setpoint
94  * @return [-1,1] normalized torque vector to apply to the vehicle
95  */
96  matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt,
97  const bool landed);
98 
99  /**
100  * Set the integral term to 0 to prevent windup
101  * @see _rate_int
102  */
104 
105  /**
106  * Get status message of controller for logging/debugging
107  * @param rate_ctrl_status status message to fill with internal states
108  */
109  void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status);
110 
111 private:
112  void updateIntegral(matrix::Vector3f &rate_error, const float dt);
113 
114  // Gains
115  matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z
116  matrix::Vector3f _gain_i; ///< rate control integral gain
117  matrix::Vector3f _gain_d; ///< rate control derivative gain
118  matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
119  matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
120 
121  // States
122  matrix::Vector3f _rate_prev; ///< angular rates of previous update
123  matrix::Vector3f _rate_prev_filtered; ///< low-pass filtered angular rates of previous update
124  matrix::Vector3f _rate_int; ///< integral term of the rate controller
125  math::LowPassFilter2pVector3f _lp_filters_d{0.f, 0.f}; ///< low-pass filters for D-term (roll, pitch & yaw)
128 };
void setIntegratorLimit(const matrix::Vector3f &integrator_limit)
Set the mximum absolute value of the integrator for all axes.
Definition: RateControl.hpp:66
static struct vehicle_status_s status
Definition: Commander.cpp:138
A class to implement a second order low pass filter on a Vector3f Based on LowPassFilter2p.hpp by Leonard Hall LeonardTHall@gmail.com
matrix::Vector3f _lim_int
integrator term maximum absolute value
matrix::Vector3f _rate_prev_filtered
low-pass filtered angular rates of previous update
matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt, const bool landed)
Run one control loop cycle calculation.
Definition: RateControl.cpp:69
void setSaturationStatus(const MultirotorMixer::saturation_status &status)
Set saturation status.
Definition: RateControl.cpp:59
void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D)
Set the rate control gains.
Definition: RateControl.cpp:43
void setDTermCutoff(const float loop_rate, const float cutoff, const bool force)
Set update frequency and low-pass filter cutoff that is applied to the derivative term...
Definition: RateControl.cpp:50
void resetIntegral()
Set the integral term to 0 to prevent windup.
math::LowPassFilter2pVector3f _lp_filters_d
low-pass filters for D-term (roll, pitch & yaw)
RateControl()=default
matrix::Vector3f _gain_p
rate control proportional gain for all axes x, y, z
bool _mixer_saturation_positive[3]
void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
Get status message of controller for logging/debugging.
bool _mixer_saturation_negative[3]
void zero()
Definition: Matrix.hpp:421
matrix::Vector3f _rate_int
integral term of the rate controller
void setFeedForwardGain(const matrix::Vector3f &FF)
Set direct rate to torque feed forward gain.
Definition: RateControl.hpp:81
~RateControl()=default
matrix::Vector3f _gain_d
rate control derivative gain
float dt
Definition: px4io.c:73
void updateIntegral(matrix::Vector3f &rate_error, const float dt)
Definition: RateControl.cpp:96
matrix::Vector3f _gain_i
rate control integral gain
P[0][0]
Definition: quatCovMat.c:44
matrix::Vector3f _gain_ff
direct rate to torque feed forward gain only useful for helicopters
matrix::Vector3f _rate_prev
angular rates of previous update