PX4 Firmware
PX4 Autopilot Software http://px4.io
MulticopterRateControl.hpp
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 
34 #pragma once
35 
36 #include <RateControl.hpp>
37 
40 #include <lib/perf/perf_counter.h>
41 #include <px4_platform_common/defines.h>
42 #include <px4_platform_common/module.h>
43 #include <px4_platform_common/module_params.h>
44 #include <px4_platform_common/posix.h>
45 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
46 #include <uORB/Publication.hpp>
48 #include <uORB/Subscription.hpp>
62 
63 class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public ModuleParams, public px4::WorkItem
64 {
65 public:
67 
68  virtual ~MulticopterRateControl();
69 
70  /** @see ModuleBase */
71  static int task_spawn(int argc, char *argv[]);
72 
73  /** @see ModuleBase */
74  static int custom_command(int argc, char *argv[]);
75 
76  /** @see ModuleBase */
77  static int print_usage(const char *reason = nullptr);
78 
79  /** @see ModuleBase::print_status() */
80  int print_status() override;
81 
82  void Run() override;
83 
84  bool init();
85 
86 private:
87 
88  /**
89  * initialize some vectors/matrices from parameters
90  */
91  void parameters_updated();
92 
93  void vehicle_status_poll();
94 
95  /**
96  * Get the landing gear state based on the manual control switch position
97  * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN
98  */
99  float get_landing_gear_state();
100 
101  RateControl _rate_control; ///< class for rate control calculations
102 
103  uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
104  uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
105  uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
106  uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
107  uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
108  uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */
109  uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
110  uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
112 
114 
115  uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */
117  uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
118 
119  orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */
120  orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */
121 
126 
127  bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */
128  bool _landed{true};
129  bool _maybe_landed{true};
130 
132 
133  perf_counter_t _loop_perf; /**< loop duration performance counter */
134 
135  static constexpr const float initial_update_rate_hz = 1000.f; /**< loop update rate used for initialization */
136  float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */
137 
138  matrix::Vector3f _rates_sp; /**< angular rates setpoint */
139 
140  float _thrust_sp{0.0f}; /**< thrust setpoint */
141 
142  bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */
143 
146  float _dt_accumulator{0.0f};
148 
149  DEFINE_PARAMETERS(
150  (ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
151  (ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,
152  (ParamFloat<px4::params::MC_RR_INT_LIM>) _param_mc_rr_int_lim,
153  (ParamFloat<px4::params::MC_ROLLRATE_D>) _param_mc_rollrate_d,
154  (ParamFloat<px4::params::MC_ROLLRATE_FF>) _param_mc_rollrate_ff,
155  (ParamFloat<px4::params::MC_ROLLRATE_K>) _param_mc_rollrate_k,
156 
157  (ParamFloat<px4::params::MC_PITCHRATE_P>) _param_mc_pitchrate_p,
158  (ParamFloat<px4::params::MC_PITCHRATE_I>) _param_mc_pitchrate_i,
159  (ParamFloat<px4::params::MC_PR_INT_LIM>) _param_mc_pr_int_lim,
160  (ParamFloat<px4::params::MC_PITCHRATE_D>) _param_mc_pitchrate_d,
161  (ParamFloat<px4::params::MC_PITCHRATE_FF>) _param_mc_pitchrate_ff,
162  (ParamFloat<px4::params::MC_PITCHRATE_K>) _param_mc_pitchrate_k,
163 
164  (ParamFloat<px4::params::MC_YAWRATE_P>) _param_mc_yawrate_p,
165  (ParamFloat<px4::params::MC_YAWRATE_I>) _param_mc_yawrate_i,
166  (ParamFloat<px4::params::MC_YR_INT_LIM>) _param_mc_yr_int_lim,
167  (ParamFloat<px4::params::MC_YAWRATE_D>) _param_mc_yawrate_d,
168  (ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
169  (ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
170 
171  (ParamFloat<px4::params::MC_DTERM_CUTOFF>) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */
172 
173  (ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
174 
175  (ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
176  (ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,
177  (ParamFloat<px4::params::MC_ACRO_Y_MAX>) _param_mc_acro_y_max,
178  (ParamFloat<px4::params::MC_ACRO_EXPO>) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */
179  (ParamFloat<px4::params::MC_ACRO_EXPO_Y>) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */
180  (ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */
181  (ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */
182 
183  (ParamFloat<px4::params::MC_RATT_TH>) _param_mc_ratt_th,
184 
185  (ParamBool<px4::params::MC_BAT_SCALE_EN>) _param_mc_bat_scale_en,
186 
187  (ParamInt<px4::params::CBRK_RATE_CTRL>) _param_cbrk_rate_ctrl
188  )
189 
190  matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */
191 
192 };
193 
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
A class to implement a second order low pass filter on a Vector3f Based on LowPassFilter2p.hpp by Leonard Hall LeonardTHall@gmail.com
orb_advert_t _actuators_0_pub
attitude actuator controls publication
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
static int custom_command(int argc, char *argv[])
perf_counter_t _loop_perf
loop duration performance counter
static constexpr const float initial_update_rate_hz
loop update rate used for initialization
float get_landing_gear_state()
Get the landing gear state based on the manual control switch position.
PID 3 axis angular rate / angular velocity control.
uORB::Subscription _v_control_mode_sub
vehicle control mode subscription
uORB::Subscription _parameter_update_sub
parameter updates subscription
uORB::Subscription _vehicle_status_sub
vehicle status subscription
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
Header common to all counters.
float _thrust_sp
thrust setpoint
uORB::Subscription _landing_gear_sub
float _loop_update_rate_hz
current rate-controller loop update rate in [Hz]
uORB::PublicationMulti< rate_ctrl_status_s > _controller_status_pub
controller status publication
uORB::Subscription _battery_status_sub
battery status subscription
uORB::Publication< landing_gear_s > _landing_gear_pub
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
uORB::Subscription _motor_limits_sub
motor limits subscription
static int task_spawn(int argc, char *argv[])
RateControl _rate_control
class for rate control calculations
Object metadata.
Definition: uORB.h:50
static int print_usage(const char *reason=nullptr)
void parameters_updated()
initialize some vectors/matrices from parameters
manual_control_setpoint_s _manual_control_sp
bool _gear_state_initialized
true if the gear state has been initialized
matrix::Vector3f _rates_sp
angular rates setpoint
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
vehicle_control_mode_s _v_control_mode
Performance measuring tools.