From 13c3ae3b5225c831c4fc8b0d923346a521df572e Mon Sep 17 00:00:00 2001
From: Jin Chengde <jinchengde@gmail.com>
Date: Mon, 14 Oct 2019 23:07:33 +0800
Subject: [PATCH] FW RWTO: add throttle_ramp_time parameter RWTO_RAMP_TIME

 * add throttle_ramp_time parameter support fixed wing runway takeoff
---
 .../runway_takeoff/RunwayTakeoff.cpp                 |  7 +++----
 .../fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h |  2 +-
 .../runway_takeoff/runway_takeoff_params.c           | 12 ++++++++++++
 3 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
index 322f1bff5c99..9c15497a2559 100644
--- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
+++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
@@ -57,8 +57,7 @@ RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) :
 	_initialized(false),
 	_initialized_time(0),
 	_init_yaw(0),
-	_climbout(false),
-	_throttle_ramp_time(2 * 1e6)
+	_climbout(false)
 {
 }
 
@@ -79,7 +78,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl,
 
 	switch (_state) {
 	case RunwayTakeoffState::THROTTLE_RAMP:
-		if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) {
+		if (hrt_elapsed_time(&_initialized_time) > _param_rwto_ramp_time.get() * 1e6f) {
 			_state = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
 		}
 
@@ -195,7 +194,7 @@ float RunwayTakeoff::getThrottle(float tecsThrottle)
 {
 	switch (_state) {
 	case RunwayTakeoffState::THROTTLE_RAMP: {
-			float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time *
+			float throttle = (hrt_elapsed_time(&_initialized_time) / (float)_param_rwto_ramp_time.get() * 1e6f) *
 					 _param_rwto_max_thr.get();
 			return throttle < _param_rwto_max_thr.get() ?
 			       throttle :
diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
index 15c9e8b005fb..b4f9c981ac50 100644
--- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
+++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
@@ -98,7 +98,6 @@ class __EXPORT RunwayTakeoff : public ModuleParams
 	hrt_abstime _initialized_time;
 	float _init_yaw;
 	bool _climbout;
-	unsigned _throttle_ramp_time;
 	matrix::Vector2f _start_wp;
 
 	DEFINE_PARAMETERS(
@@ -110,6 +109,7 @@ class __EXPORT RunwayTakeoff : public ModuleParams
 		(ParamFloat<px4::params::RWTO_MAX_PITCH>) _param_rwto_max_pitch,
 		(ParamFloat<px4::params::RWTO_MAX_ROLL>) _param_rwto_max_roll,
 		(ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _param_rwto_airspd_scl,
+		(ParamFloat<px4::params::RWTO_RAMP_TIME>) _param_rwto_ramp_time,
 		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
 		(ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _param_fw_clmbout_diff
 	)
diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c
index 7b91c8865b93..2ea76c35ed31 100644
--- a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c
+++ b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c
@@ -144,3 +144,15 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0);
  * @group Runway Takeoff
  */
 PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3);
+
+/**
+ * Throttle ramp up time for runway takeoff
+ *
+ * @unit s
+ * @min 1.0
+ * @max 15.0
+ * @decimal 2
+ * @increment 0.1
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f);