PX4 Firmware
PX4 Autopilot Software http://px4.io
mc_att_control.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 <lib/mixer/Mixer/Mixer.hpp> // Airmode
37 #include <matrix/matrix/math.hpp>
38 #include <perf/perf_counter.h>
39 #include <px4_platform_common/px4_config.h>
40 #include <px4_platform_common/defines.h>
41 #include <px4_platform_common/module.h>
42 #include <px4_platform_common/module_params.h>
43 #include <px4_platform_common/posix.h>
44 #include <px4_platform_common/px4_work_queue/WorkItem.hpp>
45 #include <uORB/Publication.hpp>
46 #include <uORB/Subscription.hpp>
57 
58 #include <AttitudeControl.hpp>
59 
60 /**
61  * Multicopter attitude control app start / stop handling function
62  */
63 extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
64 
65 class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>, public ModuleParams,
66  public px4::WorkItem
67 {
68 public:
70 
72 
73  /** @see ModuleBase */
74  static int task_spawn(int argc, char *argv[]);
75 
76  /** @see ModuleBase */
77  static int custom_command(int argc, char *argv[]);
78 
79  /** @see ModuleBase */
80  static int print_usage(const char *reason = nullptr);
81 
82  /** @see ModuleBase::print_status() */
83  int print_status() override;
84 
85  void Run() override;
86 
87  bool init();
88 
89 private:
90 
91  /**
92  * initialize some vectors/matrices from parameters
93  */
94  void parameters_updated();
95 
96  void vehicle_status_poll();
97 
99 
100  float throttle_curve(float throttle_stick_input);
101 
102  /**
103  * Generate & publish an attitude setpoint from stick inputs
104  */
105  void generate_attitude_setpoint(float dt, bool reset_yaw_sp);
106 
107  /**
108  * Attitude controller.
109  */
110  void control_attitude();
111 
112  AttitudeControl _attitude_control; ///< class for attitude control calculations
113 
114  uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */
115  uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
116  uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
117  uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
118  uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
119  uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
120  uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
121 
123 
124  uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
125 
127 
128  orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */
129 
130  struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */
131  struct vehicle_attitude_setpoint_s _v_att_sp {}; /**< vehicle attitude setpoint */
132  struct vehicle_rates_setpoint_s _v_rates_sp {}; /**< vehicle rates setpoint */
133  struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */
134  struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */
135  struct vehicle_status_s _vehicle_status {}; /**< vehicle status */
137 
138  perf_counter_t _loop_perf; /**< loop duration performance counter */
139 
140  matrix::Vector3f _rates_sp; /**< angular rates setpoint */
141 
142  float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */
143 
145 
146  bool _reset_yaw_sp{true};
147 
149  (ParamFloat<px4::params::MC_ROLL_P>) _param_mc_roll_p,
150  (ParamFloat<px4::params::MC_PITCH_P>) _param_mc_pitch_p,
151  (ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p,
152 
153  (ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
154  (ParamFloat<px4::params::MC_PITCHRATE_MAX>) _param_mc_pitchrate_max,
155  (ParamFloat<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,
156 
157  (ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
158 
159  (ParamFloat<px4::params::MC_RATT_TH>) _param_mc_ratt_th,
160 
161  /* Stabilized mode params */
162  (ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
163  (ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min, /**< minimum throttle for stabilized */
164  (ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max, /**< maximum throttle for stabilized */
165  (ParamFloat<px4::params::MPC_THR_HOVER>)
166  _param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */
167  (ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve, /**< throttle curve behavior */
168 
169  (ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode
170  )
171 
172  bool _is_tailsitter{false};
173 
174  float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */
175 
176 };
177 
DEFINE_PARAMETERS((ParamFloat< px4::params::MC_ROLL_P >) _param_mc_roll_p,(ParamFloat< px4::params::MC_PITCH_P >) _param_mc_pitch_p,(ParamFloat< px4::params::MC_YAW_P >) _param_mc_yaw_p,(ParamFloat< px4::params::MC_ROLLRATE_MAX >) _param_mc_rollrate_max,(ParamFloat< px4::params::MC_PITCHRATE_MAX >) _param_mc_pitchrate_max,(ParamFloat< px4::params::MC_YAWRATE_MAX >) _param_mc_yawrate_max,(ParamFloat< px4::params::MPC_MAN_Y_MAX >) _param_mpc_man_y_max,(ParamFloat< px4::params::MC_RATT_TH >) _param_mc_ratt_th,(ParamFloat< px4::params::MPC_MAN_TILT_MAX >) _param_mpc_man_tilt_max,(ParamFloat< px4::params::MPC_MANTHR_MIN >) _param_mpc_manthr_min,(ParamFloat< px4::params::MPC_THR_MAX >) _param_mpc_thr_max,(ParamFloat< px4::params::MPC_THR_HOVER >) _param_mpc_thr_hover,(ParamInt< px4::params::MPC_THR_CURVE >) _param_mpc_thr_curve,(ParamInt< px4::params::MC_AIRMODE >) _param_mc_airmode) bool _is_tailsitter
float _man_tilt_max
maximum tilt allowed for manual flight [rad]
uORB::Subscription _v_att_sp_sub
vehicle attitude setpoint subscription
Generic, programmable, procedural control signal mixers.
orb_advert_t _vehicle_attitude_setpoint_pub
uORB::Subscription _vehicle_land_detected_sub
vehicle land detected subscription
Definition: I2C.hpp:51
uORB::Subscription _v_rates_sp_sub
vehicle rates setpoint subscription
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub
matrix::Vector3f _rates_sp
angular rates setpoint
float throttle_curve(float throttle_stick_input)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
uORB::Subscription _v_control_mode_sub
vehicle control mode subscription
A quaternion based attitude controller.
void parameters_updated()
initialize some vectors/matrices from parameters
Header common to all counters.
orb_id_t _attitude_sp_id
pointer to correct attitude setpoint uORB metadata structure
void generate_attitude_setpoint(float dt, bool reset_yaw_sp)
Generate & publish an attitude setpoint from stick inputs.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
uORB::Subscription _params_sub
parameter updates subscription
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
Object metadata.
Definition: uORB.h:50
float dt
Definition: px4io.c:73
float _man_yaw_sp
current yaw setpoint in manual mode
perf_counter_t _loop_perf
loop duration performance counter
uORB::Publication< vehicle_rates_setpoint_s > _v_rates_sp_pub
rate setpoint publication
uORB::Subscription _vehicle_status_sub
vehicle status subscription
static int print_usage(const char *reason=nullptr)
static int task_spawn(int argc, char *argv[])
static int custom_command(int argc, char *argv[])
AttitudeControl _attitude_control
class for attitude control calculations
uORB::Subscription _manual_control_sp_sub
manual control setpoint subscription
void control_attitude()
Attitude controller.
Performance measuring tools.
__EXPORT int mc_att_control_main(int argc, char *argv[])
Multicopter attitude control app start / stop handling function.