PX4 Firmware
PX4 Autopilot Software http://px4.io
LandDetector.cpp
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 LandDetector.cpp
36  *
37  * @author Johan Jansen <jnsn.johan@gmail.com>
38  * @author Julian Oes <julian@oes.ch>
39  */
40 
41 #include "LandDetector.h"
42 
43 using namespace time_literals;
44 
45 namespace land_detector
46 {
47 
48 LandDetector::LandDetector() :
49  ModuleParams(nullptr),
50  ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
51 {}
52 
54 {
56 }
57 
59 {
61  ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL);
62 }
63 
65 {
67 
70  }
71 
74  _update_state();
75 
76  const bool freefallDetected = _freefall_hysteresis.get_state();
77  const bool ground_contactDetected = _ground_contact_hysteresis.get_state();
78  const bool maybe_landedDetected = _maybe_landed_hysteresis.get_state();
79  const bool landDetected = _landed_hysteresis.get_state();
80  const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : INFINITY;
81  const bool in_ground_effect = _ground_effect_hysteresis.get_state();
82 
83  const hrt_abstime now = hrt_absolute_time();
84 
85  // publish at 1 Hz, very first time, or when the result has changed
86  if ((hrt_elapsed_time(&_land_detected.timestamp) >= 1_s) ||
87  (_land_detected.landed != landDetected) ||
88  (_land_detected.freefall != freefallDetected) ||
89  (_land_detected.maybe_landed != maybe_landedDetected) ||
90  (_land_detected.ground_contact != ground_contactDetected) ||
91  (_land_detected.in_ground_effect != in_ground_effect) ||
92  (fabsf(_land_detected.alt_max - alt_max) > FLT_EPSILON)) {
93 
94  if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */
95  // We did take off
96  _takeoff_time = now;
97  }
98 
100  _land_detected.landed = landDetected;
101  _land_detected.freefall = freefallDetected;
102  _land_detected.maybe_landed = maybe_landedDetected;
103  _land_detected.ground_contact = ground_contactDetected;
104  _land_detected.alt_max = alt_max;
105  _land_detected.in_ground_effect = in_ground_effect;
106 
108  }
109 
110  // set the flight time when disarming (not necessarily when landed, because all param changes should
111  // happen on the same event and it's better to set/save params while not in armed state)
114  _takeoff_time = 0;
115 
116  uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
117 
118  _param_total_flight_time_high.set(flight_time);
119  _param_total_flight_time_high.commit_no_notification();
120 
121  flight_time = _total_flight_time & 0xffffffff;
122 
123  _param_total_flight_time_low.set(flight_time);
124  _param_total_flight_time_low.commit_no_notification();
125  }
126 
128 
130 
131  if (should_exit()) {
132  ScheduleClear();
133  exit_and_cleanup();
134  }
135 }
136 
138 {
139  parameter_update_s param_update;
140  _parameter_update_sub.copy(&param_update);
141 
142  updateParams();
144 }
145 
147 {
148  /* when we are landed we also have ground contact for sure but only one output state can be true at a particular time
149  * with higher priority for landed */
150  const hrt_abstime now_us = hrt_absolute_time();
156 }
157 
159 {
163 }
164 
166 {
167  _total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32;
168  _total_flight_time |= static_cast<uint32_t>(_param_total_flight_time_low.get());
169 }
170 
171 } // namespace land_detector
virtual float _get_max_altitude()
Definition: LandDetector.h:132
virtual void _update_topics()
Updates subscribed uORB topics.
vehicle_local_position_s _vehicle_local_position
Definition: LandDetector.h:160
virtual bool _get_landed_state()=0
hrt_abstime _total_flight_time
total vehicle flight time in microseconds
Definition: LandDetector.h:175
uORB::Subscription _vehicle_local_position_sub
Definition: LandDetector.h:182
actuator_armed_s _actuator_armed
Definition: LandDetector.h:148
static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL
Run main land detector loop at this interval.
Definition: LandDetector.h:140
systemlib::Hysteresis _maybe_landed_hysteresis
Definition: LandDetector.h:144
#define FLT_EPSILON
systemlib::Hysteresis _ground_contact_hysteresis
Definition: LandDetector.h:145
void set_state_and_update(const bool new_state, const hrt_abstime &now_us)
Definition: hysteresis.cpp:57
bool _previous_armed_state
stores the previous actuator_armed.armed state
Definition: LandDetector.h:172
vehicle_acceleration_s _vehicle_acceleration
Definition: LandDetector.h:149
uORB::Subscription _actuator_armed_sub
Definition: LandDetector.h:179
uORB::Subscription _vehicle_acceleration_sub
Definition: LandDetector.h:181
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
bool get_state() const
Definition: hysteresis.h:60
void perf_free(perf_counter_t handle)
Free a counter.
virtual bool _get_ground_contact_state()
Definition: LandDetector.h:122
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
void perf_end(perf_counter_t handle)
End a performance event.
bool updated()
Check if there is a new update.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
virtual bool _get_ground_effect_state()
Definition: LandDetector.h:137
virtual void _update_params()
Updates parameters.
virtual bool _get_maybe_landed_state()
Definition: LandDetector.h:117
uORB::Publication< vehicle_land_detected_s > _vehicle_land_detected_pub
Definition: LandDetector.h:162
systemlib::Hysteresis _ground_effect_hysteresis
Definition: LandDetector.h:146
Definition: bst.cpp:62
uORB::Subscription _parameter_update_sub
Definition: LandDetector.h:180
systemlib::Hysteresis _freefall_hysteresis
Definition: LandDetector.h:142
Land detector interface for multicopter, fixedwing and VTOL implementations.
bool update(void *dst)
Update the struct.
vehicle_land_detected_s _land_detected
Definition: LandDetector.h:151
virtual bool _get_freefall_state()
Definition: LandDetector.h:127
bool copy(void *dst)
Copy the struct.
systemlib::Hysteresis _landed_hysteresis
Definition: LandDetector.h:143
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void start()
Get the work queue going.