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

landdetector: add additional landdetection state

This commit is contained in:
Dennis Mannhart 2017-05-24 16:42:14 +02:00 committed by Lorenz Meier
parent 529def11e8
commit f8e9f380d0
9 changed files with 76 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/vehicle_land_detected.msg
View File

@ -1,4 +1,5 @@
bool landed # true if vehicle is currently landed on the ground
bool freefall # true if vehicle is currently in free-fall
bool ground_contact # true if vehicle has ground contact but is not landed
float32 alt_max # maximum altitude in [m] that can be reached
bool maybe_landed # true if the vehicle might have landed
float32 alt_max # maximum altitude in [m] that can be reached

7
src/modules/land_detector/FixedwingLandDetector.cpp
View File

@ -112,6 +112,13 @@ bool FixedwingLandDetector::_get_ground_contact_state()
return false;
}
bool FixedwingLandDetector::_get_maybe_landed_state()
{
// TODO
return false;
}
bool FixedwingLandDetector::_get_landed_state()
{
// only trigger flight conditions if we are armed

2
src/modules/land_detector/FixedwingLandDetector.h
View File

@ -65,6 +65,8 @@ protected:
virtual bool _get_landed_state() override;
virtual bool _get_maybe_landed_state() override;
virtual bool _get_ground_contact_state() override;
virtual bool _get_freefall_state() override;

10
src/modules/land_detector/LandDetector.cpp
View File

@ -57,6 +57,7 @@ LandDetector::LandDetector() :
_state{},
_freefall_hysteresis(false),
_landed_hysteresis(true),
_maybe_landed_hysteresis(true),
_ground_contact_hysteresis(true),
_total_flight_time{0},
_takeoff_time{0},
@ -64,6 +65,7 @@ LandDetector::LandDetector() :
{
// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
_maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US);
_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US);
}
@ -93,6 +95,7 @@ void LandDetector::_cycle()
_landDetected.freefall = false;
_landDetected.landed = false;
_landDetected.ground_contact = false;
_landDetected.maybe_landed = false;
_p_total_flight_time_high = param_find("LND_FLIGHT_T_HI");
_p_total_flight_time_low = param_find("LND_FLIGHT_T_LO");
@ -117,6 +120,7 @@ void LandDetector::_cycle()
bool freefallDetected = (_state == LandDetectionState::FREEFALL);
bool landDetected = (_state == LandDetectionState::LANDED);
bool maybe_landedDetected = (_state == LandDetectionState::MAYBE_LANDED);
bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT);
// Only publish very first time or when the result has changed.
@ -124,6 +128,7 @@ void LandDetector::_cycle()
(_landDetected.freefall != freefallDetected) ||
(_landDetected.landed != landDetected) ||
(_landDetected.ground_contact != ground_contactDetected) ||
(_landDetected.maybe_landed != maybe_landedDetected) ||
(fabsf(_landDetected.alt_max - alt_max_prev) > FLT_EPSILON)) {
if (!landDetected && _landDetected.landed) {
@ -144,6 +149,7 @@ void LandDetector::_cycle()
_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
_landDetected.landed = (_state == LandDetectionState::LANDED);
_landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT);
_landDetected.maybe_landed = (_state == LandDetectionState::MAYBE_LANDED);
_landDetected.alt_max = _altitude_max;
int instance;
@ -188,6 +194,7 @@ void LandDetector::_update_state()
* with higher priority for landed */
_freefall_hysteresis.set_state_and_update(_get_freefall_state());
_landed_hysteresis.set_state_and_update(_get_landed_state());
_maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state());
_ground_contact_hysteresis.set_state_and_update(_landed_hysteresis.get_state() || _get_ground_contact_state());
if (_freefall_hysteresis.get_state()) {
@ -196,6 +203,9 @@ void LandDetector::_update_state()
} else if (_landed_hysteresis.get_state()) {
_state = LandDetectionState::LANDED;
} else if (_maybe_landed_hysteresis.get_state()) {
_state = LandDetectionState::MAYBE_LANDED;
} else if (_ground_contact_hysteresis.get_state()) {
_state = LandDetectionState::GROUND_CONTACT;

14
src/modules/land_detector/LandDetector.h
View File

@ -60,7 +60,8 @@ public:
FLYING = 0,
LANDED = 1,
FREEFALL = 2,
GROUND_CONTACT = 3
GROUND_CONTACT = 3,
MAYBE_LANDED = 4
};
LandDetector();
@ -115,6 +116,11 @@ protected:
*/
virtual bool _get_landed_state() = 0;
/**
* @return true if UAV is in almost landed state
*/
virtual bool _get_maybe_landed_state() = 0;
/**
* @return true if UAV is touching ground but not landed
*/
@ -141,7 +147,10 @@ protected:
static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50;
/** Time in us that landing conditions have to hold before triggering a land. */
static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 1500000;
static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 500000;
/** Time in us that almost landing conditions have to hold before triggering almost landed . */
static constexpr uint64_t MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 1500000;
/** Time in us that ground contact condition have to hold before triggering contact ground */
static constexpr uint64_t GROUND_CONTACT_TRIGGER_TIME_US = 1000000;
@ -158,6 +167,7 @@ protected:
systemlib::Hysteresis _freefall_hysteresis;
systemlib::Hysteresis _landed_hysteresis;
systemlib::Hysteresis _maybe_landed_hysteresis;
systemlib::Hysteresis _ground_contact_hysteresis;
float _altitude_max;

35
src/modules/land_detector/MulticopterLandDetector.cpp
View File

@ -186,10 +186,15 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// 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
return manual_control_idle_or_auto && _has_minimal_thrust() && (!verticalMovement || !_has_altitude_lock());
if (manual_control_idle_or_auto && _has_low_thrust() &&
(!verticalMovement || !_has_altitude_lock())) {
return true;
}
return false;
}
bool MulticopterLandDetector::_get_landed_state()
bool MulticopterLandDetector::_get_maybe_landed_state()
{
// Time base for this function
const uint64_t now = hrt_absolute_time();
@ -257,6 +262,17 @@ bool MulticopterLandDetector::_get_landed_state()
return false;
}
bool MulticopterLandDetector::_get_landed_state()
{
// if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0)
// therefore check if all other condition of the landed state remain true
if (_maybe_landed_hysteresis.get_state()) {
return true;
}
return false;
}
float MulticopterLandDetector::_get_takeoff_throttle()
{
/* Position mode */
@ -314,9 +330,21 @@ bool MulticopterLandDetector::_has_manual_control_present()
return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0;
}
bool MulticopterLandDetector::_has_low_thrust()
{
// 30% of throttle range between min and hover
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.3f;
PX4_INFO("_actuatl control 3: %.5f, sys_min_throttle: %.5f", (double)_actuators.control[3], (double)sys_min_throttle);
// Check if thrust output is less than the minimum auto throttle param.
return _actuators.control[3] <= sys_min_throttle;
}
bool MulticopterLandDetector::_has_minimal_thrust()
{
// 10% of throttle range between min and hover
// 10% of throttle range between min and hover once we entered ground contact
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * _params.throttleRange;
// Determine the system min throttle based on flight mode
@ -328,5 +356,4 @@ bool MulticopterLandDetector::_has_minimal_thrust()
return _actuators.control[3] <= sys_min_throttle;
}
}

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

@ -74,6 +74,8 @@ protected:
virtual bool _get_ground_contact_state() override;
virtual bool _get_maybe_landed_state() override;
virtual bool _get_freefall_state() override;
virtual float _get_max_altitude() override;
@ -139,6 +141,7 @@ private:
bool _has_position_lock();
bool _has_manual_control_present();
bool _has_minimal_thrust();
bool _has_low_thrust();
};

7
src/modules/land_detector/VtolLandDetector.cpp
View File

@ -76,6 +76,13 @@ bool VtolLandDetector::_get_ground_contact_state()
return MulticopterLandDetector::_get_ground_contact_state();
}
bool VtolLandDetector::_get_maybe_landed_state()
{
// TODO
return false;
}
bool VtolLandDetector::_get_landed_state()
{
// this is returned from the mutlicopter land detector

2
src/modules/land_detector/VtolLandDetector.h
View File

@ -62,6 +62,8 @@ protected:
virtual bool _get_landed_state() override;
virtual bool _get_maybe_landed_state() override;
virtual bool _get_ground_contact_state() override;
virtual bool _get_freefall_state() override;