fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 19:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

landdetector: hit ground logic

Browse Source
This commit is contained in:
Dennis Mannhart
2017-06-30 10:53:48 +02:00
committed by Lorenz Meier
parent 363ed43d38
commit 549d8da6ee
2 changed files with 27 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+21 -1
View File
@@ -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
src/modules/land_detector/MulticopterLandDetector.h
+6
View File
@@ -44,6 +44,7 @@
#include <systemlib/param/param.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
@@ -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();
};