From 549d8da6eeefcb392d7a906698d9d3884a4dc2a7 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Fri, 30 Jun 2017 10:53:48 +0200 Subject: [PATCH] landdetector: hit ground logic --- .../land_detector/MulticopterLandDetector.cpp | 22 ++++++++++++++++++- .../land_detector/MulticopterLandDetector.h | 6 +++++ 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index d2c495e3e1..b1f3bb2179 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -75,6 +75,7 @@ MulticopterLandDetector::MulticopterLandDetector() : _paramHandle(), _params(), _vehicleLocalPositionSub(-1), + _vehicleLocalPositionSetpointSub(-1), _actuatorsSub(-1), _armingSub(-1), _attitudeSub(-1), @@ -83,6 +84,7 @@ MulticopterLandDetector::MulticopterLandDetector() : _vehicle_control_mode_sub(-1), _battery_sub(-1), _vehicleLocalPosition{}, + _vehicleLocalPositionSetpoint{}, _actuators{}, _arming{}, _vehicleAttitude{}, @@ -105,12 +107,14 @@ MulticopterLandDetector::MulticopterLandDetector() : _paramHandle.manual_stick_down_threshold = param_find("LNDMC_MAN_DWNTHR"); _paramHandle.altitude_max = param_find("LNDMC_ALT_MAX"); _paramHandle.manual_stick_up_position_takeoff_threshold = param_find("LNDMC_POS_UPTHR"); + _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); } void MulticopterLandDetector::_initialize_topics() { // subscribe to position, attitude, arming and velocity changes _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _vehicleLocalPositionSetpointSub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); _attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude)); _actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); @@ -124,6 +128,7 @@ void MulticopterLandDetector::_initialize_topics() void MulticopterLandDetector::_update_topics() { _orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + _orb_update(ORB_ID(vehicle_local_position_setpoint), _vehicleLocalPositionSetpointSub, &_vehicleLocalPositionSetpoint); _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); _orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators); _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); @@ -149,6 +154,7 @@ void MulticopterLandDetector::_update_params() param_get(_paramHandle.manual_stick_down_threshold, &_params.manual_stick_down_threshold); param_get(_paramHandle.altitude_max, &_params.altitude_max); param_get(_paramHandle.manual_stick_up_position_takeoff_threshold, &_params.manual_stick_up_position_takeoff_threshold); + param_get(_paramHandle.landSpeed, &_params.landSpeed); } @@ -206,9 +212,14 @@ bool MulticopterLandDetector::_get_ground_contact_state() // an accurate in-air indication. bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor; + // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, + // we then can assume that the vehicle hit ground + bool in_descend = _is_velocity_control_active() && (_vehicleLocalPositionSetpoint.vz >= 0.9f * _params.landSpeed); + bool hit_ground = in_descend && !verticalMovement; + // If pilots commands down or in auto mode and we are already below minimal thrust and we do not move down we assume ground contact // TODO: we need an accelerometer based check for vertical movement for flying without GPS - if (manual_control_idle_or_auto && _has_low_thrust() && + if (manual_control_idle_or_auto && (_has_low_thrust() || hit_ground) && (!verticalMovement || !_has_altitude_lock())) { return true; } @@ -349,6 +360,15 @@ bool MulticopterLandDetector::_has_manual_control_present() return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0; } +bool MulticopterLandDetector::_is_velocity_control_active() +{ + bool is_finite = PX4_ISFINITE(_vehicleLocalPositionSetpoint.vx) && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vy) + && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vz); + + return (_vehicleLocalPositionSetpoint.timestamp != 0) && + (hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500000) && is_finite; +} + bool MulticopterLandDetector::_has_low_thrust() { // 30% of throttle range between min and hover diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index ced060caac..6cb42ea19b 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -44,6 +44,7 @@ #include #include +#include #include #include #include @@ -97,6 +98,7 @@ private: param_t manual_stick_down_threshold; param_t altitude_max; param_t manual_stick_up_position_takeoff_threshold; + param_t landSpeed; } _paramHandle; struct { @@ -112,9 +114,11 @@ private: float manual_stick_down_threshold; float altitude_max; float manual_stick_up_position_takeoff_threshold; + float landSpeed; } _params; int _vehicleLocalPositionSub; + int _vehicleLocalPositionSetpointSub; int _actuatorsSub; int _armingSub; int _attitudeSub; @@ -124,6 +128,7 @@ private: int _battery_sub; struct vehicle_local_position_s _vehicleLocalPosition; + struct vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint; struct actuator_controls_s _actuators; struct actuator_armed_s _arming; struct vehicle_attitude_s _vehicleAttitude; @@ -142,6 +147,7 @@ private: bool _has_manual_control_present(); bool _has_minimal_thrust(); bool _has_low_thrust(); + bool _is_velocity_control_active(); };