PX4 Firmware
PX4 Autopilot Software http://px4.io
RunwayTakeoff.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 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  * @file RunwayTakeoff.h
35  * Runway takeoff handling for fixed-wing UAVs with steerable wheels.
36  *
37  * @author Roman Bapst <roman@px4.io>
38  * @author Andreas Antener <andreas@uaventure.com>
39  */
40 
41 #ifndef RUNWAYTAKEOFF_H
42 #define RUNWAYTAKEOFF_H
43 
44 #include <stdbool.h>
45 #include <stdint.h>
46 #include <math.h>
47 
48 #include <drivers/drv_hrt.h>
49 #include <px4_platform_common/module_params.h>
50 #include <systemlib/mavlink_log.h>
51 #include <mathlib/mathlib.h>
52 #include <matrix/math.hpp>
53 
54 namespace runwaytakeoff
55 {
56 
58  THROTTLE_RAMP = 0, /**< ramping up throttle */
59  CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */
60  TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */
61  CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */
62  FLY = 4 /**< fly towards takeoff waypoint */
63 };
64 
65 class __EXPORT RunwayTakeoff : public ModuleParams
66 {
67 public:
68  RunwayTakeoff(ModuleParams *parent);
69  ~RunwayTakeoff() = default;
70 
71  void init(float yaw, double current_lat, double current_lon);
72  void update(float airspeed, float alt_agl, double current_lat, double current_lon, orb_advert_t *mavlink_log_pub);
73 
74  RunwayTakeoffState getState() { return _state; }
75  bool isInitialized() { return _initialized; }
76 
77  bool runwayTakeoffEnabled() { return _param_rwto_tkoff.get(); }
78  float getMinAirspeedScaling() { return _param_rwto_airspd_scl.get(); }
79  float getInitYaw() { return _init_yaw; }
80 
81  bool controlYaw();
82  bool climbout() { return _climbout; }
83  float getPitch(float tecsPitch);
84  float getRoll(float navigatorRoll);
85  float getYaw(float navigatorYaw);
86  float getThrottle(float tecsThrottle);
87  bool resetIntegrators();
88  float getMinPitch(float sp_min, float climbout_min, float min);
89  float getMaxPitch(float max);
90  matrix::Vector2f getStartWP();
91 
92  void reset();
93 
94 private:
95  /** state variables **/
99  float _init_yaw;
100  bool _climbout;
102 
103  DEFINE_PARAMETERS(
104  (ParamBool<px4::params::RWTO_TKOFF>) _param_rwto_tkoff,
105  (ParamInt<px4::params::RWTO_HDG>) _param_rwto_hdg,
106  (ParamFloat<px4::params::RWTO_NAV_ALT>) _param_rwto_nav_alt,
107  (ParamFloat<px4::params::RWTO_MAX_THR>) _param_rwto_max_thr,
108  (ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
109  (ParamFloat<px4::params::RWTO_MAX_PITCH>) _param_rwto_max_pitch,
110  (ParamFloat<px4::params::RWTO_MAX_ROLL>) _param_rwto_max_roll,
111  (ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _param_rwto_airspd_scl,
112  (ParamFloat<px4::params::RWTO_RAMP_TIME>) _param_rwto_ramp_time,
113  (ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
114  (ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _param_fw_clmbout_diff
115  )
116 
117 };
118 
119 }
120 
121 #endif // RUNWAYTAKEOFF_H
static orb_advert_t * mavlink_log_pub
ramping up throttle
Definition: RunwayTakeoff.h:58
Definition: I2C.hpp:51
taking off, get ground clearance, roll 0
Definition: RunwayTakeoff.h:60
int reset(enum LPS22HB_BUS busid)
Reset the driver.
RunwayTakeoffState _state
state variables
Definition: RunwayTakeoff.h:96
High-resolution timer with callouts and timekeeping.
void init()
Activates/configures the hardware registers.
fly towards takeoff waypoint
Definition: RunwayTakeoff.h:62
__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
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
clamped to runway, controlling yaw directly (wheel or rudder)
Definition: RunwayTakeoff.h:59
RunwayTakeoffState getState()
Definition: RunwayTakeoff.h:74
climbout to safe height before navigation, roll limited
Definition: RunwayTakeoff.h:61