mc_pos_control: AUTO implemented, fixes

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -319,11 +319,11 @@ private:
 	/**
 	 * Control position.
 	 */
-	bool		control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
+	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
 					 const struct mission_item_triplet_s &_mission_item_triplet);
 
 	float calculate_target_airspeed(float airspeed_demand);
-	void calculate_gndspeed_undershoot(const math::Vector2f &current_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet);
+	void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet);
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -680,13 +680,13 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 }
 
 void
-FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &current_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet)
+FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet)
 {
 
 	if (_global_pos_valid) {
 
 		/* rotate ground speed vector with current attitude */
-		math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
+		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
 		yaw_vector.normalize();
 		float ground_speed_body = yaw_vector * ground_speed;
 
@@ -697,7 +697,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &cu
 			distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon);
 			delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude;
 		} else {
-			distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), mission_item_triplet.current.lat, mission_item_triplet.current.lon);
+			distance = get_distance_to_next_waypoint(current_position(0), current_position(1), mission_item_triplet.current.lat, mission_item_triplet.current.lon);
 			delta_altitude = mission_item_triplet.current.altitude -  _global_pos.alt;
 		}
 
@@ -730,7 +730,7 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 }
 
 bool
-FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
+FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
 		const struct mission_item_triplet_s &mission_item_triplet)
 {
 	bool setpoint = true;
@@ -765,26 +765,26 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		_tecs.set_speed_weight(_parameters.speed_weight);
 
 		/* current waypoint (the one currently heading for) */
-		math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
+		math::Vector<2> next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
 
 		/* current waypoint (the one currently heading for) */
-		math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
+		math::Vector<2> curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
 
 
 		/* previous waypoint */
-		math::Vector2f prev_wp;
+		math::Vector<2> prev_wp;
 
 		if (mission_item_triplet.previous_valid) {
-			prev_wp.setX(mission_item_triplet.previous.lat);
-			prev_wp.setY(mission_item_triplet.previous.lon);
+			prev_wp(0) = mission_item_triplet.previous.lat;
+			prev_wp(1) = mission_item_triplet.previous.lon;
 
 		} else {
 			/*
 			 * No valid previous waypoint, go for the current wp.
 			 * This is automatically handled by the L1 library.
 			 */
-			prev_wp.setX(mission_item_triplet.current.lat);
-			prev_wp.setY(mission_item_triplet.current.lon);
+			prev_wp(0) = mission_item_triplet.current.lat;
+			prev_wp(1) = mission_item_triplet.current.lon;
 
 		}
 
@@ -820,7 +820,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			/* Horizontal landing control */
 			/* switch to heading hold for the last meters, continue heading hold after */
-			float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY());
+			float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 			//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
 			const float heading_hold_distance = 15.0f;
 			if (wp_distance < heading_hold_distance || land_noreturn_horizontal) {
@@ -829,7 +829,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				
 
 				// if (mission_item_triplet.previous_valid) {
-				// 	target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
+				// 	target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), next_wp(0), next_wp(1));
 				// } else {
 
 				if (!land_noreturn_horizontal) //set target_bearing in first occurrence
@@ -870,7 +870,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-			float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length;
+			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
 			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
 			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
 
@@ -989,8 +989,8 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 		// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
 		//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
-		// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(),
-		//       (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid");
+		// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
+		//       (double)next_wp(0), (double)next_wp(1), (mission_item_triplet.previous_valid) ? "valid" : "invalid");
 
 		// XXX at this point we always want no loiter hold if a
 		// mission is active

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -61,7 +61,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -113,7 +113,7 @@ private:
 	int 	_manual_sub;			/**< notification of manual control updates */
 	int		_arming_sub;			/**< arming status of outputs */
 	int		_local_pos_sub;			/**< vehicle local position */
-	int		_global_pos_sp_sub;		/**< vehicle global position setpoint */
+	int		_mission_items_sub;		/**< mission item triplet */
 
 	orb_advert_t	_local_pos_sp_pub;		/**< local position setpoint publication */
 	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
@@ -126,7 +126,7 @@ private:
 	struct actuator_armed_s				_arming;		/**< actuator arming status */
 	struct vehicle_local_position_s		_local_pos;		/**< vehicle local position */
 	struct vehicle_local_position_setpoint_s		_local_pos_sp;		/**< vehicle local position */
-	struct vehicle_global_position_setpoint_s	_global_pos_sp;	/**< vehicle global position setpoint */
+	struct mission_item_triplet_s		_mission_items;	/**< vehicle global position setpoint */
 	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;	/**< vehicle global velocity setpoint */
 
 	struct {
@@ -232,7 +232,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_manual_sub(-1),
 	_arming_sub(-1),
 	_local_pos_sub(-1),
-	_global_pos_sp_sub(-1),
+	_mission_items_sub(-1),
 
 /* publications */
 	_local_pos_sp_pub(-1),
@@ -246,7 +246,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	memset(&_arming, 0, sizeof(_arming));
 	memset(&_local_pos, 0, sizeof(_local_pos));
 	memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
-	memset(&_global_pos_sp, 0, sizeof(_global_pos_sp));
+	memset(&_mission_items, 0, sizeof(_mission_items));
 	memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
 
 	_params.pos_p.zero();
@@ -395,9 +395,9 @@ MulticopterPositionControl::poll_subscriptions()
 	if (updated)
 		orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
 
-	orb_check(_global_pos_sp_sub, &updated);
+	orb_check(_mission_items_sub, &updated);
 	if (updated)
-		orb_copy(ORB_ID(vehicle_global_position_setpoint), _global_pos_sp_sub, &_global_pos_sp);
+		orb_copy(ORB_ID(mission_item_triplet), _mission_items_sub, &_mission_items);
 }
 
 float
@@ -439,7 +439,7 @@ MulticopterPositionControl::task_main()
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
 	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	_global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+	_mission_items_sub = orb_subscribe(ORB_ID(mission_item_triplet));
 
 	parameters_update(true);
 
@@ -449,8 +449,6 @@ MulticopterPositionControl::task_main()
 	/* get an initial update for all sensor and status data */
 	poll_subscriptions();
 
-	bool reset_mission_sp = false;
-	bool global_pos_sp_valid = false;
 	bool reset_man_sp_z = true;
 	bool reset_man_sp_xy = true;
 	bool reset_int_z = true;
@@ -466,9 +464,10 @@ MulticopterPositionControl::task_main()
 	const float alt_ctl_dz = 0.2f;
 	const float pos_ctl_dz = 0.05f;
 
+	hrt_abstime ref_timestamp = 0;
+	int32_t ref_lat = 0.0f;
+	int32_t ref_lon = 0.0f;
 	float ref_alt = 0.0f;
-	hrt_abstime ref_alt_t = 0;
-	hrt_abstime local_ref_timestamp = 0;
 
 	math::Vector<3> sp_move_rate;
 	sp_move_rate.zero();
@@ -533,22 +532,32 @@ MulticopterPositionControl::task_main()
 
 			sp_move_rate.zero();
 
+			if (_local_pos.ref_timestamp != ref_timestamp) {
+				/* initialize local projection with new reference */
+				double lat_home = _local_pos.ref_lat * 1e-7;
+				double lon_home = _local_pos.ref_lon * 1e-7;
+				map_projection_init(lat_home, lon_home);
+				mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
+
+				if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) {
+					/* correct setpoint in manual mode to stay in the same point */
+					float ref_change_x = 0.0f;
+					float ref_change_y = 0.0f;
+					map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y);
+					_pos_sp(0) += ref_change_x;
+					_pos_sp(1) += ref_change_y;
+					_pos_sp(2) += _local_pos.ref_alt - ref_alt;
+				}
+				ref_timestamp = _local_pos.ref_timestamp;
+				ref_lat = _local_pos.ref_lat;
+				ref_lon = _local_pos.ref_lon;
+				ref_alt = _local_pos.ref_alt;
+			}
+
 			if (_control_mode.flag_control_manual_enabled) {
 				/* manual control */
-				/* check for reference point updates and correct setpoint */
-				if (_local_pos.ref_timestamp != ref_alt_t) {
-					if (ref_alt_t != 0) {
-						/* home alt changed, don't follow large ground level changes in manual flight */
-						_pos_sp(2) += _local_pos.ref_alt - ref_alt;
-					}
-
-					ref_alt_t = _local_pos.ref_timestamp;
-					ref_alt = _local_pos.ref_alt;
-					// TODO also correct XY setpoint
-				}
-
-				/* reset setpoints to current position if needed */
 				if (_control_mode.flag_control_altitude_enabled) {
+					/* reset setpoint Z to current altitude if needed */
 					if (reset_man_sp_z) {
 						reset_man_sp_z = false;
 						_pos_sp(2) = _pos(2);
@@ -560,6 +569,7 @@ MulticopterPositionControl::task_main()
 				}
 
 				if (_control_mode.flag_control_position_enabled) {
+					/* reset setpoint XY to current position if needed */
 					if (reset_man_sp_xy) {
 						reset_man_sp_xy = false;
 						_pos_sp(0) = _pos(0);
@@ -594,21 +604,39 @@ MulticopterPositionControl::task_main()
 					_pos_sp = _pos + pos_sp_offs.emult(_params.vel_max);
 				}
 
-				/* copy yaw setpoint to vehicle_local_position_setpoint topic */
-				_local_pos_sp.yaw = _att_sp.yaw_body;
-
 				/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
 				reset_auto_sp_xy = !_control_mode.flag_control_position_enabled;
 				reset_auto_sp_z = !_control_mode.flag_control_altitude_enabled;
 				reset_takeoff_sp = true;
-
-				/* force reprojection of global setpoint after manual mode */
-				reset_mission_sp = true;
 			} else {
-				// TODO AUTO
-				_pos_sp = _pos;
+				/* AUTO */
+				if (_mission_items.current_valid) {
+					struct mission_item_s item = _mission_items.current;
+					map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1));
+
+					// TODO home altitude can be != ref_alt, check home_position topic
+					_pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt);
+
+					/* in case of interrupted mission don't go to waypoint but stop */
+					reset_auto_sp_xy = true;
+					reset_auto_sp_z = true;
+
+				} else {
+					/* no waypoint, loiter, reset position setpoint if needed */
+					if (reset_auto_sp_xy) {
+						reset_auto_sp_xy = false;
+						_pos_sp(0) = _pos(0);
+						_pos_sp(1) = _pos(1);
+					}
+					if (reset_auto_sp_z) {
+						reset_auto_sp_z = false;
+						_pos_sp(2) = _pos(2);
+					}
+				}
 			}
 
+			/* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */
+			_local_pos_sp.yaw = _att_sp.yaw_body;
 			_local_pos_sp.x = _pos_sp(0);
 			_local_pos_sp.y = _pos_sp(1);
 			_local_pos_sp.z = _pos_sp(2);
@@ -874,7 +902,6 @@ MulticopterPositionControl::task_main()
 			reset_man_sp_xy = true;
 			reset_int_z = true;
 			reset_int_xy = true;
-			reset_mission_sp = true;
 			reset_auto_sp_xy = true;
 			reset_auto_sp_z = true;
 		}