Merge branch 'beta' into acro2

1 files changed, 37 insertions, 27 deletions

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 e9b92119d..4fb9bd663 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -645,7 +645,7 @@ MulticopterPositionControl::task_main()
 
 				if (pos_sp_offs_norm > 1.0f) {
 					pos_sp_offs /= pos_sp_offs_norm;
-					add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
+					add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
 					_alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2);
 				}
 
@@ -672,7 +672,7 @@ MulticopterPositionControl::task_main()
 					_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
 				}
 
-			} else if (_control_mode.flag_control_auto_enabled) {
+			} else {
 				/* always use AMSL altitude for AUTO */
 				select_alt(true);
 
@@ -688,22 +688,24 @@ MulticopterPositionControl::task_main()
 					_reset_lat_lon_sp = true;
 					_reset_alt_sp = true;
 
+					/* update position setpoint */
 					_lat_sp = _pos_sp_triplet.current.lat;
 					_lon_sp = _pos_sp_triplet.current.lon;
 					_alt_sp = _pos_sp_triplet.current.alt;
 
+					/* update yaw setpoint if needed */
+					if (isfinite(_pos_sp_triplet.current.yaw)) {
+						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+					}
+
 				} else {
 					/* no waypoint, loiter, reset position setpoint if needed */
 					reset_lat_lon_sp();
 					reset_alt_sp();
 				}
-			} else {
-				/* no control, reset setpoint */
-				reset_lat_lon_sp();
-				reset_alt_sp();
 			}
 
-			if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
+			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
 				/* idle state, don't run controller and set zero thrust */
 				R.identity();
 				memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
@@ -711,7 +713,7 @@ MulticopterPositionControl::task_main()
 
 				_att_sp.roll_body = 0.0f;
 				_att_sp.pitch_body = 0.0f;
-				_att_sp.yaw_body = 0.0f;
+				_att_sp.yaw_body = _att.yaw;
 				_att_sp.thrust = 0.0f;
 
 				_att_sp.timestamp = hrt_absolute_time();
@@ -745,7 +747,7 @@ MulticopterPositionControl::task_main()
 				}
 
 				/* use constant descend rate when landing, ignore altitude setpoint */
-				if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 					_vel_sp(2) = _params.land_speed;
 				}
 
@@ -839,30 +841,38 @@ MulticopterPositionControl::task_main()
 						thr_min = 0.0f;
 					}
 
+					float tilt_max = _params.tilt_max;
+
+					/* adjust limits for landing mode */
+					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
+							_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+						/* limit max tilt and min lift when landing */
+						tilt_max = _params.land_tilt_max;
+						if (thr_min < 0.0f)
+							thr_min = 0.0f;
+					}
+
+					/* limit min lift */
 					if (-thrust_sp(2) < thr_min) {
 						thrust_sp(2) = -thr_min;
 						saturation_z = true;
 					}
 
-					/* limit max tilt */
-					float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
-					float tilt_max = _params.tilt_max;
-					if (!_control_mode.flag_control_manual_enabled) {
-						if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
-							/* limit max tilt and min lift when landing */
-							tilt_max = _params.land_tilt_max;
-							if (thr_min < 0.0f)
-								thr_min = 0.0f;
-						}
-					}
-
 					if (_control_mode.flag_control_velocity_enabled) {
-						if (tilt > tilt_max && thr_min >= 0.0f) {
-							/* crop horizontal component */
-							float k = tanf(tilt_max) / tanf(tilt);
-							thrust_sp(0) *= k;
-							thrust_sp(1) *= k;
-							saturation_xy = true;
+						/* limit max tilt */
+						if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
+							/* absolute horizontal thrust */
+							float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+							if (thrust_sp_xy_len > 0.01f) {
+								/* max horizontal thrust for given vertical thrust*/
+								float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
+								if (thrust_sp_xy_len > thrust_xy_max) {
+									float k = thrust_xy_max / thrust_sp_xy_len;
+									thrust_sp(0) *= k;
+									thrust_sp(1) *= k;
+									saturation_xy = true;
+								}
+							}
 						}
 					} else {
 						/* thrust compensation for altitude only control mode */