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

Fix land detection for altitude hold

Browse Source
This commit is contained in:
Lorenz Meier
2016-04-05 15:32:13 -07:00
parent 65d491cafc
commit 321440281b
2 changed files with 11 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+8
View File
@@ -53,6 +53,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
_armingSub(-1),
_parameterSub(-1),
_attitudeSub(-1),
_manualSub(-1),
_vehicleLocalPosition{},
_actuators{},
_arming{},
@@ -73,6 +74,7 @@ void MulticopterLandDetector::initialize()
_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
_parameterSub = orb_subscribe(ORB_ID(parameter_update));
_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
// download parameters
updateParameterCache(true);
@@ -84,6 +86,7 @@ void MulticopterLandDetector::updateSubscriptions()
orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
}
bool MulticopterLandDetector::update()
@@ -107,6 +110,11 @@ bool MulticopterLandDetector::get_landed_state()
_arming_time = hrt_absolute_time();
}
// Check if user commands throttle and if so, report not landed
if (_manual.z > 0.3f) {
return false;
}
// Check if thrust output is less than max throttle param.
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;