diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index ac6f0559176d..ca878f8d563f 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1060,7 +1060,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons. the pre-takeoff throttle and the idle throttle normally map to the same parameter. */ - _att_sp.thrust = _parameters.throttle_idle; + _att_sp.thrust = _launchDetector.getThrottleIdle(_parameters.throttle_idle); } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && @@ -1078,7 +1078,8 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto } else { /* Copy thrust and pitch values from tecs */ - if (_vehicle_land_detected.landed) { + if (_vehicle_land_detected.landed && !(_launch_detection_state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS + && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { // when we are landed state we want the motor to spin at idle speed _att_sp.thrust = min(_parameters.throttle_idle, throttle_max); @@ -1231,7 +1232,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector float takeoff_throttle = _parameters.throttle_max; if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - takeoff_throttle = _parameters.throttle_idle; + takeoff_throttle = _launchDetector.getThrottleIdle(_parameters.throttle_idle); } /* select maximum pitch: the launchdetector may impose another limit for the pitch @@ -1820,7 +1821,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee _last_tecs_update = hrt_absolute_time(); // do not run TECS if we are not in air - bool run_tecs = !_vehicle_land_detected.landed; + bool run_tecs = !_vehicle_land_detected.landed || (_launch_detection_state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS + && mode == tecs_status_s::TECS_MODE_TAKEOFF); + // handles case of land detected / launch detected interference during take-off // do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition // (it should also not run during VTOL blending because airspeed is too low still) @@ -1896,10 +1899,12 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee } /* tell TECS to update its state, but let it know when it cannot actually control the plane */ - bool in_air_alt_control = (!_vehicle_land_detected.landed && - (_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_altitude_enabled)); + bool in_air_alt_control = ((!_vehicle_land_detected.landed || + (_launch_detection_state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS + && mode == tecs_status_s::TECS_MODE_TAKEOFF)) + && (_control_mode.flag_control_auto_enabled + || _control_mode.flag_control_velocity_enabled + || _control_mode.flag_control_altitude_enabled)); /* update TECS vehicle state estimates */ _tecs.update_vehicle_state_estimates(_airspeed, _R_nb, diff --git a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp index e277fd3c7e30..a6f0a38efe98 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp +++ b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp @@ -124,4 +124,11 @@ float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) } } +float CatapultLaunchMethod::getThrottleIdle(float throttleIdleDefault) +{ + // use operator defined launching idle (ignore default idle) + return _throttleIdle.get(); +} + + } // namespace launchdetection diff --git a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h index f356d85e23b6..71b6b26a3d09 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h +++ b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h @@ -59,6 +59,7 @@ class CatapultLaunchMethod : public LaunchMethod, public ModuleParams LaunchDetectionResult getLaunchDetected() const override; void reset() override; float getPitchMax(float pitchMaxDefault) override; + float getThrottleIdle(float throttleIdleDefault) override; private: hrt_abstime _last_timestamp{0}; @@ -71,9 +72,10 @@ class CatapultLaunchMethod : public LaunchMethod, public ModuleParams (ParamFloat) _thresholdAccel, (ParamFloat) _thresholdTime, (ParamFloat) _motorDelay, - (ParamFloat) _pitchMaxPreThrottle /**< Upper pitch limit before throttle is turned on. + (ParamFloat) _pitchMaxPreThrottle, /**< Upper pitch limit before throttle is turned on. Can be used to make sure that the AC does not climb too much while attached to a bungee */ + (ParamFloat) _throttleIdle ) }; diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp index 36f36f1ab0bf..7182fc1d4df5 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp @@ -119,4 +119,25 @@ float LaunchDetector::getPitchMax(float pitchMaxDefault) } } +float LaunchDetector::getThrottleIdle(float throttleIdleDefault) +{ + if (!launchDetectionEnabled()) { + return throttleIdleDefault; + } + + /* if a lauchdetectionmethod is active or only one exists return the idle throttle from this method, + * otherwise use the default */ + if (_activeLaunchDetectionMethodIndex < 0) { + if (sizeof(_launchMethods) / sizeof(LaunchMethod *) > 1) { + return throttleIdleDefault; + + } else { + return _launchMethods[0]->getThrottleIdle(throttleIdleDefault); + } + + } else { + return _launchMethods[_activeLaunchDetectionMethodIndex]->getThrottleIdle(throttleIdleDefault); + } +} + } // namespace launchdetection diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h index 30db9726c7c7..c058936fdcaf 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h @@ -64,6 +64,8 @@ class __EXPORT LaunchDetector : public ModuleParams /* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */ float getPitchMax(float pitchMaxDefault); + /* Returns an idle throttle specific to the pre- "motors enabled" launch detection phase */ + float getThrottleIdle(float throttleIdleDefault); private: /* holds an index to the launchMethod in the array _launchMethods diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h b/src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h index 06eb5af9706d..f8f03ec27297 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h @@ -65,6 +65,8 @@ class LaunchMethod /* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */ virtual float getPitchMax(float pitchMaxDefault) = 0; + /* Returns an idle throttle specific to the pre- "motors enabled" launch detection phase */ + virtual float getThrottleIdle(float throttleIdleDefault) = 0; }; diff --git a/src/modules/fw_pos_control_l1/launchdetection/launchdetection_params.c b/src/modules/fw_pos_control_l1/launchdetection/launchdetection_params.c index dd883e04adab..a95ce87e481d 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/launchdetection_params.c +++ b/src/modules/fw_pos_control_l1/launchdetection/launchdetection_params.c @@ -108,3 +108,14 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); * @group FW Launch detection */ PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f); + +/** + * Idle throttle setting during launch detection before motors enabled. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_THR_IDLE, 0.0f);