TEMP

src/lib/FlightTasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -77,10 +77,6 @@ bool FlightTaskManualAcceleration::update()
 
 	// Map sticks input to acceleration
 	_acceleration_setpoint = Vector3f(&_sticks_expo(0)) * 10;
-	_velocity_setpoint = Vector3f();
-
-	printf("ACC IN TASK:\n");
-	_acceleration_setpoint.print();
 
 	// Rotate horizontal acceleration input to body heading
 	float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
@@ -89,6 +85,11 @@ bool FlightTaskManualAcceleration::update()
 	_acceleration_setpoint(0) = v_r(0);
 	_acceleration_setpoint(1) = v_r(1);
 
+	_acceleration_setpoint -= 2.f * _velocity;
+
+	// printf("ACC TASK:\n");
+	// _acceleration_setpoint.print();
+
 	_constraints.want_takeoff = _checkTakeoff();
 	return true;
 }

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -318,6 +318,8 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	_thrust_setpoint(0) = sp(0);
 	_thrust_setpoint(1) = sp(1);
 	_thrust_setpoint(2) = NAN;
+	_acceleration_setpoint(0) = 10.f * sp(0);
+	_acceleration_setpoint(1) = 10.f * sp(1);
 
 	_updateAltitudeLock();
 	_respectGroundSlowdown();

src/lib/FlightTasks/tasks/Utility/PositionLock.hpp

@@ -59,10 +59,10 @@ class PositionLock
 	 * @return yaw setpoint to execute to have a yaw lock at the correct moment in time
 	 */
 	void updateYawFromStick(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, const float yaw,
-				const float dt)
+				const float deltatime)
 	{
-		_yawspeed_slew_rate.setSlewRate(1.f);
-		yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, dt);
+		_yawspeed_slew_rate.setSlewRate(2.f * M_PI_F);
+		yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime);
 		yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint);
 	}
 

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -59,6 +59,7 @@ vehicle_attitude_setpoint_s accelerationToAttitude(const Vector3f &acc_sp, const
 	Vector3f body_z = Vector3f(-acc_sp(0), -acc_sp(1), A_GRAVITY);
 	vehicle_attitude_setpoint_s att_sp = bodyzToAttitude(body_z, yaw_sp);
 	att_sp.thrust_body[2] = acc_sp(2) * (hover_thrust / A_GRAVITY) - hover_thrust;
+	att_sp.thrust_body[2] = att_sp.thrust_body[2] / (Vector3f(0, 0, 1).dot(body_z.normalized()));
 	return att_sp;
 }
 

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -141,71 +141,56 @@ void PositionControl::_velocityController(const float &dt)
 	// - the desired thrust in NE-direction is limited by the thrust excess after
 	// 	 consideration of the desired thrust in D-direction. In addition, the thrust in
 	// 	 NE-direction is also limited by the maximum tilt.
-	Vector3f thr_sp_velocity; // thrust setpoint output of the velocity controller
-	Vector3f acc_sp_velocity; // acceleration setpoint output of the velocity controller
+	static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2
+	const float hover_scale = CONSTANTS_ONE_G / _param_mpc_thr_hover.get(); // For backwards compatibility of the gains
+	const Vector3f vel_gain_p(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), _param_mpc_z_vel_p.get());
+	const Vector3f vel_gain_i(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get());
+	const Vector3f vel_gain_d(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get());
 
 	const Vector3f vel_err = _vel_sp - _vel;
 
-	// Acceleration to thrust addition conversion
-	static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2
-	const float scale = CONSTANTS_ONE_G / _param_mpc_thr_hover.get();
-	const Vector3f thr_ff = _acc_sp * _param_mpc_thr_hover.get() / CONSTANTS_ONE_G;
-
-	// Consider thrust in D-direction.
-	float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) +  _param_mpc_z_vel_d.get() * _vel_dot(2) + _vel_int(
-					 2) - _param_mpc_thr_hover.get();
-	_addIfNotNan(thrust_desired_D, thr_ff(2));
+	Vector3f acc_sp_velocity = vel_gain_p.emult(vel_err) + _vel_int + vel_gain_d.emult(_vel_dot);
+	acc_sp_velocity *= hover_scale;
+	_addIfNotNanVector(_acc_sp, acc_sp_velocity);
 
-	acc_sp_velocity(2) += scale * (_param_mpc_z_vel_p.get() * vel_err(2) +  _param_mpc_z_vel_d.get() * _vel_dot(2) + _vel_int(
-				       2));
+	Vector3f thr_sp_velocity = (_acc_sp / hover_scale) - _param_mpc_thr_hover.get();
 
 	// The Thrust limits are negated and swapped due to NED-frame.
-	float uMax = -_param_mpc_thr_min.get();
+	float uMax = math::min(-_param_mpc_thr_min.get(), -10e-4f);
 	float uMin = -_param_mpc_thr_max.get();
 
-	// make sure there's always enough thrust vector length to infer the attitude
-	uMax = math::min(uMax, -10e-4f);
-
-	// Apply Anti-Windup in D-direction.
-	bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
-			       (thrust_desired_D <= uMin && vel_err(2) <= 0.0f);
+	// Apply Anti-Windup in vertical direction
+	bool stop_integral_D = (thr_sp_velocity(2) >= uMax && vel_err(2) >= 0.0f) ||
+			       (thr_sp_velocity(2) <= uMin && vel_err(2) <= 0.0f);
 
 	if (!stop_integral_D) {
-		_vel_int(2) += vel_err(2) * _param_mpc_z_vel_i.get() * dt;
+		_vel_int(2) += vel_err(2) * vel_gain_i(2) * dt;
 
 		// limit thrust integral
 		_vel_int(2) = math::min(fabsf(_vel_int(2)), _param_mpc_thr_max.get()) * math::sign(_vel_int(2));
 	}
 
 	// Saturate thrust setpoint in D-direction.
-	thr_sp_velocity(2) = math::constrain(thrust_desired_D, uMin, uMax);
+	thr_sp_velocity(2) = math::constrain(thr_sp_velocity(2), uMin, uMax);
 
 	if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) {
 		// Thrust set-point in NE-direction is already provided. Only
 		// scaling by the maximum tilt is required.
 		float thr_xy_max = fabsf(thr_sp_velocity(2)) * tanf(_constraints.tilt);
 		_thr_sp(0) *= thr_xy_max;
 		_thr_sp(1) *= thr_xy_max;
+		thr_sp_velocity(0) = thr_sp_velocity(1) = 0.f;
 
 	} else {
-		// PID-velocity controller for NE-direction.
-		Vector2f thrust_desired_NE;
-		thrust_desired_NE(0) = _param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _vel_int(0);
-		thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _vel_int(1);
-		_addIfNotNan(thrust_desired_NE(0), thr_ff(0));
-		_addIfNotNan(thrust_desired_NE(1), thr_ff(1));
-
-		acc_sp_velocity(0) += scale * (_param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _vel_int(0));
-		acc_sp_velocity(1) += scale * (_param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _vel_int(1));
-
-		// Get maximum allowed thrust in NE based on tilt and excess thrust.
+		// Get maximum allowed horizontal thrust based on tilt and excess thrust.
 		float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
 		float thrust_max_NE = sqrtf(_param_mpc_thr_max.get() * _param_mpc_thr_max.get() - _thr_sp(2) * _thr_sp(2));
 		thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
 
 		// Saturate thrust in NE-direction.
-		thr_sp_velocity(0) = thrust_desired_NE(0);
-		thr_sp_velocity(1) = thrust_desired_NE(1);
+		Vector2f thrust_desired_NE;
+		thrust_desired_NE(0) = thr_sp_velocity(0);
+		thrust_desired_NE(1) = thr_sp_velocity(1);
 
 		if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
 			float mag = thrust_desired_NE.length();
@@ -222,8 +207,8 @@ void PositionControl::_velocityController(const float &dt)
 		vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - thr_sp_velocity(1)) * arw_gain;
 
 		// Update integral
-		_vel_int(0) += _param_mpc_xy_vel_i.get() * vel_err_lim(0) * dt;
-		_vel_int(1) += _param_mpc_xy_vel_i.get() * vel_err_lim(1) * dt;
+		_vel_int(0) += vel_gain_i(0) * vel_err_lim(0) * dt;
+		_vel_int(1) += vel_gain_i(1) * vel_err_lim(1) * dt;
 	}
 
 	// No control input from setpoints or corresponding states which are NAN, reset integrator if necessary
@@ -265,6 +250,7 @@ void PositionControl::_addIfNotNan(float &setpoint, const float addition)
 		// Setpoint NAN, take addition
 		setpoint = addition;
 	}
+
 	// Addition is NAN or both are NAN, nothing to do
 }
 

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -129,7 +129,8 @@ class PositionControl : public ModuleParams
 	void _positionController(); /** applies the P-position-controller */
 	void _velocityController(const float &dt); /** applies the PID-velocity-controller */
 	void _addIfNotNan(float &setpoint, const float addition); /** adds to the setpoint but handles NAN cases correctly */
-	void _addIfNotNanVector(matrix::Vector3f &setpoint, const matrix::Vector3f &addition); /** _addIfNotNan for all vector components */
+	void _addIfNotNanVector(matrix::Vector3f &setpoint,
+				const matrix::Vector3f &addition); /** _addIfNotNan for all vector components */
 
 	// States
 	matrix::Vector3f _pos; /**< position */

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -698,6 +698,7 @@ MulticopterPositionControl::run()
 
 			// 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::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;