PX4 Firmware
PX4 Autopilot Software http://px4.io
ecl_wheel_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_wheel_controller.cpp
36  * Implementation of a simple PID wheel controller for heading tracking.
37  *
38  * Authors and acknowledgements in header.
39  */
40 
41 #include "ecl_wheel_controller.h"
42 #include <float.h>
43 #include <geo/geo.h>
44 #include <mathlib/mathlib.h>
45 #include <matrix/math.hpp>
46 
47 using matrix::wrap_pi;
48 
50 {
51  /* Do not calculate control signal with bad inputs */
52  if (!(ISFINITE(ctl_data.body_z_rate) &&
53  ISFINITE(ctl_data.groundspeed) &&
54  ISFINITE(ctl_data.groundspeed_scaler))) {
55  return math::constrain(_last_output, -1.0f, 1.0f);
56  }
57 
58  /* get the usual dt estimate */
59  uint64_t dt_micros = ecl_elapsed_time(&_last_run);
61  float dt = (float)dt_micros * 1e-6f;
62 
63  /* lock integral for long intervals */
64  bool lock_integrator = ctl_data.lock_integrator;
65 
66  if (dt_micros > 500000) {
67  lock_integrator = true;
68  }
69 
70  /* input conditioning */
71  float min_speed = 1.0f;
72 
73  /* Calculate body angular rate error */
74  _rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error
75 
76  if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
77 
78  float id = _rate_error * dt * ctl_data.groundspeed_scaler;
79 
80  /*
81  * anti-windup: do not allow integrator to increase if actuator is at limit
82  */
83  if (_last_output < -1.0f) {
84  /* only allow motion to center: increase value */
85  id = math::max(id, 0.0f);
86 
87  } else if (_last_output > 1.0f) {
88  /* only allow motion to center: decrease value */
89  id = math::min(id, 0.0f);
90  }
91 
92  /* add and constrain */
94  }
95 
96  /* Apply PI rate controller and store non-limited output */
99 
100  return math::constrain(_last_output, -1.0f, 1.0f);
101 }
102 
103 
105 {
106  /* Do not calculate control signal with bad inputs */
107  if (!(ISFINITE(ctl_data.yaw_setpoint) &&
108  ISFINITE(ctl_data.yaw))) {
109  return _rate_setpoint;
110  }
111 
112  /* Calculate the error */
113  float yaw_error = wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw);
114 
115  /* Apply P controller: rate setpoint from current error and time constant */
116  _rate_setpoint = yaw_error / _tc;
117 
118  /* limit the rate */
119  if (_max_rate > 0.01f) {
120  if (_rate_setpoint > 0.0f) {
122 
123  } else {
125  }
126 
127  }
128 
129  return _rate_setpoint;
130 }
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_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
Type wrap_pi(Type x)
Wrap value in range [-π, π)
Definition of a simple orthogonal coordinated turn yaw PID controller.
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
float dt
Definition: px4io.c:73
float control_bodyrate(const struct ECL_ControlData &ctl_data) override
float groundspeed_scaler
#define ISFINITE(x)
Definition: ecl.h:100
uint64_t _last_run