dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fix land detection for altitude hold

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/land_detector/MulticopterLandDetector.cpp
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;

3
src/modules/land_detector/MulticopterLandDetector.h
View File

@ -49,6 +49,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h>
class MulticopterLandDetector : public LandDetector
@ -107,11 +108,13 @@ private:
int _armingSub;
int _parameterSub;
int _attitudeSub;
int _manualSub;
struct vehicle_local_position_s _vehicleLocalPosition;
struct actuator_controls_s _actuators;
struct actuator_armed_s _arming;
struct vehicle_attitude_s _vehicleAttitude;
struct manual_control_setpoint_s _manual;
/* timestamp in microseconds since a possible land was detected */
uint64_t _landTimer;