From 7229ec2a378758350a9a4121c511c480550f7e15 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Oct 2017 11:59:09 +0200 Subject: [PATCH] Move throttle check from land detector to posctrl This commit is an attempt to fix a race condition happening on takeoff between the land detector and the multicopter position controller. Previously, an auto-takeoff leads to the following events: 1. A takeoff setpoint is given. 2. The thrust setpoint spikes because we don't enter smooth takeoff yet. 3. The land detector detects a takeoff because of the high thrust. 4. The position controller sees the landed state transition and initiates the smooth takeoff. Thrust goes back down. 5. Depending on control gains the takeoff is successful or fails if the smoothing takes too long which causes thrust to be too low, so the land detector detects land again. The two obvious problems with this are: - The intermittent spike. - The failed takeoff because of the smoothing leads to a delay.. With this change, the logic for a takeoff detection is moved from the land detector to the position controller. The events are now: 1. A takeoff setpoint is given. 2. The position controller detects the takeoff setpoint and initiates the smooth takeoff. 3. As thrust ramps up, the land detector detects the take off. In the same way, we now detect the intent to takeoff in manual, altitude, control, position control in the position controller instead of in the land detector. --- ROMFS/px4fmu_common/init.d/4070_aerofc | 2 - .../land_detector/MulticopterLandDetector.cpp | 55 +------------------ .../land_detector/MulticopterLandDetector.h | 8 --- .../land_detector/land_detector_params.c | 31 ----------- .../mc_pos_control/mc_pos_control_main.cpp | 46 ++++++++++------ 5 files changed, 29 insertions(+), 113 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4070_aerofc b/ROMFS/px4fmu_common/init.d/4070_aerofc index f9a6d8a992..1000e21363 100644 --- a/ROMFS/px4fmu_common/init.d/4070_aerofc +++ b/ROMFS/px4fmu_common/init.d/4070_aerofc @@ -25,8 +25,6 @@ then param set COM_DISARM_LAND 3 - param set LNDMC_MAN_DWNTHR 0.2500 - param set LNDMC_POS_UPTHR 0.5500 param set LNDMC_Z_VEL_MAX 2.0000 param set MC_ROLL_P 8.0000 diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 7419e53eec..3e1681965a 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -79,7 +79,6 @@ MulticopterLandDetector::MulticopterLandDetector() : _actuatorsSub(-1), _armingSub(-1), _attitudeSub(-1), - _manualSub(-1), _sensor_bias_sub(-1), _vehicle_control_mode_sub(-1), _battery_sub(-1), @@ -88,7 +87,6 @@ MulticopterLandDetector::MulticopterLandDetector() : _actuators{}, _arming{}, _vehicleAttitude{}, - _manual{}, _sensors{}, _control_mode{}, _battery{}, @@ -104,9 +102,7 @@ MulticopterLandDetector::MulticopterLandDetector() : _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); _paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR"); _paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI"); - _paramHandle.manual_stick_down_threshold = param_find("LNDMC_MAN_DWNTHR"); _paramHandle.altitude_max = param_find("LNDMC_ALT_MAX"); - _paramHandle.manual_stick_up_position_takeoff_threshold = param_find("LNDMC_POS_UPTHR"); _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). @@ -124,7 +120,6 @@ void MulticopterLandDetector::_initialize_topics() _actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); - _manualSub = orb_subscribe(ORB_ID(manual_control_setpoint)); _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _battery_sub = orb_subscribe(ORB_ID(battery_status)); @@ -137,7 +132,6 @@ void MulticopterLandDetector::_update_topics() _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); _orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators); _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); - _orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); _orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors); _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); _orb_update(ORB_ID(battery_status), _battery_sub, &_battery); @@ -156,9 +150,7 @@ void MulticopterLandDetector::_update_params() param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold); param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time); _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time)); - param_get(_paramHandle.manual_stick_down_threshold, &_params.manual_stick_down_threshold); param_get(_paramHandle.altitude_max, &_params.altitude_max); - param_get(_paramHandle.manual_stick_up_position_takeoff_threshold, &_params.manual_stick_up_position_takeoff_threshold); param_get(_paramHandle.landSpeed, &_params.landSpeed); } @@ -190,13 +182,6 @@ bool MulticopterLandDetector::_get_ground_contact_state() return true; } - // 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 - // the user intent to take off (even if the system might physically still have - // ground contact at this point). - const bool manual_control_idle = (_has_manual_control_present() && _manual.z < _params.manual_stick_down_threshold); - const bool manual_control_idle_or_auto = manual_control_idle || !_control_mode.flag_control_manual_enabled; - // land speed threshold float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f); @@ -229,9 +214,8 @@ bool MulticopterLandDetector::_get_ground_contact_state() && (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold); bool hit_ground = in_descend && !verticalMovement; - // 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 - if (manual_control_idle_or_auto && (_has_low_thrust() || hit_ground) && (!horizontalMovement || !_has_position_lock()) + if ((_has_low_thrust() || hit_ground) && (!horizontalMovement || !_has_position_lock()) && (!verticalMovement || !_has_altitude_lock())) { return true; } @@ -249,18 +233,6 @@ bool MulticopterLandDetector::_get_maybe_landed_state() return true; } - // 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 (_manual.z < _get_takeoff_throttle()) { - return true; - - } else { - // Pilot wants to take off, assume no groundcontact anymore and therefore allow thrust - _ground_contact_hysteresis.set_state_and_update(false); - return false; - } - } - if (_has_minimal_thrust()) { if (_min_trust_start == 0) { _min_trust_start = now; @@ -321,26 +293,6 @@ bool MulticopterLandDetector::_get_landed_state() } -float MulticopterLandDetector::_get_takeoff_throttle() -{ - /* Position mode */ - if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_altitude_enabled) { - /* Should be above 0.5 because below that we do not gain altitude and won't take off. - * Also it should be quite high such that we don't accidentally take off when using - * a spring loaded throttle and have a useful vertical speed to start with. */ - return _params.manual_stick_up_position_takeoff_threshold; - } - - /* Manual/attitude mode */ - if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { - /* Should be quite low and certainly below hover throttle because pilot controls throttle manually. */ - return 0.15f; - } - - /* As default for example in acro mode we do not want to stay landed. */ - return 0.0f; -} - float MulticopterLandDetector::_get_max_altitude() { /* ToDo: add a meaningful altitude */ @@ -373,11 +325,6 @@ bool MulticopterLandDetector::_has_position_lock() return _has_altitude_lock() && _vehicleLocalPosition.xy_valid; } -bool MulticopterLandDetector::_has_manual_control_present() -{ - return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0; -} - bool MulticopterLandDetector::_is_climb_rate_enabled() { bool has_updated = (_vehicleLocalPositionSetpoint.timestamp != 0) diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 22a9bfa976..de0b1090b7 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -52,7 +52,6 @@ #include #include #include -#include #include #include #include @@ -110,9 +109,7 @@ private: param_t minManThrottle; param_t freefall_acc_threshold; param_t freefall_trigger_time; - param_t manual_stick_down_threshold; param_t altitude_max; - param_t manual_stick_up_position_takeoff_threshold; param_t landSpeed; } _paramHandle; @@ -126,9 +123,7 @@ private: float minManThrottle; float freefall_acc_threshold; float freefall_trigger_time; - float manual_stick_down_threshold; float altitude_max; - float manual_stick_up_position_takeoff_threshold; float landSpeed; } _params; @@ -137,7 +132,6 @@ private: int _actuatorsSub; int _armingSub; int _attitudeSub; - int _manualSub; int _sensor_bias_sub; int _vehicle_control_mode_sub; int _battery_sub; @@ -147,7 +141,6 @@ private: struct actuator_controls_s _actuators; struct actuator_armed_s _arming; struct vehicle_attitude_s _vehicleAttitude; - struct manual_control_setpoint_s _manual; struct sensor_bias_s _sensors; struct vehicle_control_mode_s _control_mode; struct battery_status_s _battery; @@ -159,7 +152,6 @@ private: float _get_takeoff_throttle(); bool _has_altitude_lock(); bool _has_position_lock(); - bool _has_manual_control_present(); bool _has_minimal_thrust(); bool _has_low_thrust(); bool _is_climb_rate_enabled(); diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 15fb41a51d..0ebffb9023 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -119,37 +119,6 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_RANGE, 0.1f); */ PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3); -/** - * Manual flight stick down threshold for landing - * - * When controlling manually the throttle stick value (0 to 1) - * has to be bellow this threshold in order to pass the check for landing. - * So if set to 1 it's allowed to land with any stick position. - * - * @min 0 - * @max 1 - * @unit norm - * @decimal 2 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_MAN_DWNTHR, 0.15f); - -/** - * Manual position flight stick up threshold for taking off - * - * When controlling manually in position mode the throttle stick value (0 to 1) - * has to get above this threshold after arming in order to take off. - * - * @min 0 - * @max 1 - * @unit norm - * @decimal 2 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_POS_UPTHR, 0.65f); - /** * Fixedwing max horizontal velocity * diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index cd38d035b7..e0a1e71c36 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -128,7 +128,7 @@ private: bool _reset_yaw_sp = true; /** 0; + + // If the throttle stick is well above 50%, so above 62.5% (50% + 0.15 * 50%) we trigger manual takeoff. + const bool manual_wants_takeoff = (has_manual_control_present && _manual.z > 0.15f); + + return manual_wants_takeoff; +} + void MulticopterPositionControl::task_main() { @@ -3048,7 +3052,6 @@ MulticopterPositionControl::task_main() _reset_yaw_sp = true; _hold_offboard_xy = false; _hold_offboard_z = false; - _in_takeoff = false; _in_landing = false; _lnd_reached_ground = false; @@ -3073,12 +3076,19 @@ MulticopterPositionControl::task_main() _vel_sp_prev = _vel; } - /* switch to smooth takeoff if we got out of landed state */ - if (!_vehicle_land_detected.landed && was_landed) { - _in_takeoff = true; + if (!_in_smooth_takeoff && _vehicle_land_detected.landed && _control_mode.flag_armed && + (in_auto_takeoff() || manual_wants_takeoff())) { + _in_smooth_takeoff = true; + // This ramp starts negative and goes to positive later because we want to + // be as smooth as possible. If we start at 0, we alrady jump to hover throttle. _takeoff_vel_limit = -0.5f; } + else if (!_control_mode.flag_armed) { + // If we're disarmed and for some reason were in a smooth takeoff, we reset that. + _in_smooth_takeoff = false; + } + /* set triplets to invalid if we just landed */ if (_vehicle_land_detected.landed && !was_landed) { _pos_sp_triplet.current.valid = false;