controller.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
3  *
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright notice, this
10  * list of conditions and the following disclaimer.
11  *
12  * * Redistributions in binary form must reproduce the above copyright notice,
13  * this list of conditions and the following disclaimer in the documentation
14  * and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include "controller.h"
33 
34 #include "command_manager.h"
35 #include "estimator.h"
36 
37 #include "rosflight.h"
38 
39 #include <cstdbool>
40 #include <cstdint>
41 
42 namespace rosflight_firmware
43 {
45 
47 {
48  prev_time_us_ = 0;
49 
51  float min = -max;
53 
67 }
68 
70 {
71  // Time calculation
72  if (prev_time_us_ == 0)
73  {
75  return;
76  }
77 
78  int32_t dt_us = (RF_.estimator_.state().timestamp_us - prev_time_us_);
79  if (dt_us < 0)
80  {
82  return;
83  }
85 
86  // Check if integrators should be updated
88  bool update_integrators =
89  (RF_.state_manager_.state().armed) && (RF_.command_manager_.combined_control().F.value > 0.1f) && dt_us < 10000;
90 
91  // Run the PID loops
92  turbomath::Vector pid_output =
93  run_pid_loops(dt_us, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators);
94 
95  // Add feedforward torques
100 }
101 
103 {
104  // Make sure we are disarmed
105  if (!(RF_.state_manager_.state().armed))
106  {
107  // Tell the user that we are doing a equilibrium torque calibration
108  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Capturing equilbrium offsets from RC");
109 
110  // Prepare for calibration
111  // artificially tell the flight controller it is leveled
112  Estimator::State fake_state;
113  fake_state.angular_velocity.x = 0.0f;
114  fake_state.angular_velocity.y = 0.0f;
115  fake_state.angular_velocity.z = 0.0f;
116 
117  fake_state.attitude.x = 0.0f;
118  fake_state.attitude.y = 0.0f;
119  fake_state.attitude.z = 0.0f;
120  fake_state.attitude.w = 1.0f;
121 
122  fake_state.roll = 0.0f;
123  fake_state.pitch = 0.0f;
124  fake_state.yaw = 0.0f;
125 
126  // pass the rc_control through the controller
127  // dt is zero, so what this really does is applies the P gain with the settings
128  // your RC transmitter, which if it flies level is a really good guess for
129  // the static offset torques
130  turbomath::Vector pid_output = run_pid_loops(0, fake_state, RF_.command_manager_.rc_control(), false);
131 
132  // the output from the controller is going to be the static offsets
136 
137  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Equilibrium torques found and applied.");
138  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Please zero out trims on your transmitter");
139  }
140  else
141  {
143  "Cannot perform equilibrium offset calibration while armed");
144  }
145 }
146 
147 void Controller::param_change_callback(uint16_t param_id)
148 {
149  switch (param_id)
150  {
166  case PARAM_MAX_COMMAND:
167  case PARAM_PID_TAU:
168  init();
169  break;
170  default:
171  // do nothing
172  break;
173  }
174 }
175 
177  const Estimator::State &state,
178  const control_t &command,
179  bool update_integrators)
180 {
181  // Based on the control types coming from the command manager, run the appropriate PID loops
183 
184  float dt = 1e-6 * dt_us;
185 
186  // ROLL
187  if (command.x.type == RATE)
188  out.x = roll_rate_.run(dt, state.angular_velocity.x, command.x.value, update_integrators);
189  else if (command.x.type == ANGLE)
190  out.x = roll_.run(dt, state.roll, command.x.value, update_integrators, state.angular_velocity.x);
191  else
192  out.x = command.x.value;
193 
194  // PITCH
195  if (command.y.type == RATE)
196  out.y = pitch_rate_.run(dt, state.angular_velocity.y, command.y.value, update_integrators);
197  else if (command.y.type == ANGLE)
198  out.y = pitch_.run(dt, state.pitch, command.y.value, update_integrators, state.angular_velocity.y);
199  else
200  out.y = command.y.value;
201 
202  // YAW
203  if (command.z.type == RATE)
204  out.z = yaw_rate_.run(dt, state.angular_velocity.z, command.z.value, update_integrators);
205  else
206  out.z = command.z.value;
207 
208  return out;
209 }
210 
212  kp_(0.0f),
213  ki_(0.0f),
214  kd_(0.0f),
215  max_(1.0f),
216  min_(-1.0f),
217  integrator_(0.0f),
218  differentiator_(0.0f),
219  prev_x_(0.0f),
220  tau_(0.05)
221 {
222 }
223 
224 void Controller::PID::init(float kp, float ki, float kd, float max, float min, float tau)
225 {
226  kp_ = kp;
227  ki_ = ki;
228  kd_ = kd;
229  max_ = max;
230  min_ = min;
231  tau_ = tau;
232 }
233 
234 float Controller::PID::run(float dt, float x, float x_c, bool update_integrator)
235 {
236  float xdot;
237  if (dt > 0.0001f)
238  {
239  // calculate D term (use dirty derivative if we don't have access to a measurement of the
240  // derivative) The dirty derivative is a sort of low-pass filtered version of the derivative.
243  (2.0f * tau_ - dt) / (2.0f * tau_ + dt) * differentiator_ + 2.0f / (2.0f * tau_ + dt) * (x - prev_x_);
244  xdot = differentiator_;
245  }
246  else
247  {
248  xdot = 0.0f;
249  }
250  prev_x_ = x;
251 
252  return run(dt, x, x_c, update_integrator, xdot);
253 }
254 
255 float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, float xdot)
256 {
257  // Calculate Error
258  float error = x_c - x;
259 
260  // Initialize Terms
261  float p_term = error * kp_;
262  float i_term = 0.0f;
263  float d_term = 0.0f;
264 
265  // If there is a derivative term
266  if (kd_ > 0.0f)
267  {
268  d_term = kd_ * xdot;
269  }
270 
271  // If there is an integrator term and we are updating integrators
272  if ((ki_ > 0.0f) && update_integrator)
273  {
274  // integrate
275  integrator_ += error * dt;
276  // calculate I term
277  i_term = ki_ * integrator_;
278  }
279 
280  // sum three terms
281  float u = p_term - d_term + i_term;
282 
283  // Integrator anti-windup
285  float u_sat = (u > max_) ? max_ : (u < min_) ? min_ : u;
286  if (u != u_sat && fabs(i_term) > fabs(u - p_term + d_term) && ki_ > 0.0f)
287  integrator_ = (u_sat - p_term + d_term) / ki_;
288 
289  // Set output
290  return u_sat;
291 }
292 
293 } // namespace rosflight_firmware
static volatile bool error
Definition: drv_i2c.c:92
void log(CommLinkInterface::LogSeverity severity, const char *fmt,...)
const control_t & combined_control() const
float fabs(float x)
Definition: turbomath.cpp:491
const control_t & rc_control() const
turbomath::Vector run_pid_loops(uint32_t dt, const Estimator::State &state, const control_t &command, bool update_integrators)
Definition: controller.cpp:176
bool set_param_float(uint16_t id, float value)
Sets the value of a floating point parameter by ID and calls the parameter callback.
Definition: param.cpp:378
void init(float kp, float ki, float kd, float max, float min, float tau)
Definition: controller.cpp:224
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:326
const State & state() const
Definition: estimator.h:62
float run(float dt, float x, float x_c, bool update_integrator)
Definition: controller.cpp:234
void param_change_callback(uint16_t param_id) override
Definition: controller.cpp:147
const State & state() const
void set_error(uint16_t error)
turbomath::Quaternion attitude
Definition: estimator.h:53
turbomath::Vector angular_velocity
Definition: estimator.h:52


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Sat May 9 2020 03:16:52