FW handle TAKEOFF like a regular POSITION setpoint if already flying

PX4 · Jul 1, 2018 · 990d394 · 990d394
1 parent b0fa951
commit 990d394
Showing 1 changed file with 15 additions and 1 deletion.
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -802,9 +802,22 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 			mission_throttle = pos_sp_curr.cruising_throttle;
 		}
 
+
 		uint8_t position_sp_type = pos_sp_curr.type;
 
-		// achieve position setpoint altitude via loiter
+		// TAKEOFF: handle like a regular POSITION setpoint if already flying
+		if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
+
+			const bool airspeed_min = (_airspeed >= _parameters.airspeed_min);
+
+			if (!in_takeoff_situation() && airspeed_min) {
+
+				// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
+				position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+			}
+		}
+
+		// POSITION: achieve position setpoint altitude via loiter
 		if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 
 			float dist_xy = -1.0f;
@@ -825,6 +838,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 			}
 		}
 
+
 		if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 			_att_sp.thrust = 0.0f;
 			_att_sp.roll_body = 0.0f;