PX4 Firmware
PX4 Autopilot Software http://px4.io
vtol_att_control_main.h
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 /**
35  * @file VTOL_att_control_main.cpp
36  * Implementation of an attitude controller for VTOL airframes. This module receives data
37  * from both the fixed wing- and the multicopter attitude controllers and processes it.
38  * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward-
39  * flight or transition). It also publishes the resulting controls on the actuator controls topics.
40  *
41  * @author Roman Bapst <bapstr@ethz.ch>
42  * @author Lorenz Meier <lm@inf.ethz.ch>
43  * @author Thomas Gubler <thomasgubler@gmail.com>
44  * @author David Vorsin <davidvorsin@gmail.com>
45  * @author Sander Smeets <sander@droneslab.com>
46  * @author Andreas Antener <andreas@uaventure.com>
47  *
48  */
49 
50 #pragma once
51 
52 #include <drivers/drv_hrt.h>
53 #include <drivers/drv_pwm_output.h>
54 #include <lib/ecl/geo/geo.h>
55 #include <lib/mathlib/mathlib.h>
56 #include <lib/parameters/param.h>
57 #include <lib/perf/perf_counter.h>
58 #include <matrix/math.hpp>
59 #include <px4_platform_common/px4_config.h>
60 #include <px4_platform_common/defines.h>
61 #include <px4_platform_common/module.h>
62 #include <px4_platform_common/posix.h>
63 #include <px4_platform_common/px4_work_queue/WorkItem.hpp>
64 #include <uORB/Publication.hpp>
65 #include <uORB/Subscription.hpp>
82 
83 #include "standard.h"
84 #include "tailsitter.h"
85 #include "tiltrotor.h"
86 
87 extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
88 
89 class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public px4::WorkItem
90 {
91 public:
92 
95 
96  /** @see ModuleBase */
97  static int task_spawn(int argc, char *argv[]);
98 
99  /** @see ModuleBase */
100  static int custom_command(int argc, char *argv[]);
101 
102  /** @see ModuleBase */
103  static int print_usage(const char *reason = nullptr);
104 
105  void Run() override;
106 
107  bool init();
108 
109  /** @see ModuleBase::print_status() */
110  int print_status() override;
111 
113  void abort_front_transition(const char *reason);
114 
122  struct vehicle_attitude_s *get_att() {return &_v_att;}
131 
132  struct Params *get_params() {return &_params;}
133 
134 private:
135 
136  uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)};
137  uORB::SubscriptionCallbackWorkItem _actuator_inputs_mc{this, ORB_ID(actuator_controls_virtual_mc)};
138 
139  uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed subscription
140  uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)};
142  uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription
143  uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor subscription
144  uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; //manual control setpoint subscription
145  uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)};
147  uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription
149  uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; //vehicle attitude subscription
150  uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; //vehicle control mode subscription
152 
153  uORB::Publication<actuator_controls_s> _actuators_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust)
157 
158  orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle
159 
160  vehicle_attitude_setpoint_s _v_att_sp{}; //vehicle attitude setpoint
161  vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint
162  vehicle_attitude_setpoint_s _mc_virtual_att_sp{}; // virtual mc attitude setpoint
163 
164  actuator_controls_s _actuators_fw_in{}; //actuator controls from fw_att_control
165  actuator_controls_s _actuators_mc_in{}; //actuator controls from mc_att_control
166  actuator_controls_s _actuators_out_0{}; //actuator controls going to the mc mixer
167  actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons)
168 
170  manual_control_setpoint_s _manual_control_sp{}; //manual control setpoint
173  vehicle_attitude_s _v_att{}; //vehicle attitude
175  vehicle_control_mode_s _v_control_mode{}; //vehicle control mode
180 
181  Params _params{}; // struct holding the parameters
182 
183  struct {
207  } _params_handles{};
208 
209  /* for multicopters it is usual to have a non-zero idle speed of the engines
210  * for fixed wings we want to have an idle speed of zero since we do not want
211  * to waste energy when gliding. */
212  int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC};
214 
215  VtolType *_vtol_type{nullptr}; // base class for different vtol types
216 
217  bool _initialized{false};
218 
219  perf_counter_t _loop_perf; /**< loop performance counter */
220 
221  void vehicle_cmd_poll();
222 
223  int parameters_update(); //Update local parameter cache
224 
225  void handle_command();
226 };
__EXPORT int vtol_att_control_main(int argc, char *argv[])
struct vehicle_attitude_setpoint_s * get_mc_virtual_att_sp()
uORB::Publication< actuator_controls_s > _actuators_1_pub
vehicle_land_detected_s _land_detected
uORB::Subscription _parameter_update_sub
vehicle_attitude_s _v_att
uORB::Subscription _mc_virtual_att_sp_sub
Definition of geo / math functions to perform geodesic calculations.
uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw
actuator_controls_s _actuators_out_1
uORB::Subscription _pos_sp_triplet_sub
uORB::Publication< vtol_vehicle_status_s > _vtol_vehicle_status_pub
vehicle_attitude_setpoint_s _fw_virtual_att_sp
Definition: I2C.hpp:51
perf_counter_t _loop_perf
loop performance counter
struct vehicle_attitude_setpoint_s * get_fw_virtual_att_sp()
High-resolution timer with callouts and timekeeping.
struct vehicle_land_detected_s * get_land_detected()
actuator_controls_s _actuators_fw_in
Global flash based parameter store.
vehicle_attitude_setpoint_s _mc_virtual_att_sp
struct vehicle_local_position_s * get_local_pos()
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
uORB::Subscription _land_detected_sub
static int custom_command(int argc, char *argv[])
vehicle_attitude_setpoint_s _v_att_sp
vtol_vehicle_status_s _vtol_vehicle_status
Header common to all counters.
uORB::Subscription _local_pos_sub
struct vehicle_local_position_setpoint_s * get_local_pos_sp()
uORB::Subscription _tecs_status_sub
uORB::Publication< actuator_controls_s > _actuators_0_pub
airspeed_validated_s _airspeed_validated
uORB::Subscription _airspeed_validated_sub
actuator_controls_s _actuators_mc_in
struct VtolAttitudeControl::@145 _params_handles
struct vehicle_attitude_setpoint_s * get_att_sp()
vehicle_command_s _vehicle_cmd
struct vtol_vehicle_status_s * get_vtol_vehicle_status()
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
manual_control_setpoint_s _manual_control_sp
struct actuator_controls_s * get_actuators_out1()
static int task_spawn(int argc, char *argv[])
struct actuator_controls_s * get_actuators_mc_in()
struct vehicle_control_mode_s * get_control_mode()
struct Params * get_params()
vehicle_local_position_setpoint_s _local_pos_sp
struct actuator_controls_s * get_actuators_out0()
uORB::Subscription _fw_virtual_att_sp_sub
void vehicle_cmd_poll()
Check for command updates.
struct actuator_controls_s * get_actuators_fw_in()
uORB::Subscription _vehicle_cmd_sub
vehicle_local_position_s _local_pos
uORB::Subscription _manual_control_sp_sub
vehicle_control_mode_s _v_control_mode
struct vehicle_attitude_s * get_att()
struct tecs_status_s * get_tecs_status()
uORB::SubscriptionCallbackWorkItem _actuator_inputs_mc
uORB::Subscription _local_pos_sp_sub
position_setpoint_triplet_s _pos_sp_triplet
uORB::Subscription _v_att_sub
struct position_setpoint_triplet_s * get_pos_sp_triplet()
void handle_command()
Check received command.
actuator_controls_s _actuators_out_0
void abort_front_transition(const char *reason)
uORB::Subscription _v_control_mode_sub
VTOL with fixed multirotor motor configurations (such as quad) and a pusher (or puller aka tractor) m...
uint32_t param_t
Parameter handle.
Definition: param.h:98
struct airspeed_validated_s * get_airspeed()
static int print_usage(const char *reason=nullptr)
Performance measuring tools.
uORB::Publication< vehicle_attitude_setpoint_s > _v_att_sp_pub