From 966f0cb26efde943ccc574e2faca2e0e79aeb912 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Sat, 8 Jun 2019 15:18:56 +0100
Subject: [PATCH] temp

---
 .../FlightTaskManualAcceleration.cpp          | 57 ++++++++++++++++---
 .../mc_pos_control/mc_pos_control_main.cpp    |  3 +-
 2 files changed, 51 insertions(+), 9 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/lib/FlightTasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
index 7028701fa106..3449c8dace4c 100644
--- a/src/lib/FlightTasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
+++ b/src/lib/FlightTasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
@@ -41,11 +41,6 @@
 
 using namespace matrix;
 
-FlightTaskManualAcceleration::FlightTaskManualAcceleration()
-{
-
-}
-
 bool FlightTaskManualAcceleration::updateInitialize()
 {
 	bool ret = FlightTaskManual::updateInitialize();
@@ -67,17 +62,63 @@ bool FlightTaskManualAcceleration::update()
 			Quatf(_sub_attitude->get().delta_q_reset));
 
 	// Map stick input to acceleration
-	Vector2f stick_xy(&_sticks(0));
+	Vector2f stick_xy(&_sticks_expo(0));
 	_position_lock.limitStickUnitLengthXY(stick_xy);
 	_position_lock.rotateIntoHeadingFrameXY(stick_xy, _yaw, _yaw_setpoint);
 
-	_acceleration_setpoint = Vector3f(stick_xy(0), stick_xy(1), _sticks(2));
-
+	_acceleration_setpoint = Vector3f(stick_xy(0), stick_xy(1), _sticks_expo(2));
 	_acceleration_setpoint *= 10;
 
 	// Add drag to limit speed and brake again
 	_acceleration_setpoint -= 2.f * _velocity;
 
+	// Position lock
+	if (Vector2f(stick_xy).length() > FLT_EPSILON) {
+		_position_setpoint(0) = _position_setpoint(1) = NAN;
+		_velocity_setpoint(0) = _velocity_setpoint(1) = NAN;
+	} else {
+		Vector2f position_xy(_position_setpoint);
+		Vector2f velocity_xy(_velocity_setpoint);
+
+		if (!PX4_ISFINITE(position_xy(0))) {
+			position_xy = Vector2f(_position);
+			velocity_xy = Vector2f(_velocity);
+		}
+
+		position_xy += Vector2f(velocity_xy) * _deltatime;
+
+		const Vector2f velocity_xy_last = velocity_xy;
+		velocity_xy += Vector2f(_acceleration_setpoint) * _deltatime;
+		if (velocity_xy.norm_squared() > velocity_xy_last.norm_squared()) {
+			velocity_xy = velocity_xy_last;
+		}
+		printf("%.3f\n", (double)velocity_xy.norm_squared());
+
+		_position_setpoint(0) = position_xy(0);
+		_position_setpoint(1) = position_xy(1);
+		_velocity_setpoint(0) = velocity_xy(0);
+		_velocity_setpoint(1) = velocity_xy(1);
+	}
+
+	// Altitude lock
+	if (fabsf(_sticks_expo(2)) > FLT_EPSILON) {
+		_position_setpoint(2) = NAN;
+		_velocity_setpoint(2) = NAN;
+	} else {
+		if (!PX4_ISFINITE(_position_setpoint(2))) {
+			_position_setpoint(2) = _position(2);
+			_velocity_setpoint(2) = _velocity(2);
+		}
+
+		_position_setpoint(2) += _velocity_setpoint(2) * _deltatime;
+
+		const float velocity_setpoint_z_last = _velocity_setpoint(2);
+		_velocity_setpoint(2) += _acceleration_setpoint(2) * _deltatime;
+		if (fabsf(_velocity_setpoint(2)) > fabsf(velocity_setpoint_z_last)) {
+			_velocity_setpoint(2) = velocity_setpoint_z_last;
+		}
+	}
+
 	_constraints.want_takeoff = _checkTakeoff();
 	return true;
 }
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 8855f2b656a2..8a927beea05f 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -688,7 +688,8 @@ MulticopterPositionControl::run()
 			_flight_tasks.updateVelocityControllerIO(Vector3f(local_pos_sp.x, local_pos_sp.y, local_pos_sp.z), local_pos_sp.thrust);
 
 			// Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint.
-			_att_sp = ControlMath::thrustToAttitude(matrix::Vector3f(local_pos_sp.thrust), local_pos_sp.yaw);
+			//_att_sp = ControlMath::thrustToAttitude(matrix::Vector3f(local_pos_sp.thrust), local_pos_sp.yaw);
+			_att_sp = ControlMath::accelerationToAttitude(Vector3f(local_pos_sp.acceleration), local_pos_sp.yaw, _param_mpc_thr_hover.get());
 			_att_sp.yaw_sp_move_rate = local_pos_sp.yawspeed;
 			_att_sp.fw_control_yaw = false;
 			_att_sp.apply_flaps = false;