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