Changed ground position control to steer based on demand acceleration (…

…PX4#11956)
AmeliaEScott · Jul 11, 2019 · 0f69daa · 0f69daa
1 parent 27ef189
commit 0f69daa
Show file tree

Hide file tree

Showing 4 changed files with 53 additions and 0 deletions.
diff --git a/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp b/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp
@@ -311,6 +311,9 @@ GroundRoverAttitudeControl::task_main()
 
 						_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_status.scale;
 					}
+
+					_actuators.control[actuator_controls_s::INDEX_YAW] = _att_sp.yaw_body;
+					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _att_sp.thrust_body[0];
 				}
 
 			} else {

diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
@@ -87,6 +87,9 @@ GroundRoverPositionControl::GroundRoverPositionControl() :
 	_parameter_handles.throttle_max = param_find("GND_THR_MAX");
 	_parameter_handles.throttle_cruise = param_find("GND_THR_CRUISE");
 
+	_parameter_handles.wheel_base = param_find("GND_WHEEL_BASE");
+	_parameter_handles.max_turn_angle = param_find("GND_MAX_ANG");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -138,6 +141,9 @@ GroundRoverPositionControl::parameters_update()
 	param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
 	param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
 
+	param_get(_parameter_handles.wheel_base, &(_parameters.wheel_base));
+	param_get(_parameter_handles.max_turn_angle, &(_parameters.max_turn_angle));
+
 	_gnd_control.set_l1_damping(_parameters.l1_damping);
 	_gnd_control.set_l1_period(_parameters.l1_period);
 	_gnd_control.set_l1_roll_limit(math::radians(0.0f));
@@ -274,6 +280,20 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_pos
 			_att_sp.fw_control_yaw = true;
 			_att_sp.thrust_body[0] = mission_throttle;
 
+			_att_sp.thrust_body[0] = _parameters.throttle_cruise;
+			_att_sp.yaw_body = _parameters.gndspeed_trim;
+
+			float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
+			desired_r = math::constrain(desired_r, 0.0000001f, 9999999999.0f);
+			float desired_theta = (3.14159f / 2.0f) - (float) std::atan2(desired_r, _parameters.wheel_base);
+			float control_effort = (desired_theta / _parameters.max_turn_angle) * math::sign(
+						       _gnd_control.nav_lateral_acceleration_demand());
+			control_effort = math::constrain(control_effort, -1.0f, 1.0f);
+			_att_sp.yaw_body = control_effort;
+
+			//PX4_INFO("Acc: %7.4f | R: %8.1f | Theta: %6.3f | Ctrl: %7.4f", (double) _gnd_control.nav_lateral_acceleration_demand(),
+			//	 (double) desired_r, (double) desired_theta, (double) control_effort);
+
 		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 
 			/* waypoint is a loiter waypoint so we want to stop*/

diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp
@@ -151,6 +151,9 @@ class GroundRoverPositionControl
 		float throttle_cruise;
 		float throttle_slew_max;
 
+		float wheel_base;
+		float max_turn_angle;
+
 	} _parameters{};			/**< local copies of interesting parameters */
 
 	struct {
@@ -174,6 +177,8 @@ class GroundRoverPositionControl
 		param_t throttle_cruise;
 		param_t throttle_slew_max;
 
+		param_t wheel_base;
+		param_t max_turn_angle;
 	} _parameter_handles{};		/**< handles for interesting parameters */
 
 

diff --git a/src/modules/gnd_pos_control/gnd_pos_control_params.c b/src/modules/gnd_pos_control/gnd_pos_control_params.c
@@ -260,3 +260,28 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
  * @group FW TECS
  */
 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 FW TECS
+ */
+PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 2.0f);
+
+/**
+ * Maximum turn angle for Ackerman steering
+ *
+ *
+ * @unit rad
+ * @min 0.0
+ * @max 3.14159
+ * @decimal 3
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);