From f387c0ccc3fca38a2989b784847743b752a78de6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 5 Jan 2014 14:19:19 +0100
Subject: launchdetection: rename pre takeoff throttle param and fix usage

---
 src/lib/launchdetection/LaunchDetector.cpp               |  4 ++--
 src/lib/launchdetection/LaunchDetector.h                 |  4 ++--
 src/lib/launchdetection/launchdetection_params.c         |  2 +-
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 ++++++++++++--
 4 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index d894d6a6f..67b32ad82 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -44,7 +44,7 @@
 
 LaunchDetector::LaunchDetector() :
 	launchdetection_on(NULL, "LAUN_ALL_ON", false),
-	throttle_min(NULL, "LAUN_THR_MIN", false)
+	throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
 {
 	/* init all detectors */
 	launchMethods[0] = new CatapultLaunchMethod();
@@ -85,7 +85,7 @@ void LaunchDetector::updateParams() {
 
 	warnx(" LaunchDetector::updateParams()");
 	launchdetection_on.update();
-	throttle_min.update();
+	throttlePreTakeoff.update();
 
 	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
 		launchMethods[i]->updateParams();
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 3b8aa0ced..7c2ff826c 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -59,14 +59,14 @@ public:
 	void updateParams();
 	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
 
-	float getMinThrottle() {return throttle_min.get(); }
+	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
 
 //	virtual bool getLaunchDetected();
 protected:
 private:
 	LaunchMethod* launchMethods[1];
 	control::BlockParamInt launchdetection_on;
-	control::BlockParamFloat throttle_min;
+	control::BlockParamFloat throttlePreTakeoff;
 
 
 };
diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index e07a2b26d..63a8981aa 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -69,4 +69,4 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
 // @DisplayName		Throttle setting while detecting the launch
 // @Description		The throttle is set to this value while the system is waiting for the takeoff
 // @Range		0 to 1
-PARAM_DEFINE_FLOAT(LAUN_THR_MIN, 0.0f);
+PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 826ee0cdb..04caf0bbc 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -174,6 +174,7 @@ private:
 
 	/* takeoff/launch states */
 	bool launch_detected;
+	bool usePreTakeoffThrust;
 	bool launch_detection_message_sent;
 
 	/* Landingslope object */
@@ -396,6 +397,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_mavlink_fd(-1),
 	launchDetector(),
 	launch_detected(false),
+	usePreTakeoffThrust(false),
 	launch_detection_message_sent(false)
 {
 	/* safely initialize structs */
@@ -1004,6 +1006,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
 			if (launch_detected) {
+				usePreTakeoffThrust = false;
 
 				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
 				if (altitude_error > 15.0f) {
@@ -1028,7 +1031,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				}
 
 			} else {
-				throttle_max = launchDetector.getMinThrottle();
+				usePreTakeoffThrust = true;
 			}
 		}
 
@@ -1053,6 +1056,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		/* reset takeoff/launch state */
 		if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
 			launch_detected = false;
+			usePreTakeoffThrust = false;
 			launch_detection_message_sent = false;
 		}
 
@@ -1150,8 +1154,14 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		setpoint = false;
 	}
 
+	if (usePreTakeoffThrust) {
+		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
+	}
+	else {
+		_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
+	}
 	_att_sp.pitch_body = _tecs.get_pitch_demand();
-	_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
+
 
 	return setpoint;
 }
-- 
cgit v1.2.3