PX4 Firmware
PX4 Autopilot Software http://px4.io
MulticopterRateControl.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2019 PX4 Development Team. 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 PX4 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 
35 
36 #include <drivers/drv_hrt.h>
38 #include <mathlib/math/Limits.hpp>
40 
41 using namespace matrix;
42 using namespace time_literals;
43 using math::radians;
44 
46  ModuleParams(nullptr),
47  WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
48  _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
49 {
50  _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
51 
53 }
54 
56 {
58 }
59 
60 bool
62 {
64  PX4_ERR("vehicle_angular_velocity callback registration failed!");
65  return false;
66  }
67 
68  return true;
69 }
70 
71 void
73 {
74  // rate control parameters
75  // The controller gain K is used to convert the parallel (P + I/s + sD) form
76  // to the ideal (K * [1 + 1/sTi + sTd]) form
77  const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get());
78 
80  rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())),
81  rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())),
82  rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get())));
83 
85  Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get()));
86 
87  _rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), false);
88 
90  Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()));
91 
92 
93  // manual rate control acro mode rate limits
94  _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()),
95  radians(_param_mc_acro_y_max.get()));
96 
98 }
99 
100 void
102 {
103  /* check if there is new status information */
105  /* set correct uORB ID, depending on if vehicle is VTOL or not */
106  if (_actuators_id == nullptr) {
107  if (_vehicle_status.is_vtol) {
108  _actuators_id = ORB_ID(actuator_controls_virtual_mc);
109 
110  } else {
111  _actuators_id = ORB_ID(actuator_controls_0);
112  }
113  }
114  }
115 }
116 
117 float
119 {
120  // Only switch the landing gear up if we are not landed and if
121  // the user switched from gear down to gear up.
122  // If the user had the switch in the gear up position and took off ignore it
123  // until he toggles the switch to avoid retracting the gear immediately on takeoff.
124  if (_landed) {
125  _gear_state_initialized = false;
126  }
127 
128  float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
129 
130  if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
131  landing_gear = landing_gear_s::GEAR_UP;
132 
133  } else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
134  // Switching the gear off does put it into a safe defined state
136  }
137 
138  return landing_gear;
139 }
140 
141 void
143 {
144  if (should_exit()) {
146  exit_and_cleanup();
147  return;
148  }
149 
151 
152  // Check if parameters have changed
154  // clear update
155  parameter_update_s param_update;
156  _parameter_update_sub.copy(&param_update);
157 
158  updateParams();
160  }
161 
162  /* run controller on gyro changes */
163  vehicle_angular_velocity_s angular_velocity;
164 
165  if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
166  const hrt_abstime now = hrt_absolute_time();
167 
168  // Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
169  const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
170  _last_run = now;
171 
172  const Vector3f rates{angular_velocity.xyz};
173 
174  /* check for updates in other topics */
176 
178  vehicle_land_detected_s vehicle_land_detected;
179 
180  if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
181  _landed = vehicle_land_detected.landed;
182  _maybe_landed = vehicle_land_detected.maybe_landed;
183  }
184  }
185 
187 
188  const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
189 
190  // generate the rate setpoint from sticks?
191  bool manual_rate_sp = false;
192 
197 
198  // landing gear controlled from stick inputs if we are in Manual/Stabilized mode
199  // limit landing gear update rate to 50 Hz
200  if (hrt_elapsed_time(&_landing_gear.timestamp) > 20_ms) {
204  }
205 
207  manual_rate_sp = true;
208  }
209 
210  // Check if we are in rattitude mode and the pilot is within the center threshold on pitch and roll
211  // if true then use published rate setpoint, otherwise generate from manual_control_setpoint (like acro)
213  manual_rate_sp =
214  (fabsf(_manual_control_sp.y) > _param_mc_ratt_th.get()) ||
215  (fabsf(_manual_control_sp.x) > _param_mc_ratt_th.get());
216  }
217 
218  } else {
220  }
221 
222  if (manual_rate_sp) {
223  if (manual_control_updated) {
224 
225  // manual rates control - ACRO mode
226  const Vector3f man_rate_sp{
227  math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
228  math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
229  math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
230 
231  _rates_sp = man_rate_sp.emult(_acro_rate_max);
233 
234  // publish rate setpoint
235  vehicle_rates_setpoint_s v_rates_sp{};
236  v_rates_sp.roll = _rates_sp(0);
237  v_rates_sp.pitch = _rates_sp(1);
238  v_rates_sp.yaw = _rates_sp(2);
239  v_rates_sp.thrust_body[0] = 0.0f;
240  v_rates_sp.thrust_body[1] = 0.0f;
241  v_rates_sp.thrust_body[2] = -_thrust_sp;
242  v_rates_sp.timestamp = hrt_absolute_time();
243 
244  _v_rates_sp_pub.publish(v_rates_sp);
245  }
246 
247  } else {
248  // use rates setpoint topic
249  vehicle_rates_setpoint_s v_rates_sp;
250 
251  if (_v_rates_sp_sub.update(&v_rates_sp)) {
252  _rates_sp(0) = v_rates_sp.roll;
253  _rates_sp(1) = v_rates_sp.pitch;
254  _rates_sp(2) = v_rates_sp.yaw;
255  _thrust_sp = -v_rates_sp.thrust_body[2];
256  }
257  }
258 
259  // calculate loop update rate while disarmed or at least a few times (updating the filter is expensive)
260  if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) {
261  _dt_accumulator += dt;
262  ++_loop_counter;
263 
264  if (_dt_accumulator > 1.0f) {
265  const float loop_update_rate = (float)_loop_counter / _dt_accumulator;
266  _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
267  _dt_accumulator = 0;
268  _loop_counter = 0;
269  _rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), true);
270  }
271  }
272 
273  // run the rate controller
275 
276  // reset integral if disarmed
277  if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
279  }
280 
281  // update saturation status from mixer feedback
282  if (_motor_limits_sub.updated()) {
283  multirotor_motor_limits_s motor_limits;
284 
285  if (_motor_limits_sub.copy(&motor_limits)) {
286  MultirotorMixer::saturation_status saturation_status;
287  saturation_status.value = motor_limits.saturation_status;
288 
289  _rate_control.setSaturationStatus(saturation_status);
290  }
291  }
292 
293  // run rate controller
294  const Vector3f att_control = _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed);
295 
296  // publish rate controller status
297  rate_ctrl_status_s rate_ctrl_status{};
298  _rate_control.getRateControlStatus(rate_ctrl_status);
299  rate_ctrl_status.timestamp = hrt_absolute_time();
300  _controller_status_pub.publish(rate_ctrl_status);
301 
302  // publish actuator controls
303  actuator_controls_s actuators{};
304  actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
305  actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
306  actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
307  actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
308  actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
309  actuators.timestamp_sample = angular_velocity.timestamp_sample;
310 
311  // scale effort by battery status if enabled
312  if (_param_mc_bat_scale_en.get()) {
315 
316  if (_battery_status_sub.copy(&battery_status)) {
317  _battery_status_scale = battery_status.scale;
318  }
319  }
320 
321  if (_battery_status_scale > 0.0f) {
322  for (int i = 0; i < 4; i++) {
323  actuators.control[i] *= _battery_status_scale;
324  }
325  }
326  }
327 
328  actuators.timestamp = hrt_absolute_time();
330 
332  if (!_vehicle_status.is_vtol) {
333  // publish actuator controls
334  actuator_controls_s actuators{};
335  actuators.timestamp = hrt_absolute_time();
337  }
338  }
339  }
340 
342 }
343 
344 int MulticopterRateControl::task_spawn(int argc, char *argv[])
345 {
347 
348  if (instance) {
349  _object.store(instance);
350  _task_id = task_id_is_work_queue;
351 
352  if (instance->init()) {
353  return PX4_OK;
354  }
355 
356  } else {
357  PX4_ERR("alloc failed");
358  }
359 
360  delete instance;
361  _object.store(nullptr);
362  _task_id = -1;
363 
364  return PX4_ERROR;
365 }
366 
368 {
369  PX4_INFO("Running");
370 
372 
373  return 0;
374 }
375 
376 int MulticopterRateControl::custom_command(int argc, char *argv[])
377 {
378  return print_usage("unknown command");
379 }
380 
381 int MulticopterRateControl::print_usage(const char *reason)
382 {
383  if (reason) {
384  PX4_WARN("%s\n", reason);
385  }
386 
387  PRINT_MODULE_DESCRIPTION(
388  R"DESCR_STR(
389 ### Description
390 This implements the multicopter rate controller. It takes rate setpoints (in acro mode
391 via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
392 
393 The controller has a PID loop for angular rate error.
394 
395 )DESCR_STR");
396 
397  PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
398  PRINT_MODULE_USAGE_COMMAND("start");
399  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
400 
401  return 0;
402 }
403 
404 /**
405  * Multicopter rate control app start / stop handling function
406  */
407 extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[]);
408 
409 int mc_rate_control_main(int argc, char *argv[])
410 {
411  return MulticopterRateControl::main(argc, argv);
412 }
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
void setIntegratorLimit(const matrix::Vector3f &integrator_limit)
Set the mximum absolute value of the integrator for all axes.
Definition: RateControl.hpp:66
uORB::Subscription _manual_control_sp_sub
manual control setpoint subscription
bool _actuators_0_circuit_breaker_enabled
circuit breaker to suppress output
uORB::Subscription _v_rates_sp_sub
vehicle rates setpoint subscription
measure the time elapsed performing an event
Definition: perf_counter.h:56
bool circuit_breaker_enabled_by_val(int32_t breaker_val, int32_t magic)
orb_advert_t _actuators_0_pub
attitude actuator controls publication
bool update(void *dst)
Copy the struct if updated.
uORB::Publication< vehicle_rates_setpoint_s > _v_rates_sp_pub
rate setpoint publication
uORB::Subscription _vehicle_land_detected_sub
vehicle land detected subscription
orb_id_t _actuators_id
pointer to correct actuator controls0 uORB metadata structure
__EXPORT int mc_rate_control_main(int argc, char *argv[])
Multicopter rate control app start / stop handling function.
static int custom_command(int argc, char *argv[])
int main(int argc, char **argv)
Definition: main.cpp:3
perf_counter_t _loop_perf
loop duration performance counter
Definition: I2C.hpp:51
float get_landing_gear_state()
Get the landing gear state based on the manual control switch position.
const T superexpo(const T &value, const T &e, const T &g)
Definition: Functions.hpp:90
Limiting / constrain helper functions.
LidarLite * instance
Definition: ll40ls.cpp:65
uORB::Subscription _v_control_mode_sub
vehicle control mode subscription
matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt, const bool landed)
Run one control loop cycle calculation.
Definition: RateControl.cpp:69
High-resolution timer with callouts and timekeeping.
uORB::Subscription _parameter_update_sub
parameter updates subscription
collection of rather simple mathematical functions that get used over and over again ...
uORB::Subscription _vehicle_status_sub
vehicle status subscription
void setSaturationStatus(const MultirotorMixer::saturation_status &status)
Set saturation status.
Definition: RateControl.cpp:59
void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D)
Set the rate control gains.
Definition: RateControl.cpp:43
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void setDTermCutoff(const float loop_rate, const float cutoff, const bool force)
Set update frequency and low-pass filter cutoff that is applied to the derivative term...
Definition: RateControl.cpp:50
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
void perf_free(perf_counter_t handle)
Free a counter.
void resetIntegral()
Set the integral term to 0 to prevent windup.
#define perf_alloc(a, b)
Definition: px4io.h:59
constexpr T radians(T degrees)
Definition: Limits.hpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
int8_t landing_gear
Definition: landing_gear.h:57
float _thrust_sp
thrust setpoint
uORB::Subscription _landing_gear_sub
float _loop_update_rate_hz
current rate-controller loop update rate in [Hz]
void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
Get status message of controller for logging/debugging.
uORB::PublicationMulti< rate_ctrl_status_s > _controller_status_pub
controller status publication
void perf_end(perf_counter_t handle)
End a performance event.
uORB::Subscription _battery_status_sub
battery status subscription
uORB::Publication< landing_gear_s > _landing_gear_pub
bool updated()
Check if there is a new update.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
uORB::Subscription _motor_limits_sub
motor limits subscription
void setFeedForwardGain(const matrix::Vector3f &FF)
Set direct rate to torque feed forward gain.
Definition: RateControl.hpp:81
static int task_spawn(int argc, char *argv[])
Vector3< float > Vector3f
Definition: Vector3.hpp:136
RateControl _rate_control
class for rate control calculations
#define CBRK_RATE_CTRL_KEY
static int print_usage(const char *reason=nullptr)
uint64_t timestamp
Definition: landing_gear.h:56
float dt
Definition: px4io.c:73
void parameters_updated()
initialize some vectors/matrices from parameters
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
Definition: bst.cpp:62
manual_control_setpoint_s _manual_control_sp
bool _gear_state_initialized
true if the gear state has been initialized
bool update(void *dst)
Update the struct.
bool publish(const T &data)
Publish the struct.
matrix::Vector3f _rates_sp
angular rates setpoint
bool copy(void *dst)
Copy the struct.
void perf_begin(perf_counter_t handle)
Begin a performance event.
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
Definition: uORB.h:177
Matrix< Type, M, N > emult(const Matrix< Type, M, N > &other) const
Definition: Matrix.hpp:157
vehicle_control_mode_s _v_control_mode