PX4 Firmware
PX4 Autopilot Software http://px4.io
FixedwingLandDetector.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 FixedwingLandDetector.cpp
36  *
37  * @author Johan Jansen <jnsn.johan@gmail.com>
38  * @author Lorenz Meier <lorenz@px4.io>
39  * @author Julian Oes <julian@oes.ch>
40  */
41 
42 #include "FixedwingLandDetector.h"
43 
44 namespace land_detector
45 {
46 
48 {
49  // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
52 }
53 
55 {
58 }
59 
61 {
62  // Only trigger flight conditions if we are armed.
63  if (!_actuator_armed.armed) {
64  return true;
65  }
66 
67  bool landDetected = false;
68 
70 
71  // Horizontal velocity complimentary filter.
72  float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx +
74 
75  if (PX4_ISFINITE(val)) {
77  }
78 
79  // Vertical velocity complimentary filter.
80  val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicle_local_position.vz);
81 
82  if (PX4_ISFINITE(val)) {
84  }
85 
87 
88  // A leaking lowpass prevents biases from building up, but
89  // gives a mostly correct response for short impulses.
91  const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1));
92 
93  _xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;
94 
95  // Crude land detector for fixedwing.
96  landDetected = _airspeed_filtered < _param_lndfw_airspd.get()
97  && _velocity_xy_filtered < _param_lndfw_vel_xy_max.get()
98  && _velocity_z_filtered < _param_lndfw_vel_z_max.get()
99  && _xy_accel_filtered < _param_lndfw_xyaccel_max.get();
100 
101  } else {
102  // Control state topic has timed out and we need to assume we're landed.
103  landDetected = true;
104  }
105 
106  return landDetected;
107 }
108 
109 } // namespace land_detector
virtual void _update_topics()
Updates subscribed uORB topics.
vehicle_local_position_s _vehicle_local_position
Definition: LandDetector.h:160
float true_airspeed_m_s
Definition: airspeed.h:55
actuator_armed_s _actuator_armed
Definition: LandDetector.h:148
Land detector implementation for fixedwing.
void _update_topics() override
Updates subscribed uORB topics.
vehicle_acceleration_s _vehicle_acceleration
Definition: LandDetector.h:149
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 set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
Definition: hysteresis.cpp:46
static constexpr hrt_abstime FLYING_TRIGGER_TIME_US
bool update(void *dst)
Update the struct.
static constexpr hrt_abstime LANDED_TRIGGER_TIME_US
Time in us that landing conditions have to hold before triggering a land.
systemlib::Hysteresis _landed_hysteresis
Definition: LandDetector.h:143