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

Land detector fix for VTOL in FW mode

Browse Source
This commit is contained in:
sanderux
2017-08-09 08:12:54 +02:00
committed by Lorenz Meier
parent 72f9371ca4
commit 49d2e8a3ff
2 changed files with 23 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+12
View File
@@ -123,6 +123,7 @@ void MulticopterLandDetector::_initialize_topics()
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_battery_sub = orb_subscribe(ORB_ID(battery_status)); _battery_sub = orb_subscribe(ORB_ID(battery_status));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
} }
void MulticopterLandDetector::_update_topics() void MulticopterLandDetector::_update_topics()
@@ -136,6 +137,7 @@ void MulticopterLandDetector::_update_topics()
_orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); _orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
_orb_update(ORB_ID(battery_status), _battery_sub, &_battery); _orb_update(ORB_ID(battery_status), _battery_sub, &_battery);
_orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
} }
void MulticopterLandDetector::_update_params() void MulticopterLandDetector::_update_params()
@@ -192,6 +194,11 @@ bool MulticopterLandDetector::_get_ground_contact_state()
_arming_time = now; _arming_time = now;
} }
// Only trigger in RW mode
if (!_vehicle_status.is_rotary_wing) {
return false;
}
// If in manual flight mode never report landed if the user has more than idle throttle // If in manual flight mode never report landed if the user has more than idle throttle
// Check if user commands throttle and if so, report no ground contact based on // Check if user commands throttle and if so, report no ground contact based on
// the user intent to take off (even if the system might physically still have // the user intent to take off (even if the system might physically still have
@@ -242,6 +249,11 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
return true; return true;
} }
// Only trigger in RW mode
if (!_vehicle_status.is_rotary_wing) {
return false;
}
// If we control manually and are still landed, we want to stay idle until the pilot rises the throttle for takeoff // If we control manually and are still landed, we want to stay idle until the pilot rises the throttle for takeoff
if (_state == LandDetectionState::LANDED && _has_manual_control_present()) { if (_state == LandDetectionState::LANDED && _has_manual_control_present()) {
if (_manual.z < _get_takeoff_throttle()) { if (_manual.z < _get_takeoff_throttle()) {
src/modules/land_detector/MulticopterLandDetector.h
+11 -9
View File
@@ -126,16 +126,18 @@ private:
int _ctrl_state_sub; int _ctrl_state_sub;
int _vehicle_control_mode_sub; int _vehicle_control_mode_sub;
int _battery_sub; int _battery_sub;
int _vehicle_status_sub;
struct vehicle_local_position_s _vehicleLocalPosition; struct vehicle_local_position_s _vehicleLocalPosition;
struct vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint; struct vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint;
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; struct manual_control_setpoint_s _manual;
struct control_state_s _ctrl_state; struct control_state_s _ctrl_state;
struct vehicle_control_mode_s _control_mode; struct vehicle_control_mode_s _control_mode;
struct battery_status_s _battery; struct battery_status_s _battery;
struct vehicle_status_s _vehicle_status;
uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first
uint64_t _arming_time; uint64_t _arming_time;