PX4 Firmware
PX4 Autopilot Software http://px4.io
ecl_pitch_controller.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.cpp
36  * Implementation of a simple orthogonal pitch PID controller.
37  *
38  * Authors and acknowledgements in header.
39  */
40 
41 #include "ecl_pitch_controller.h"
42 #include <math.h>
43 #include <float.h>
44 #include <geo/geo.h>
45 #include <mathlib/mathlib.h>
46 
48 {
49 
50  /* Do not calculate control signal with bad inputs */
51  if (!(ISFINITE(ctl_data.pitch_setpoint) &&
52  ISFINITE(ctl_data.roll) &&
53  ISFINITE(ctl_data.pitch) &&
54  ISFINITE(ctl_data.airspeed))) {
55  ECL_WARN("not controlling pitch");
56  return _rate_setpoint;
57  }
58 
59  /* Calculate the error */
60  float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
61 
62  /* Apply P controller: rate setpoint from current error and time constant */
63  _rate_setpoint = pitch_error / _tc;
64 
65  return _rate_setpoint;
66 }
67 
69 {
70  /* Do not calculate control signal with bad inputs */
71  if (!(ISFINITE(ctl_data.roll) &&
72  ISFINITE(ctl_data.pitch) &&
73  ISFINITE(ctl_data.body_y_rate) &&
74  ISFINITE(ctl_data.body_z_rate) &&
75  ISFINITE(ctl_data.yaw_rate_setpoint) &&
76  ISFINITE(ctl_data.airspeed_min) &&
77  ISFINITE(ctl_data.airspeed_max) &&
78  ISFINITE(ctl_data.scaler))) {
79  return math::constrain(_last_output, -1.0f, 1.0f);
80  }
81 
82  /* get the usual dt estimate */
83  uint64_t dt_micros = ecl_elapsed_time(&_last_run);
85  float dt = (float)dt_micros * 1e-6f;
86 
87  /* lock integral for long intervals */
88  bool lock_integrator = ctl_data.lock_integrator;
89 
90  if (dt_micros > 500000) {
91  lock_integrator = true;
92  }
93 
95 
96  if (!lock_integrator && _k_i > 0.0f) {
97 
98  float id = _rate_error * dt * ctl_data.scaler;
99 
100  /*
101  * anti-windup: do not allow integrator to increase if actuator is at limit
102  */
103  if (_last_output < -1.0f) {
104  /* only allow motion to center: increase value */
105  id = math::max(id, 0.0f);
106 
107  } else if (_last_output > 1.0f) {
108  /* only allow motion to center: decrease value */
109  id = math::min(id, 0.0f);
110  }
111 
112  /* add and constrain */
114  }
115 
116  /* Apply PI rate controller and store non-limited output */
118  _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
119  + _integrator; //scaler is proportional to 1/airspeed
120 
121  return math::constrain(_last_output, -1.0f, 1.0f);
122 }
123 
125 {
126  /* Transform setpoint to body angular rates (jacobian) */
127  _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
128  cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
129 
131 
132  return control_bodyrate(ctl_data);
133 }
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
Definition of geo / math functions to perform geodesic calculations.
#define ECL_WARN
Definition: ecl.h:91
Definition of a simple orthogonal pitch PID controller.
#define ecl_absolute_time()
Definition: ecl.h:85
#define ecl_elapsed_time(t)
Definition: ecl.h:86
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
float control_attitude(const struct ECL_ControlData &ctl_data) override
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
float _bodyrate_setpoint
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
float dt
Definition: px4io.c:73
float control_euler_rate(const struct ECL_ControlData &ctl_data) override
float control_bodyrate(const struct ECL_ControlData &ctl_data) override
#define ISFINITE(x)
Definition: ecl.h:100
void set_bodyrate_setpoint(float rate)
uint64_t _last_run