PX4 Firmware
PX4 Autopilot Software http://px4.io
MulticopterLandDetector.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2016 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 MulticopterLandDetector.h
36  * Land detection implementation for multicopters.
37  *
38  * @author Johan Jansen <jnsn.johan@gmail.com>
39  * @author Morten Lysgaard <morten@lysgaard.no>
40  * @author Julian Oes <julian@oes.ch>
41  */
42 
43 #pragma once
44 
51 
52 #include "LandDetector.h"
53 
54 using namespace time_literals;
55 
56 namespace land_detector
57 {
58 
60 {
61 public:
63  ~MulticopterLandDetector() override = default;
64 
65 protected:
66  void _update_params() override;
67  void _update_topics() override;
68 
69  bool _get_landed_state() override;
70  bool _get_ground_contact_state() override;
71  bool _get_maybe_landed_state() override;
72  bool _get_freefall_state() override;
73  bool _get_ground_effect_state() override;
74 
75  float _get_max_altitude() override;
76 private:
77 
78  /** Get control mode dependent pilot throttle threshold with which we should quit landed state and take off. */
79  float _get_takeoff_throttle();
80 
81  bool _has_low_thrust();
82  bool _has_minimal_thrust();
83  bool _has_altitude_lock();
84  bool _has_position_lock();
85  bool _is_climb_rate_enabled();
86 
87  /** Time in us that landing conditions have to hold before triggering a land. */
88  static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms;
89 
90  /** Time in us that almost landing conditions have to hold before triggering almost landed . */
91  static constexpr hrt_abstime MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 250_ms;
92 
93  /** Time in us that ground contact condition have to hold before triggering contact ground */
94  static constexpr hrt_abstime GROUND_CONTACT_TRIGGER_TIME_US = 350_ms;
95 
96  /** Time interval in us in which wider acceptance thresholds are used after landed. */
97  static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s;
98 
99  /** Handles for interesting parameters. **/
100  struct {
105  } _paramHandle{};
106 
107  struct {
108  float minThrottle;
111  float landSpeed;
112  } _params{};
113 
114  uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
116  uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
117  uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
118  uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
119  uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
120  uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
121 
122  actuator_controls_s _actuator_controls {};
123  battery_status_s _battery_status {};
124  vehicle_angular_velocity_s _vehicle_angular_velocity{};
125  vehicle_control_mode_s _vehicle_control_mode {};
126  vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {};
127 
128  hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first
129  hrt_abstime _landed_time{0};
130 
131  bool _in_descend{false}; ///< vehicle is desending
132  bool _horizontal_movement{false}; ///< vehicle is moving horizontally
133 
134  DEFINE_PARAMETERS_CUSTOM_PARENT(
135  LandDetector,
136  (ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max,
137  (ParamFloat<px4::params::LNDMC_FFALL_THR>) _param_lndmc_ffall_thr,
138  (ParamFloat<px4::params::LNDMC_FFALL_TTRI>) _param_lndmc_ffall_ttri,
139  (ParamFloat<px4::params::LNDMC_LOW_T_THR>) _param_lndmc_low_t_thr,
140  (ParamFloat<px4::params::LNDMC_ROT_MAX>) _param_lndmc_rot_max,
141  (ParamFloat<px4::params::LNDMC_XY_VEL_MAX>) _param_lndmc_xy_vel_max,
142  (ParamFloat<px4::params::LNDMC_Z_VEL_MAX>) _param_lndmc_z_vel_max
143  );
144 };
145 
146 } // namespace land_detector
static int _battery_sub
Definition: messages.cpp:60
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
Land detector interface for multicopter, fixedwing and VTOL implementations.
uint32_t param_t
Parameter handle.
Definition: param.h:98