diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 538961c324..64c0b923ec 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -1215,21 +1215,22 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c const float roll_wingtip_strike = getMaxRollAngleNearGround(_current_altitude, _takeoff_ground_alt); _ctrl_configuration_handler.setLateralAccelMax(rollAngleToLateralAccel(roll_wingtip_strike)); - const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? - _param_fw_thr_idle.get() : NAN; + const float direct_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? + _param_fw_thr_pre_laun.get() : NAN; + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { .timestamp = now, .altitude = NAN, .height_rate = _param_fw_t_clmb_max.get(), .equivalent_airspeed = takeoff_airspeed, .pitch_direct = NAN, - .throttle_direct = NAN + .throttle_direct = direct_throttle }; _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); _ctrl_configuration_handler.setPitchMin(radians(_takeoff_pitch_min.get())); - _ctrl_configuration_handler.setThrottleMax(max_takeoff_throttle); + _ctrl_configuration_handler.setThrottleMax(_param_fw_thr_max.get()); _ctrl_configuration_handler.setClimbRateTarget(_param_fw_t_clmb_max.get()); _ctrl_configuration_handler.setDisableUnderspeedProtection(true); @@ -1245,7 +1246,7 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c fixed_wing_longitudinal_setpoint_s long_control_sp{empty_longitudinal_control_setpoint}; long_control_sp.timestamp = now; long_control_sp.pitch_direct = radians(_takeoff_pitch_min.get()); - long_control_sp.throttle_direct = _param_fw_thr_idle.get(); + long_control_sp.throttle_direct = _param_fw_thr_pre_laun.get(); _longitudinal_ctrl_sp_pub.publish(long_control_sp); } @@ -1358,13 +1359,17 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? _param_fw_thr_idle.get() : NAN; + + const float direct_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? + _param_fw_thr_pre_laun.get() : NAN; + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { .timestamp = now, .altitude = NAN, .height_rate = _param_fw_t_clmb_max.get(), .equivalent_airspeed = takeoff_airspeed, .pitch_direct = NAN, - .throttle_direct = NAN + .throttle_direct = direct_throttle }; _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp index fcc6270a22..c2df1a75df 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.hpp @@ -858,6 +858,8 @@ private: (ParamFloat) _param_nav_gpsf_r, (ParamFloat) _param_t_spdweight, + (ParamFloat) _param_fw_thr_pre_laun, + // external parameters (ParamBool) _param_fw_use_airspd, (ParamFloat) _param_nav_loiter_rad, diff --git a/src/modules/fw_mode_manager/fw_mode_manager_params.c b/src/modules/fw_mode_manager/fw_mode_manager_params.c index 3a2edb6052..62c3a14588 100644 --- a/src/modules/fw_mode_manager/fw_mode_manager_params.c +++ b/src/modules/fw_mode_manager/fw_mode_manager_params.c @@ -580,6 +580,20 @@ PARAM_DEFINE_INT32(FW_LND_ABORT, 3); */ PARAM_DEFINE_INT32(FW_LAUN_DETCN_ON, 0); +/** + * Pre-launch throttle + * + * Throttle value set before launch detection and until the motor delay is up. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Auto Takeoff + */ +PARAM_DEFINE_FLOAT(FW_THR_PRE_LAUN, 0.0f); + /** * Flaps setting during take-off *