PX4 Firmware
PX4 Autopilot Software http://px4.io
ecl_yaw_controller.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2016 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_yaw_controller.cpp
36  * Implementation of a simple orthogonal coordinated turn yaw PID controller.
37  *
38  * Authors and acknowledgements in header.
39  */
40 
41 #include "ecl_yaw_controller.h"
42 #include <float.h>
43 #include <geo/geo.h>
44 #include <mathlib/mathlib.h>
45 
47 {
48  switch (_coordinated_method) {
49  case COORD_METHOD_OPEN:
50  return control_attitude_impl_openloop(ctl_data);
51 
53  return control_attitude_impl_accclosedloop(ctl_data);
54 
55  default:
56  static ecl_abstime last_print = 0;
57 
58  if (ecl_elapsed_time(&last_print) > 5e6) {
59  ECL_WARN("invalid param setting FW_YCO_METHOD");
60  last_print = ecl_absolute_time();
61  }
62  }
63 
64  return _rate_setpoint;
65 }
66 
68 {
69  /* Do not calculate control signal with bad inputs */
70  if (!(ISFINITE(ctl_data.roll) &&
71  ISFINITE(ctl_data.pitch) &&
72  ISFINITE(ctl_data.roll_rate_setpoint) &&
73  ISFINITE(ctl_data.pitch_rate_setpoint))) {
74  return _rate_setpoint;
75  }
76 
77  float constrained_roll;
78  bool inverted = false;
79 
80  /* roll is used as feedforward term and inverted flight needs to be considered */
81  if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
82  /* not inverted, but numerically still potentially close to infinity */
83  constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
84 
85  } else {
86  inverted = true;
87 
88  // inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity
89  //note: the ranges are extended by 10 deg here to avoid numeric resolution effects
90  if (ctl_data.roll > 0.0f) {
91  /* right hemisphere */
92  constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
93 
94  } else {
95  /* left hemisphere */
96  constrained_roll = math::constrain(ctl_data.roll, math::radians(-180.0f), math::radians(-100.0f));
97  }
98  }
99 
100  constrained_roll = math::constrain(constrained_roll, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint));
101 
102 
103  if (!inverted) {
104  /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
105  _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
106  ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
107  }
108 
109  if (!ISFINITE(_rate_setpoint)) {
110  ECL_WARN("yaw rate sepoint not finite");
111  _rate_setpoint = 0.0f;
112  }
113 
114  return _rate_setpoint;
115 }
116 
118 {
119  /* Do not calculate control signal with bad inputs */
120  if (!(ISFINITE(ctl_data.roll) && ISFINITE(ctl_data.pitch) && ISFINITE(ctl_data.body_y_rate) &&
121  ISFINITE(ctl_data.body_z_rate) && ISFINITE(ctl_data.pitch_rate_setpoint) &&
122  ISFINITE(ctl_data.airspeed_min) && ISFINITE(ctl_data.airspeed_max) &&
123  ISFINITE(ctl_data.scaler))) {
124  return math::constrain(_last_output, -1.0f, 1.0f);
125  }
126 
127  /* get the usual dt estimate */
128  uint64_t dt_micros = ecl_elapsed_time(&_last_run);
130  float dt = (float)dt_micros * 1e-6f;
131 
132  /* lock integral for long intervals */
133  bool lock_integrator = ctl_data.lock_integrator;
134 
135  if (dt_micros > 500000) {
136  lock_integrator = true;
137  }
138 
139  /* input conditioning */
140  float airspeed = ctl_data.airspeed;
141 
142  if (!ISFINITE(airspeed)) {
143  /* airspeed is NaN, +- INF or not available, pick center of band */
144  airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
145 
146  } else if (airspeed < ctl_data.airspeed_min) {
147  airspeed = ctl_data.airspeed_min;
148  }
149 
150  /* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
152  // XXX lateral acceleration needs to go into integrator with a gain
153  //_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
154  }
155 
156  /* Calculate body angular rate error */
157  _rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; // body angular rate error
158 
159  if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
160 
161  float id = _rate_error * dt;
162 
163  /*
164  * anti-windup: do not allow integrator to increase if actuator is at limit
165  */
166  if (_last_output < -1.0f) {
167  /* only allow motion to center: increase value */
168  id = math::max(id, 0.0f);
169 
170  } else if (_last_output > 1.0f) {
171  /* only allow motion to center: decrease value */
172  id = math::min(id, 0.0f);
173  }
174 
175  /* add and constrain */
177  }
178 
179  /* Apply PI rate controller and store non-limited output */
181  ctl_data.scaler; //scaler is proportional to 1/airspeed
182 
183 
184  return math::constrain(_last_output, -1.0f, 1.0f);
185 }
186 
188 {
189  (void)ctl_data; // unused
190 
191  /* dont set a rate setpoint */
192  return 0.0f;
193 }
194 
196 {
197  /* Transform setpoint to body angular rates (jacobian) */
198  _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
199  cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
200 
202 
203  return control_bodyrate(ctl_data);
204 
205 }
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
float pitch_rate_setpoint
Definition of geo / math functions to perform geodesic calculations.
float control_bodyrate(const struct ECL_ControlData &ctl_data) override
float control_attitude(const struct ECL_ControlData &ctl_data) override
#define ECL_WARN
Definition: ecl.h:91
static constexpr float CONSTANTS_ONE_G
Definition: geo.h:51
#define ecl_absolute_time()
Definition: ecl.h:85
Definition of a simple orthogonal coordinated turn yaw PID controller.
#define ecl_elapsed_time(t)
Definition: ecl.h:86
constexpr T radians(T degrees)
Definition: Limits.hpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
float _bodyrate_setpoint
float control_euler_rate(const struct ECL_ControlData &ctl_data) override
void set_bodyrate_setpoint(float rate)
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
float dt
Definition: px4io.c:73
float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data)
float roll_rate_setpoint
float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data)
#define ISFINITE(x)
Definition: ecl.h:100
uint64_t _last_run
uint64_t ecl_abstime
Definition: ecl.h:88