Rover position controller: Modernize implementation

This commit moves the steering output from yaw to the roll channel to better reflect the lateral control input / reaction mapping. It also removes old unused parameters and modernizes the mainloop to remove unnecessary polling.

ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix

@@ -13,25 +13,28 @@ Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (ro
 See the README for more information on the scaler format.
 
 
-Output 0
+Output 1 - Empty
 -----------------------------------------
 Z:
 
-Steering mixer using roll on output 1
+Output 2 - Steering mixer using yaw
 ------------------------------------------
 M: 1
+O: 10000 10000 0 -10000 10000 5000
 S: 0 2  10000   10000     0 -10000  10000
 
 
-Output 2
+Output 3 - Left row of wheels using yaw and throttle (1s rise time)
 ------------------------------------------
 M: 2
+O: 10000 10000 0 -10000 10000 10000
 S: 0 2   -500   -500      0      0  10000
 S: 0 3  10000  10000      0 -10000  10000
 
 
-Output 3
+Output 4 - Right row of wheels using yaw and throttle (1s rise time)
 ------------------------------------------
 M: 2
+O: 10000 10000 0 -10000 10000 10000
 S: 0 2    500    500      0      0  10000
 S: 0 3  10000  10000      0 -10000  10000

ROMFS/px4fmu_common/mixers/rover_generic.main.mix

@@ -13,23 +13,25 @@ Inputs to the mixer come from channel group 0 (vehicle attitude), channels 2 (ya
 See the README for more information on the scaler format.
 
 
-Output 0
+Output 1: Empty
 ---------------------------------------
 Z:
 
-Steering mixer using roll on output 1
+Output 2: Steering mixer using yaw, with 0.5s rise time
 ---------------------------------------
 M: 1
+O: 10000 10000 0 -10000 10000 5000
 S: 0 2   10000   10000      0 -10000  10000
 
 
-Output 2
+Output 3: Empty
 ---------------------------------------
 This mixer is empty.
 Z:
 
 
-Output 3
+Output 4: Throttle with 2s rise time
 ---------------------------------------
 M: 1
+O: 10000 10000 0 -10000 10000 20000
 S: 0 3  10000  10000      0 -10000  10000

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -104,17 +104,6 @@ RoverPositionControl::vehicle_control_mode_poll()
 	}
 }
 
-void
-RoverPositionControl::manual_control_setpoint_poll()
-{
-	bool manual_updated;
-	orb_check(_manual_control_setpoint_sub, &manual_updated);
-
-	if (manual_updated) {
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
-	}
-}
-
 void
 RoverPositionControl::position_setpoint_triplet_poll()
 {
@@ -357,7 +346,6 @@ RoverPositionControl::run()
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 
 	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 
 	/* rate limit control mode updates to 5Hz */
 	orb_set_interval(_control_mode_sub, 200);
@@ -369,19 +357,15 @@ RoverPositionControl::run()
 	parameters_update(true);
 
 	/* wakeup source(s) */
-	px4_pollfd_struct_t fds[5];
+	px4_pollfd_struct_t fds[3];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
 	fds[0].events = POLLIN;
-	fds[1].fd = _manual_control_setpoint_sub;
+	fds[1].fd = _local_pos_sub;  // Added local position as source of position
 	fds[1].events = POLLIN;
-	fds[2].fd = _sensor_combined_sub;
+	fds[2].fd = _vehicle_attitude_sub; // Poll attitude
 	fds[2].events = POLLIN;
-	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()) {
 
@@ -397,7 +381,6 @@ RoverPositionControl::run()
 		/* check vehicle control mode for changes to publication state */
 		vehicle_control_mode_poll();
 		attitude_setpoint_poll();
-		//manual_control_setpoint_poll();
 
 		_vehicle_acceleration_sub.update();
 
@@ -406,8 +389,8 @@ RoverPositionControl::run()
 
 		bool manual_mode = _control_mode.flag_control_manual_enabled;
 
-		/* only run controller if position changed */
+		// Respond to a position update
-		if (fds[0].revents & POLLIN || fds[4].revents & POLLIN) {
+		if (fds[0].revents & POLLIN || fds[1].revents & POLLIN) {
 			perf_begin(_loop_perf);
 
 			/* load local copies */
@@ -476,7 +459,8 @@ RoverPositionControl::run()
 			perf_end(_loop_perf);
 		}
 
-		if (fds[3].revents & POLLIN) {
+		// Respond to an attitude update and run the attitude controller if enabled
+		if (fds[2].revents & POLLIN) {
 
 			vehicle_attitude_poll();
 
@@ -490,37 +474,28 @@ RoverPositionControl::run()
 
 		}
 
-		if (fds[1].revents & POLLIN) {
+		// Manual pass-through if no other control mode is enabled
-
+		if (manual_mode) {
-			// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
+			/* manual/direct control */
-			// returning immediately and this loop will eat up resources.
 			orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
 
-			if (manual_mode) {
+			_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y;
-				/* manual/direct control */
+			_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x;
-				//PX4_INFO("Manual mode!");
+			// Set heading from the manual roll input channel
-				_act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual_control_setpoint.y;
+			_act_controls.control[actuator_controls_s::INDEX_YAW] =
-				_act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual_control_setpoint.x;
+				_manual_control_setpoint.y; // Nominally yaw: _manual_control_setpoint.r;
-				_act_controls.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.r; //TODO: Readd yaw scale param
+			// Set throttle from the manual throttle channel
-				_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
+			_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
-			}
 		}
 
-		if (fds[2].revents & POLLIN) {
+		// publish the controller output
-
+		if (_control_mode.flag_control_velocity_enabled ||
-			orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+		    _control_mode.flag_control_attitude_enabled ||
-
+		    _control_mode.flag_control_position_enabled ||
-			//orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att);
+		    manual_mode) {
+			// timestamp and publish controls
 			_act_controls.timestamp = hrt_absolute_time();
-
+			_actuator_controls_pub.publish(_act_controls);
-			/* 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);
-			}
 		}
 
 	}
@@ -531,7 +506,6 @@ RoverPositionControl::run()
 	orb_unsubscribe(_manual_control_setpoint_sub);
 	orb_unsubscribe(_pos_sp_triplet_sub);
 	orb_unsubscribe(_vehicle_attitude_sub);
-	orb_unsubscribe(_sensor_combined_sub);
 
 	warnx("exiting.\n");
 }

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -62,7 +62,6 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -109,7 +108,6 @@ private:
 	int		_pos_sp_triplet_sub{-1};
 	int		_att_sp_sub{-1};
 	int		_vehicle_attitude_sub{-1};
-	int		_sensor_combined_sub{-1};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
@@ -121,7 +119,6 @@ private:
 	vehicle_local_position_s		_local_pos{};			/**< global vehicle position */
 	actuator_controls_s				_act_controls{};		/**< direct control of actuators */
 	vehicle_attitude_s				_vehicle_att{};
-	sensor_combined_s				_sensor_combined{};
 
 	uORB::SubscriptionData<vehicle_acceleration_s>		_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 
@@ -181,7 +178,6 @@ private:
 	 */
 	void parameters_update(bool force = false);
 
-	void		manual_control_setpoint_poll();
 	void		position_setpoint_triplet_poll();
 	void		attitude_setpoint_poll();
 	void		vehicle_control_mode_poll();

src/modules/rover_pos_control/rover_pos_control_params.c

@@ -49,36 +49,50 @@
  */
 
 /**
- * L1 distance
+ * Distance from front axle to rear axle
- *
- * This is the waypoint radius
  *
+ * A value of 0.31 is typical for 1/10 RC cars.
  *
  * @unit m
  * @min 0.0
- * @max 100.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f);
+
+/**
+ * L1 distance
+ *
+ * This is the distance at which the next waypoint is activated. This should be set
+ * to about 2-4x of GND_WHEEL_BASE and not smaller than one meter (due to GPS accuracy).
+ *
+ *
+ * @unit m
+ * @min 1.0
+ * @max 50.0
  * @decimal 1
  * @increment 0.1
  * @group Rover Position Control
  */
-PARAM_DEFINE_FLOAT(GND_L1_DIST, 5.0f);
+PARAM_DEFINE_FLOAT(GND_L1_DIST, 1.0f);
 
 /**
  * L1 period
  *
  * This is the L1 distance and defines the tracking
  * point ahead of the rover it's following.
- * Using values around 2-5 for a traxxas stampede. Shorten
+ * Use values around 2-5m for a 0.3m wheel base. Tuning instructions: Shorten
  * slowly during tuning until response is sharp without oscillation.
  *
  * @unit m
- * @min 0.0
+ * @min 0.5
  * @max 50.0
  * @decimal 1
  * @increment 0.5
  * @group Rover Position Control
  */
-PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 10.0f);
+PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 5.0f);
 
 /**
  * L1 damping
@@ -246,18 +260,6 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
  */
 PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
 
-/**
- * Distance from front axle to rear axle
- *
- *
- * @unit m
- * @min 0.0
- * @decimal 3
- * @increment 0.01
- * @group Rover Position Control
- */
-PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 2.0f);
-
 /**
  * Maximum turn angle for Ackerman steering.
  * At a control output of 0, the steering wheels are at 0 radians.