Improved Hold behaviour

Added local position as a valid source of position

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -220,51 +220,56 @@ RoverPositionControl::control_position(const matrix::Vector2f &current_position,
 			}
 		}
 
-		float dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
+		float dist_target = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
-				pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
+				    (double)curr_wp(0), (double)curr_wp(1)); // pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
 
-		bool should_idle = true;
+		//PX4_INFO("Setpoint type %d", (int) pos_sp_triplet.current.type );
+		//PX4_INFO(" State machine state %d", (int) _pos_ctrl_state);
+		//PX4_INFO(" Setpoint Lat %f, Lon %f", (double) curr_wp(0), (double)curr_wp(1));
+		//PX4_INFO(" Distance to target %f", (double) dist_target);
 
-		if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+		switch (_pos_ctrl_state) {
-			// Because of noise in measurements, if the rover was always trying to reach an exact point, it would
+		case GOTO_WAYPOINT: {
-			// move around when it should be parked. So, I try to get the rover within loiter_radius/2, but then
+				if (dist_target < _param_nav_loiter_rad.get()) {
-			// once I reach that point, I don't move until I'm outside of loiter_radius.
+					_pos_ctrl_state = STOPPING;  // We are closer than loiter radius to waypoint, stop.
-			// TODO: Find out if there's a better measurement to use than loiter_radius.
-			if (dist > pos_sp_triplet.current.loiter_radius) {
-				_waypoint_reached = false;
 
-			} else if (dist <= pos_sp_triplet.current.loiter_radius / 2) {
+				} else {
-				_waypoint_reached = true;
+					_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+
+					_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
+
+					float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
+					float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());
+					float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
+								       _gnd_control.nav_lateral_acceleration_demand());
+					control_effort = math::constrain(control_effort, -1.0f, 1.0f);
+					_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
+				}
 			}
+			break;
 
-			should_idle = _waypoint_reached;
+		case STOPPING: {
+				_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
+				_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
+				// Note _prev_wp is different to the local prev_wp which is related to a mission waypoint.
+				float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1),
+							       (double)curr_wp(0), (double)curr_wp(1));
 
-		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
+				if (dist_between_waypoints > 0) {
-			   pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF ||
+					_pos_ctrl_state = GOTO_WAYPOINT; // A new waypoint has arrived go to it
-			   pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+				}
-			should_idle = false;
+
+				//PX4_INFO(" Distance between prev and curr waypoints %f", (double)dist_between_waypoints);
+			}
+			break;
+
+		default:
+			PX4_ERR("Unknown Rover State");
+			_pos_ctrl_state = STOPPING;
+			break;
 		}
 
-
+		_prev_wp = curr_wp;
-		if (should_idle) {
-			_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
-			_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
-
-		} else {
-
-			/* waypoint is a plain navigation waypoint or the takeoff waypoint, does not matter */
-			_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
-
-			_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
-
-			float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
-			float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());
-			float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
-						       _gnd_control.nav_lateral_acceleration_demand());
-			control_effort = math::constrain(control_effort, -1.0f, 1.0f);
-			_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
-
-		}
 
 	} else {
 		_control_mode_current = UGV_POSCTRL_MODE_OTHER;
@@ -333,17 +338,11 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi
 	float control_effort = euler_sp(2) / _param_max_turn_angle.get();
 	control_effort = math::constrain(control_effort, -1.0f, 1.0f);
 
-	const float control_throttle = math::constrain(att_sp.thrust_body[0], -1.0f, 1.0f);
+	_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
 
-	if (control_throttle >= 0.0f) {
+	const float control_throttle = att_sp.thrust_body[0];
-		_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
 
-	} else {
+	_act_controls.control[actuator_controls_s::INDEX_THROTTLE] =  math::constrain(control_throttle, 0.0f, 1.0f);
-		// reverse steering, if driving backwards
-		_act_controls.control[actuator_controls_s::INDEX_YAW] = -control_effort;
-	}
-
-	_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = control_throttle;
 
 }
 
@@ -370,7 +369,7 @@ RoverPositionControl::run()
 	parameters_update(true);
 
 	/* wakeup source(s) */
-	px4_pollfd_struct_t fds[4];
+	px4_pollfd_struct_t fds[5];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
@@ -379,8 +378,10 @@ RoverPositionControl::run()
 	fds[1].events = POLLIN;
 	fds[2].fd = _sensor_combined_sub;
 	fds[2].events = POLLIN;
-	fds[3].fd = _vehicle_attitude_sub;
+	fds[3].fd = _vehicle_attitude_sub; // Poll attitude
 	fds[3].events = POLLIN;
+	fds[4].fd = _local_pos_sub;  // Added local position as source of position
+	fds[4].events = POLLIN;
 
 	while (!should_exit()) {
 
@@ -406,7 +407,7 @@ RoverPositionControl::run()
 		bool manual_mode = _control_mode.flag_control_manual_enabled;
 
 		/* only run controller if position changed */
-		if (fds[0].revents & POLLIN) {
+		if (fds[0].revents & POLLIN || fds[4].revents & POLLIN) {
 			perf_begin(_loop_perf);
 
 			/* load local copies */
@@ -462,6 +463,7 @@ RoverPositionControl::run()
 
 					_pos_ctrl_status_pub.publish(pos_ctrl_status);
 
+
 				}
 
 			} else if (!manual_mode && _control_mode.flag_control_velocity_enabled) {
@@ -514,6 +516,7 @@ RoverPositionControl::run()
 			/* Only publish if any of the proper modes are enabled */
 			if (_control_mode.flag_control_velocity_enabled ||
 			    _control_mode.flag_control_attitude_enabled ||
+			    _control_mode.flag_control_position_enabled ||
 			    manual_mode) {
 				/* publish the actuator controls */
 				_actuator_controls_pub.publish(_act_controls);

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -99,10 +99,11 @@ public:
 	void run() override;
 
 private:
-	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};
-	uORB::Publication<actuator_controls_s>		_actuator_controls_pub{ORB_ID(actuator_controls_0)};
 
-	int		_control_mode_sub{-1};			/**< control mode subscription */
+	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};  /**< navigation capabilities publication */
+	uORB::Publication<actuator_controls_s>		_actuator_controls_pub{ORB_ID(actuator_controls_0)};  /**< actuator controls publication */
+
+	int		_control_mode_sub{-1};		/**< control mode subscription */
 	int		_global_pos_sub{-1};
 	int		_local_pos_sub{-1};
 	int		_manual_control_sub{-1};		/**< notification of manual control updates */
@@ -113,15 +114,15 @@ private:
 
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};
 
-	manual_control_setpoint_s		_manual{};			/**< r/c channel data */
+	manual_control_setpoint_s		_manual{};			    /**< r/c channel data */
 	position_setpoint_triplet_s		_pos_sp_triplet{};		/**< triplet of mission items */
 	vehicle_attitude_setpoint_s		_att_sp{};			/**< attitude setpoint > */
 	vehicle_control_mode_s			_control_mode{};		/**< control mode */
 	vehicle_global_position_s		_global_pos{};			/**< global vehicle position */
 	vehicle_local_position_s		_local_pos{};			/**< global vehicle position */
-	actuator_controls_s			_act_controls{};		/**< direct control of actuators */
+	actuator_controls_s				_act_controls{};		/**< direct control of actuators */
-	vehicle_attitude_s			_vehicle_att{};
+	vehicle_attitude_s				_vehicle_att{};
-	sensor_combined_s			_sensor_combined{};
+	sensor_combined_s				_sensor_combined{};
 
 	SubscriptionData<vehicle_acceleration_s>		_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 
@@ -138,13 +139,20 @@ private:
 
 	ECL_L1_Pos_Controller				_gnd_control;
 
-	bool _waypoint_reached{false};
-
 	enum UGV_POSCTRL_MODE {
 		UGV_POSCTRL_MODE_AUTO,
 		UGV_POSCTRL_MODE_OTHER
 	} _control_mode_current{UGV_POSCTRL_MODE_OTHER};			///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
 
+
+	enum POS_CTRLSTATES {
+		GOTO_WAYPOINT,
+		STOPPING
+	} _pos_ctrl_state {STOPPING};			/// Position control state machine
+
+	/* previous waypoint */
+	matrix::Vector2f _prev_wp{0.0f, 0.0f};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::GND_L1_PERIOD>) _param_l1_period,
 		(ParamFloat<px4::params::GND_L1_DAMPING>) _param_l1_damping,
@@ -165,7 +173,8 @@ private:
 		(ParamFloat<px4::params::GND_THR_CRUISE>) _param_throttle_cruise,
 
 		(ParamFloat<px4::params::GND_WHEEL_BASE>) _param_wheel_base,
-		(ParamFloat<px4::params::GND_MAX_ANG>) _param_max_turn_angle
+		(ParamFloat<px4::params::GND_MAX_ANG>) _param_max_turn_angle,
+		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad	/**< loiter radius for Rover */
 	)
 
 	/**