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;