Merge remote-tracking branch 'upstream/beta' into beta

src/lib/geo/geo.c

@@ -436,22 +436,22 @@ __EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, flo
 __EXPORT float _wrap_pi(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing) || bearing == 0) {
+	if (!isfinite(bearing)) {
 		return bearing;
 	}
 
 	int c = 0;
-
+	while (bearing > M_PI_F) {
-	while (bearing > M_PI_F && c < 30) {
 		bearing -= M_TWOPI_F;
-		c++;
+		if (c++ > 3)
+			return NAN;
 	}
 
 	c = 0;
-
+	while (bearing <= -M_PI_F) {
-	while (bearing <=  -M_PI_F && c < 30) {
 		bearing += M_TWOPI_F;
-		c++;
+		if (c++ > 3)
+			return NAN;
 	}
 
 	return bearing;
@@ -464,12 +464,18 @@ __EXPORT float _wrap_2pi(float bearing)
 		return bearing;
 	}
 
-	while (bearing >= M_TWOPI_F) {
+	int c = 0;
-		bearing = bearing - M_TWOPI_F;
+	while (bearing > M_TWOPI_F) {
+		bearing -= M_TWOPI_F;
+		if (c++ > 3)
+			return NAN;
 	}
 
-	while (bearing <  0.0f) {
+	c = 0;
-		bearing = bearing + M_TWOPI_F;
+	while (bearing <= 0.0f) {
+		bearing += M_TWOPI_F;
+		if (c++ > 3)
+			return NAN;
 	}
 
 	return bearing;
@@ -482,12 +488,18 @@ __EXPORT float _wrap_180(float bearing)
 		return bearing;
 	}
 
+	int c = 0;
 	while (bearing > 180.0f) {
-		bearing = bearing - 360.0f;
+		bearing -= 360.0f;
+		if (c++ > 3)
+			return NAN;
 	}
 
+	c = 0;
 	while (bearing <= -180.0f) {
-		bearing = bearing + 360.0f;
+		bearing += 360.0f;
+		if (c++ > 3)
+			return NAN;
 	}
 
 	return bearing;
@@ -500,12 +512,18 @@ __EXPORT float _wrap_360(float bearing)
 		return bearing;
 	}
 
-	while (bearing >= 360.0f) {
+	int c = 0;
-		bearing = bearing - 360.0f;
+	while (bearing > 360.0f) {
+		bearing -= 360.0f;
+		if (c++ > 3)
+			return NAN;
 	}
 
-	while (bearing <  0.0f) {
+	c = 0;
-		bearing = bearing + 360.0f;
+	while (bearing <= 0.0f) {
+		bearing += 360.0f;
+		if (c++ > 3)
+			return NAN;
 	}
 
 	return bearing;

src/modules/commander/commander.cpp

@@ -369,6 +369,7 @@ int arm()
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
 		return 0;
+
 	} else {
 		return 1;
 	}
@@ -381,6 +382,7 @@ int disarm()
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
 		return 0;
+
 	} else {
 		return 1;
 	}

src/modules/commander/state_machine_helper.cpp

@@ -382,6 +382,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 			break;
 
 		case FAILSAFE_STATE_RTL:
+
 			/* global position and home position required for RTL */
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
 				status->set_nav_state = NAV_STATE_RTL;
@@ -392,6 +393,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 			break;
 
 		case FAILSAFE_STATE_LAND:
+
 			/* at least relative altitude estimate required for landing */
 			if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
 				status->set_nav_state = NAV_STATE_LAND;

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -489,14 +489,18 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//}
 		} else {
 			float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
+
 			if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
 				/* move yaw setpoint */
 				yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
+
 				if (_manual_control_sp.yaw > 0.0f) {
 					yaw_sp_move_rate -= YAW_DEADZONE;
+
 				} else {
 					yaw_sp_move_rate += YAW_DEADZONE;
 				}
+
 				yaw_sp_move_rate *= _params.rc_scale_yaw;
 				_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 				_v_att_sp.R_valid = false;

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -479,9 +479,11 @@ MulticopterPositionControl::select_alt(bool global)
 {
 	if (global != _use_global_alt) {
 		_use_global_alt = global;
+
 		if (global) {
 			/* switch from barometric to global altitude */
 			_alt_sp += _global_pos.alt - _global_pos.baro_alt;
+
 		} else {
 			/* switch from global to barometric altitude */
 			_alt_sp += _global_pos.baro_alt - _global_pos.alt;
@@ -589,6 +591,7 @@ MulticopterPositionControl::task_main()
 			if (_control_mode.flag_control_manual_enabled) {
 				/* select altitude source and update setpoint */
 				select_alt(_global_pos.global_valid);
+
 				if (!_use_global_alt) {
 					alt = _global_pos.baro_alt;
 				}
@@ -672,7 +675,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 +691,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 +716,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 +750,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,40 +844,54 @@ 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) {
+						/* limit max tilt */
-							/* crop horizontal component */
+						if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
-							float k = tanf(tilt_max) / tanf(tilt);
+							/* 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 */
 						float att_comp;
 
 						if (_att.R[2][2] > TILT_COS_MAX) {
 							att_comp = 1.0f / _att.R[2][2];
+
 						} else if (_att.R[2][2] > 0.0f) {
 							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
 							saturation_z = true;
+
 						} else {
 							att_comp = 1.0f;
 							saturation_z = true;

src/modules/navigator/navigator_main.cpp

@@ -1575,6 +1575,7 @@ Navigator::on_mission_item_reached()
 		if (_rtl_state == RTL_STATE_DESCEND) {
 			/* hovering above home position, land if needed or loiter */
 			mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
+
 			if (_mission_item.autocontinue) {
 				dispatch(EVENT_LAND_REQUESTED);
 

src/modules/navigator/navigator_mission.cpp

@@ -128,31 +128,37 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool
 	if (current_onboard_mission_available()) {
 
 		const ssize_t len = sizeof(struct mission_item_s);
+
 		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return ERROR;
 		}
+
 		_current_mission_type = MISSION_TYPE_ONBOARD;
 		*onboard = true;
 		*index = _current_onboard_mission_index;
 
 		/* otherwise fallback to offboard */
+
 	} else if (current_offboard_mission_available()) {
 
 		dm_item_t dm_current;
 
 		if (_offboard_dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 
 		const ssize_t len = sizeof(struct mission_item_s);
+
 		if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			_current_mission_type = MISSION_TYPE_NONE;
 			return ERROR;
 		}
+
 		_current_mission_type = MISSION_TYPE_OFFBOARD;
 		*onboard = false;
 		*index = _current_offboard_mission_index;
@@ -173,23 +179,27 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
 	if (next_onboard_mission_available()) {
 
 		const ssize_t len = sizeof(struct mission_item_s);
+
 		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return ERROR;
 		}
 
 		/* otherwise fallback to offboard */
+
 	} else if (next_offboard_mission_available()) {
 
 		dm_item_t dm_current;
 
 		if (_offboard_dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 
 		const ssize_t len = sizeof(struct mission_item_s);
+
 		if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return ERROR;
@@ -247,9 +257,11 @@ Mission::move_to_next()
 	case MISSION_TYPE_ONBOARD:
 		_current_onboard_mission_index++;
 		break;
+
 	case MISSION_TYPE_OFFBOARD:
 		_current_offboard_mission_index++;
 		break;
+
 	case MISSION_TYPE_NONE:
 	default:
 		break;

src/systemcmds/param/param.c

@@ -320,7 +320,7 @@ do_set(const char* name, const char* val)
 			char* end;
 			f = strtod(val,&end);
 			param_set(param, &f);
-			printf(" -> new: %f\n", f);
+			printf(" -> new: %4.4f\n", (double)f);
 
 		}
 

src/systemcmds/tests/test_mixer.cpp

@@ -65,6 +65,13 @@ static int	mixer_callback(uintptr_t handle,
 
 const unsigned output_max = 8;
 static float actuator_controls[output_max];
+
+int test_mixer(int argc, char *argv[])
+{
+	/*
+	 * PWM limit structure
+	 */
+	pwm_limit_t pwm_limit;
 	static bool should_arm = false;
 	uint16_t r_page_servo_disarmed[output_max];
 	uint16_t r_page_servo_control_min[output_max];
@@ -72,13 +79,6 @@ uint16_t r_page_servo_control_max[output_max];
 	uint16_t r_page_servos[output_max];
 	uint16_t servo_predicted[output_max];
 
-/*
- * PWM limit structure
- */
-pwm_limit_t pwm_limit;
-
-int test_mixer(int argc, char *argv[])
-{
 	warnx("testing mixer");
 
 	char *filename = "/etc/mixers/IO_pass.mix";