Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 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), _armingSub(-1),
_parameterSub(-1), _parameterSub(-1),
_attitudeSub(-1), _attitudeSub(-1),
_manualSub(-1),
_vehicleLocalPosition{}, _vehicleLocalPosition{},
_actuators{}, _actuators{},
_arming{}, _arming{},
@@ -73,6 +74,7 @@ void MulticopterLandDetector::initialize()
_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
_armingSub = orb_subscribe(ORB_ID(actuator_armed)); _armingSub = orb_subscribe(ORB_ID(actuator_armed));
_parameterSub = orb_subscribe(ORB_ID(parameter_update)); _parameterSub = orb_subscribe(ORB_ID(parameter_update));
_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
// download parameters // download parameters
updateParameterCache(true); updateParameterCache(true);
@@ -84,6 +86,7 @@ void MulticopterLandDetector::updateSubscriptions()
orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
} }
bool MulticopterLandDetector::update() bool MulticopterLandDetector::update()
@@ -107,6 +110,11 @@ bool MulticopterLandDetector::get_landed_state()
_arming_time = hrt_absolute_time(); _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. // Check if thrust output is less than max throttle param.
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
src/modules/land_detector/MulticopterLandDetector.h
+3
View File
@@ -49,6 +49,7 @@
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
class MulticopterLandDetector : public LandDetector class MulticopterLandDetector : public LandDetector
@@ -107,11 +108,13 @@ private:
int _armingSub; int _armingSub;
int _parameterSub; int _parameterSub;
int _attitudeSub; int _attitudeSub;
int _manualSub;
struct vehicle_local_position_s _vehicleLocalPosition; struct vehicle_local_position_s _vehicleLocalPosition;
struct actuator_controls_s _actuators; struct actuator_controls_s _actuators;
struct actuator_armed_s _arming; struct actuator_armed_s _arming;
struct vehicle_attitude_s _vehicleAttitude; struct vehicle_attitude_s _vehicleAttitude;
struct manual_control_setpoint_s _manual;
/* timestamp in microseconds since a possible land was detected */ /* timestamp in microseconds since a possible land was detected */
uint64_t _landTimer; uint64_t _landTimer;