PX4 Firmware
PX4 Autopilot Software http://px4.io
ecl_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_controller.cpp
36  * Definition of base class for other controllers
37  *
38  * @author Lorenz Meier <lm@inf.ethz.ch>
39  * @author Thomas Gubler <thomasgubler@gmail.com>
40  *
41  * Acknowledgements:
42  *
43  * The control design is based on a design
44  * by Paul Riseborough and Andrew Tridgell, 2013,
45  * which in turn is based on initial work of
46  * Jonathan Challinger, 2012.
47  */
48 
49 #include "ecl_controller.h"
50 
51 #include <stdio.h>
52 #include <mathlib/mathlib.h>
53 
55  _last_run(0),
56  _tc(0.1f),
57  _k_p(0.0f),
58  _k_i(0.0f),
59  _k_ff(0.0f),
60  _integrator_max(0.0f),
61  _max_rate(0.0f),
62  _last_output(0.0f),
63  _integrator(0.0f),
64  _rate_error(0.0f),
65  _rate_setpoint(0.0f),
66  _bodyrate_setpoint(0.0f)
67 {
68 }
69 
71 {
72  _integrator = 0.0f;
73 }
74 
75 void ECL_Controller::set_time_constant(float time_constant)
76 {
77  if (time_constant > 0.1f && time_constant < 3.0f) {
78  _tc = time_constant;
79  }
80 }
81 
82 void ECL_Controller::set_k_p(float k_p)
83 {
84  _k_p = k_p;
85 }
86 
87 void ECL_Controller::set_k_i(float k_i)
88 {
89  _k_i = k_i;
90 }
91 
92 void ECL_Controller::set_k_ff(float k_ff)
93 {
94  _k_ff = k_ff;
95 }
96 
98 {
100 }
101 
102 void ECL_Controller::set_max_rate(float max_rate)
103 {
104  _max_rate = max_rate;
105 }
106 
108 {
110 }
111 
113 {
114  return _rate_error;
115 }
116 
118 {
119  return _rate_setpoint;
120 }
121 
123 {
124  return _bodyrate_setpoint;
125 }
126 
128 {
129  return _integrator;
130 }
131 
132 float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed)
133 {
134  float airspeed_result = airspeed;
135 
136  if (!ISFINITE(airspeed)) {
137  /* airspeed is NaN, +- INF or not available, pick center of band */
138  airspeed_result = 0.5f * (minspeed + maxspeed);
139 
140  } else if (airspeed < minspeed) {
141  airspeed_result = minspeed;
142  }
143 
144  return airspeed_result;
145 }
float constrain_airspeed(float airspeed, float minspeed, float maxspeed)
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
void set_time_constant(float time_constant)
void set_max_rate(float max_rate)
float get_desired_rate()
void set_k_i(float k_i)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void set_k_ff(float k_ff)
float get_desired_bodyrate()
float _bodyrate_setpoint
void set_bodyrate_setpoint(float rate)
void set_k_p(float k_p)
Definition of base class for other controllers.
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
void set_integrator_max(float max)
#define ISFINITE(x)
Definition: ecl.h:100