|
|
|
@ -39,14 +39,10 @@
@@ -39,14 +39,10 @@
|
|
|
|
|
* @author Julian Oes <julian@oes.ch> |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
#include "FixedwingLandDetector.h" |
|
|
|
|
|
|
|
|
|
#include <cmath> |
|
|
|
|
|
|
|
|
|
#include <px4_config.h> |
|
|
|
|
#include <px4_defines.h> |
|
|
|
|
#include <matrix/math.hpp> |
|
|
|
|
|
|
|
|
|
#include "FixedwingLandDetector.h" |
|
|
|
|
|
|
|
|
|
namespace land_detector |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
@ -90,7 +86,7 @@ bool FixedwingLandDetector::_get_landed_state()
@@ -90,7 +86,7 @@ bool FixedwingLandDetector::_get_landed_state()
|
|
|
|
|
|
|
|
|
|
bool landDetected = false; |
|
|
|
|
|
|
|
|
|
if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500 * 1000) { |
|
|
|
|
if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500_ms) { |
|
|
|
|
|
|
|
|
|
// Horizontal velocity complimentary filter.
|
|
|
|
|
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + |
|
|
|
@ -114,10 +110,10 @@ bool FixedwingLandDetector::_get_landed_state()
@@ -114,10 +110,10 @@ bool FixedwingLandDetector::_get_landed_state()
|
|
|
|
|
const matrix::Vector3f accel{_vehicle_acceleration.xyz}; |
|
|
|
|
const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1)); |
|
|
|
|
|
|
|
|
|
_accel_horz_lp = _accel_horz_lp * 0.8f + acc_hor * 0.18f; |
|
|
|
|
_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f; |
|
|
|
|
|
|
|
|
|
// crude land detector for fixedwing
|
|
|
|
|
landDetected = _accel_horz_lp < _param_lndfw_xyaccel_max.get() |
|
|
|
|
landDetected = _xy_accel_filtered < _param_lndfw_xyaccel_max.get() |
|
|
|
|
&& _airspeed_filtered < _param_lndfw_airspd.get() |
|
|
|
|
&& _velocity_xy_filtered < _param_lndfw_vel_xy_max.get() |
|
|
|
|
&& _velocity_z_filtered < _param_lndfw_vel_z_max.get(); |
|
|
|
|