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 <stdint.h>
33 #include <stdbool.h>
34 
35 #include "command_manager.h"
36 #include "estimator.h"
37 #include "rosflight.h"
38 
39 #include "controller.h"
40 
41 namespace rosflight_firmware
42 {
43 
45  RF_(rf)
46 {
47 }
48 
50 {
51  prev_time_us_ = 0;
52 
54  float min = -max;
56 
60  max, min, tau);
64  max, min, tau);
68  max, min, tau);
72  max, min, tau);
76  max, min, tau);
77 }
78 
80 {
81  // Time calculation
82  if (prev_time_us_ == 0)
83  {
85  return;
86  }
87 
88  int32_t dt_us = (RF_.estimator_.state().timestamp_us - prev_time_us_);
89  if (dt_us < 0)
90  {
92  return;
93  }
95 
96  // Check if integrators should be updated
98  bool update_integrators = (RF_.state_manager_.state().armed) && (RF_.command_manager_.combined_control().F.value > 0.1f) && dt_us < 10000;
99 
100  // Run the PID loops
101  turbomath::Vector pid_output = run_pid_loops(dt_us, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators);
102 
103  // Add feedforward torques
108 }
109 
111 {
112  // Make sure we are disarmed
113  if (!(RF_.state_manager_.state().armed))
114  {
115  // Tell the user that we are doing a equilibrium torque calibration
116  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Capturing equilbrium offsets from RC");
117 
118  // Prepare for calibration
119  // artificially tell the flight controller it is leveled
120  Estimator::State fake_state;
121  fake_state.angular_velocity.x = 0.0f;
122  fake_state.angular_velocity.y = 0.0f;
123  fake_state.angular_velocity.z = 0.0f;
124 
125  fake_state.attitude.x = 0.0f;
126  fake_state.attitude.y = 0.0f;
127  fake_state.attitude.z = 0.0f;
128  fake_state.attitude.w = 1.0f;
129 
130  fake_state.roll = 0.0f;
131  fake_state.pitch = 0.0f;
132  fake_state.yaw = 0.0f;
133 
134  // pass the rc_control through the controller
135  // dt is zero, so what this really does is applies the P gain with the settings
136  // your RC transmitter, which if it flies level is a really good guess for
137  // the static offset torques
138  turbomath::Vector pid_output = run_pid_loops(0, fake_state, RF_.command_manager_.rc_control(), false);
139 
140  // the output from the controller is going to be the static offsets
144 
145  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Equilibrium torques found and applied.");
146  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Please zero out trims on your transmitter");
147  }
148  else
149  {
150  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Cannot perform equilibrium offset calibration while armed");
151  }
152 }
153 
154 void Controller::param_change_callback(uint16_t param_id)
155 {
156  switch (param_id)
157  {
173  case PARAM_MAX_COMMAND:
174  case PARAM_PID_TAU:
175  init();
176  break;
177  default:
178  // do nothing
179  break;
180  }
181 }
182 
183 turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, const Estimator::State &state, const control_t &command, bool update_integrators)
184 {
185  // Based on the control types coming from the command manager, run the appropriate PID loops
187 
188  float dt = 1e-6*dt_us;
189 
190  // ROLL
191  if (command.x.type == RATE)
192  out.x = roll_rate_.run(dt, state.angular_velocity.x, command.x.value, update_integrators);
193  else if (command.x.type == ANGLE)
194  out.x = roll_.run(dt, state.roll, command.x.value, update_integrators, state.angular_velocity.x);
195  else
196  out.x = command.x.value;
197 
198  // PITCH
199  if (command.y.type == RATE)
200  out.y = pitch_rate_.run(dt, state.angular_velocity.y, command.y.value, update_integrators);
201  else if (command.y.type == ANGLE)
202  out.y = pitch_.run(dt, state.pitch, command.y.value, update_integrators, state.angular_velocity.y);
203  else
204  out.y = command.y.value;
205 
206  // YAW
207  if (command.z.type == RATE)
208  out.z = yaw_rate_.run(dt, state.angular_velocity.z, command.z.value, update_integrators);
209  else
210  out.z = command.z.value;
211 
212  return out;
213 }
214 
216  kp_(0.0f),
217  ki_(0.0f),
218  kd_(0.0f),
219  max_(1.0f),
220  min_(-1.0f),
221  integrator_(0.0f),
222  differentiator_(0.0f),
223  prev_x_(0.0f),
224  tau_(0.05)
225 {}
226 
227 void Controller::PID::init(float kp, float ki, float kd, float max, float min, float tau)
228 {
229  kp_ = kp;
230  ki_ = ki;
231  kd_ = kd;
232  max_ = max;
233  min_ = min;
234  tau_ = tau;
235 }
236 
237 float Controller::PID::run(float dt, float x, float x_c, bool update_integrator)
238 {
239  float xdot;
240  if (dt > 0.0001f)
241  {
242  // calculate D term (use dirty derivative if we don't have access to a measurement of the derivative)
243  // The dirty derivative is a sort of low-pass filtered version of the derivative.
245  differentiator_ = (2.0f * tau_ - dt) / (2.0f * tau_ + dt) * differentiator_
246  + 2.0f / (2.0f * tau_ + dt) * (x - prev_x_);
247  xdot = differentiator_;
248  }
249  else
250  {
251  xdot = 0.0f;
252  }
253  prev_x_ = x;
254 
255  return run(dt, x, x_c, update_integrator, xdot);
256 }
257 
258 float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, float xdot)
259 {
260  // Calculate Error
261  float error = x_c - x;
262 
263  // Initialize Terms
264  float p_term = error * kp_;
265  float i_term = 0.0f;
266  float d_term = 0.0f;
267 
268  // If there is a derivative term
269  if (kd_ > 0.0f)
270  {
271  d_term = kd_ * xdot;
272  }
273 
274  //If there is an integrator term and we are updating integrators
275  if ((ki_ > 0.0f) && update_integrator)
276  {
277  // integrate
278  integrator_ += error * dt;
279  // calculate I term
280  i_term = ki_ * integrator_;
281  }
282 
283  // sum three terms
284  float u = p_term - d_term + i_term;
285 
286  // Integrator anti-windup
288  float u_sat = (u > max_) ? max_ : (u < min_) ? min_ : u;
289  if (u != u_sat && fabs(i_term) > fabs(u - p_term + d_term) && ki_ > 0.0f)
290  integrator_ = (u_sat - p_term + d_term)/ki_;
291 
292  // Set output
293  return u_sat;
294 }
295 
296 } // 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:538
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:183
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:369
void init(float kp, float ki, float kd, float max, float min, float tau)
Definition: controller.cpp:227
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:322
const State & state() const
Definition: estimator.h:64
float run(float dt, float x, float x_c, bool update_integrator)
Definition: controller.cpp:237
void param_change_callback(uint16_t param_id) override
Definition: controller.cpp:154
const State & state() const
Definition: state_manager.h:82
void set_error(uint16_t error)
turbomath::Quaternion attitude
Definition: estimator.h:55
turbomath::Vector angular_velocity
Definition: estimator.h:54


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:18