update angular accelerations

Rebase fixes

ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover

@@ -22,7 +22,6 @@ param set-default NAV_ACC_RAD 0.5
 param set-default NAV_LOITER_RAD 2
 
 param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 2.0
 
 param set-default CA_AIRFRAME 5
 

ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover

@@ -22,7 +22,6 @@ param set-default NAV_ACC_RAD 0.5
 param set-default NAV_LOITER_RAD 2
 
 param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 2.0
 
 param set-default CA_AIRFRAME 6
 

ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1

@@ -29,7 +29,6 @@ param set-default NAV_ACC_RAD 0.5
 param set-default NAV_LOITER_RAD 2
 
 param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 3.0
 
 param set-default CA_AIRFRAME 5
 

ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat

@@ -13,16 +13,20 @@ param set-default GND_SPEED_I 8
 param set-default GND_SPEED_IMAX 0.125
 param set-default GND_SPEED_P 2
 param set-default GND_SPEED_THR_SC 1
-param set-default GND_SPEED_TRIM 1
+param set-default GND_SPEED_TRIM 3
 param set-default GND_THR_CRUISE 0.85
 param set-default GND_THR_MAX 1
 param set-default GND_THR_MIN 0
+param set-default GND_RATE_P 0.08
+param set-default GND_RATE_D 0.02
+param set-default GND_RATE_FF 0.0
+param set-default GND_RATE_I 0.0
+param set-default GND_RATE_MAX 0.5
 
-param set-default NAV_ACC_RAD 0.5
+param set-default NAV_ACC_RAD 5
 param set-default NAV_LOITER_RAD 2
 
 param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 2.0
 
 param set-default CA_AIRFRAME 9
 

ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx

@@ -34,7 +34,6 @@ param set-default GND_THR_MAX 0.5
 
 # Differential drive acts like ackermann steering with a maximum turn angle of 60 degrees, or pi/3 radians
 param set-default GND_MAX_ANG 1.042
-param set-default GND_WHEEL_BASE 0.17
 
 # TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
 # to support negative throttle.

src/modules/rover_pos_control/CMakeLists.txt

@@ -30,6 +30,7 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
 px4_add_module(
 	MODULE modules__rover_pos_control
 	MAIN rover_pos_control
@@ -37,6 +38,7 @@ px4_add_module(
 		RoverPositionControl.cpp
 		RoverPositionControl.hpp
 	DEPENDS
+		RateControl
 		l1
 		pid
 	)

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -99,6 +99,10 @@ void RoverPositionControl::parameters_update(bool force)
 				   _param_speed_d.get(),
 				   _param_speed_imax.get(),
 				   _param_gndspeed_max.get());
+		_rate_control.setGains(matrix::Vector3f(0.0, 0.0, _param_rate_p.get()), matrix::Vector3f(0.0, 0.0, _param_rate_i.get()),
+				       matrix::Vector3f(0.0, 0.0, _param_rate_d.get()));
+		_rate_control.setFeedForwardGain(matrix::Vector3f(0.0, 0.0, _param_rate_ff.get()));
+		_rate_control.setIntegratorLimit(matrix::Vector3f(0.0, 0.0, _param_rate_imax.get()));
 	}
 }
 
@@ -148,6 +152,17 @@ RoverPositionControl::manual_control_setpoint_poll()
 
 					_attitude_sp_pub.publish(_att_sp);
 
+				} else if (_control_mode.flag_control_rates_enabled) {
+					// STABILIZED mode generate the attitude setpoint from manual user inputs
+					_rates_sp.roll = 0.0;
+					_rates_sp.pitch = 0.0;
+					_rates_sp.yaw = _manual_control_setpoint.roll;
+					_rates_sp.thrust_body[0] = _manual_control_setpoint.throttle;
+
+					_rates_sp.timestamp = hrt_absolute_time();
+
+					_rates_sp_pub.publish(_rates_sp);
+
 				} else {
 					// Set heading from the manual roll input channel
 					_yaw_control = _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw;
@@ -181,6 +196,14 @@ RoverPositionControl::attitude_setpoint_poll()
 	}
 }
 
+void
+RoverPositionControl::rates_setpoint_poll()
+{
+	if (_rates_sp_sub.updated()) {
+		_rates_sp_sub.copy(&_rates_sp);
+	}
+}
+
 void
 RoverPositionControl::vehicle_attitude_poll()
 {
@@ -223,6 +246,7 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 		matrix::Vector2f ground_speed_2d(ground_speed);
 
 		float mission_throttle = _param_throttle_cruise.get();
+		float max_yaw_rate = _param_rate_max.get();
 
 		/* Just control the throttle */
 		if (_param_speed_control_mode.get() == 1) {
@@ -273,21 +297,29 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 								 prev_wp(1));
 					_gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d);
 
-					_throttle_control = 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);
-					_yaw_control = control_effort;
+					///WIP: Convert Lateral acceleration demand to rate control reference
+					float min_speed = _param_gndspeed_min.get();
+					matrix::Vector2f saturated_speed_2d = (ground_speed_2d.norm() < min_speed) ? ground_speed_2d :
+									      ground_speed_2d.normalized() * min_speed;
+					///TODO: Max yaw rate
+					float desired_yaw_rate = _gnd_control.nav_lateral_acceleration_demand() / saturated_speed_2d.norm();
+					_rates_sp.roll = 0.0;
+					_rates_sp.pitch = 0.0;
+					_rates_sp.yaw = math::constrain(desired_yaw_rate, -max_yaw_rate, max_yaw_rate);
+					_rates_sp.thrust_body[0] = math::constrain(mission_throttle, -1.0f, 1.0f);
+					_rates_sp.timestamp = hrt_absolute_time();
+					_rates_sp_pub.publish(_rates_sp);
 				}
 			}
 			break;
 
 		case STOPPING: {
-				_yaw_control = 0.0f;
-				_throttle_control = 0.0f;
+				_rates_sp.roll = 0.0;
+				_rates_sp.pitch = 0.0;
+				_rates_sp.yaw = 0.0;
+				_rates_sp.thrust_body[0] = 0.0;
+				_rates_sp.timestamp = hrt_absolute_time();
+				_rates_sp_pub.publish(_rates_sp);
 				// 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));
@@ -364,31 +396,57 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi
 	// quaternion attitude control law, qe is rotation from q to qd
 	const Quatf qe = Quatf(att.q).inversed() * Quatf(att_sp.q_d);
 	const Eulerf euler_sp = qe;
+	// PX4_INFO("Yaw error: %f", double(euler_sp(2)));
 
-	float control_effort = euler_sp(2) / _param_max_turn_angle.get();
-	control_effort = math::constrain(control_effort, -1.0f, 1.0f);
-
-	_yaw_control = control_effort;
-
-	const float control_throttle = att_sp.thrust_body[0];
-
-	_throttle_control =  math::constrain(control_throttle, 0.0f, 1.0f);
-
+	_rates_sp.roll = 0.0;
+	_rates_sp.pitch = 0.0;
+	_rates_sp.yaw = euler_sp(2) / _param_max_turn_angle.get();
+	_rates_sp.thrust_body[0] = math::constrain(att_sp.thrust_body[0], 0.0f, 1.0f);
+	_rates_sp.timestamp = hrt_absolute_time();
+	_rates_sp_pub.publish(_rates_sp);
 }
 
+void
+RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const vehicle_local_position_s &local_pos,
+				    const vehicle_rates_setpoint_s &rates_sp)
+{
+	float dt = (_control_rates_last_called > 0) ? hrt_elapsed_time(&_control_rates_last_called) * 1e-6f : 0.01f;
+	_control_rates_last_called = hrt_absolute_time();
+
+	const matrix::Vector3f vehicle_rates(rates.xyz[0], rates.xyz[1], rates.xyz[2]);
+	const matrix::Vector3f rates_setpoint(rates_sp.roll, rates_sp.pitch, rates_sp.yaw);
+
+	const matrix::Vector3f current_velocity(local_pos.vx, local_pos.vy, local_pos.vz);
+	bool lock_integrator = bool(current_velocity.norm() < _param_rate_i_minspeed.get());
+
+	const matrix::Vector3f angular_acceleration{rates.xyz_derivative};
+	const matrix::Vector3f torque = _rate_control.update(vehicle_rates, rates_setpoint, angular_acceleration, dt,
+					lock_integrator);
+	///TODO: Handle mimimum speed constraints
+	float steering_input = math::constrain(torque(2), -1.0f, 1.0f);
+
+	///TODO: Add slew rate constraints
+	_steering_input = steering_input;
+
+	_act_controls.control[actuator_controls_s::INDEX_YAW] = _steering_input;
+
+	const float control_throttle = rates_sp.thrust_body[0];
+
+	_act_controls.control[actuator_controls_s::INDEX_THROTTLE] =  math::constrain(control_throttle, 0.0f, 1.0f);
+}
+
+
 void
 RoverPositionControl::Run()
 {
 	parameters_update(true);
 
 	/* run controller on gyro changes */
-	vehicle_angular_velocity_s angular_velocity;
-
-	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
-
+	if (_vehicle_angular_velocity_sub.update(&_vehicle_rates)) {
 		/* check vehicle control mode for changes to publication state */
 		vehicle_control_mode_poll();
 		attitude_setpoint_poll();
+		rates_setpoint_poll();
 		vehicle_attitude_poll();
 		manual_control_setpoint_poll();
 
@@ -397,6 +455,10 @@ RoverPositionControl::Run()
 		/* update parameters from storage */
 		parameters_update();
 
+		if (!PX4_ISFINITE(_steering_input)) {
+			_steering_input = 0.0;
+		}
+
 		/* only run controller if position changed */
 		if (_local_pos_sub.update(&_local_pos)) {
 
@@ -473,6 +535,11 @@ RoverPositionControl::Run()
 
 		}
 
+		//Body Rate control
+		if (_control_mode.flag_control_rates_enabled) {
+			control_rates(_vehicle_rates, _local_pos, _rates_sp);
+		}
+
 		/* Only publish if any of the proper modes are enabled */
 		if (_control_mode.flag_control_velocity_enabled ||
 		    _control_mode.flag_control_attitude_enabled ||

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -41,14 +41,15 @@
  * @author Marco Zorzi <mzorzi@student.ethz.ch>
  */
 
-#include <float.h>
 
+#include <float.h>
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/pid/pid.h>
+#include <lib/rate_control/rate_control.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
@@ -67,6 +68,8 @@
 #include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -105,6 +108,7 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+	uORB::Publication<vehicle_rates_setpoint_s>	_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
 	uORB::Publication<vehicle_attitude_setpoint_s>	_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};  /**< navigation capabilities publication */
 
@@ -116,14 +120,17 @@ private:
 	uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-
+	uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
 	manual_control_setpoint_s		_manual_control_setpoint{};			    /**< r/c channel data */
 	position_setpoint_triplet_s		_pos_sp_triplet{};		/**< triplet of mission items */
 	vehicle_attitude_setpoint_s		_att_sp{};			/**< attitude setpoint > */
+	vehicle_rates_setpoint_s		_rates_sp{};			/**< rate 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 */
 	vehicle_attitude_s				_vehicle_att{};
+	vehicle_angular_velocity_s _vehicle_rates{};
+
 	trajectory_setpoint_s _trajectory_setpoint{};
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
@@ -133,6 +140,7 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	hrt_abstime _control_position_last_called{0}; 	/**<last call of control_position  */
+	hrt_abstime _control_rates_last_called{0};
 	hrt_abstime _manual_setpoint_last_called{0};
 
 	MapProjection _global_local_proj_ref{};
@@ -145,6 +153,8 @@ private:
 	uint8_t _pos_reset_counter{0};		// captures the number of times the estimator has reset the horizontal position
 
 	ECL_L1_Pos_Controller				_gnd_control;
+	RateControl				_rate_control;
+	float _steering_input{0.0};
 
 	enum UGV_POSCTRL_MODE {
 		UGV_POSCTRL_MODE_AUTO,
@@ -177,6 +187,7 @@ private:
 
 		(ParamFloat<px4::params::GND_SPEED_TRIM>) _param_gndspeed_trim,
 		(ParamFloat<px4::params::GND_SPEED_MAX>) _param_gndspeed_max,
+		(ParamFloat<px4::params::GND_SPEED_MIN>) _param_gndspeed_min,
 
 		(ParamInt<px4::params::GND_SP_CTRL_MODE>) _param_speed_control_mode,
 		(ParamFloat<px4::params::GND_SPEED_P>) _param_speed_p,
@@ -185,12 +196,20 @@ private:
 		(ParamFloat<px4::params::GND_SPEED_IMAX>) _param_speed_imax,
 		(ParamFloat<px4::params::GND_SPEED_THR_SC>) _param_throttle_speed_scaler,
 
+		(ParamFloat<px4::params::GND_RATE_P>) _param_rate_p,
+		(ParamFloat<px4::params::GND_RATE_I>) _param_rate_i,
+		(ParamFloat<px4::params::GND_RATE_D>) _param_rate_d,
+		(ParamFloat<px4::params::GND_RATE_FF>) _param_rate_ff,
+		(ParamFloat<px4::params::GND_RATE_IMAX>) _param_rate_imax,
+		(ParamFloat<px4::params::GND_RATE_MAX>) _param_rate_max,
+		(ParamFloat<px4::params::GND_RATE_IMINSPD>) _param_rate_i_minspeed,
+
 		(ParamFloat<px4::params::GND_THR_MIN>) _param_throttle_min,
 		(ParamFloat<px4::params::GND_THR_MAX>) _param_throttle_max,
 		(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_ATT_P>) _param_att_p,
 		(ParamFloat<px4::params::GND_MAN_Y_MAX>) _param_gnd_man_y_max,
 		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad	/**< loiter radius for Rover */
 	)
@@ -202,6 +221,7 @@ private:
 
 	void		position_setpoint_triplet_poll();
 	void		attitude_setpoint_poll();
+	void		rates_setpoint_poll();
 	void		vehicle_control_mode_poll();
 	void 		vehicle_attitude_poll();
 	void		manual_control_setpoint_poll();
@@ -213,5 +233,7 @@ private:
 					 const position_setpoint_triplet_s &_pos_sp_triplet);
 	void		control_velocity(const matrix::Vector3f &current_velocity);
 	void		control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
+	void		control_rates(const vehicle_angular_velocity_s &rates, const vehicle_local_position_s &local_pos,
+				      const vehicle_rates_setpoint_s &rates_sp);
 
 };

src/modules/rover_pos_control/rover_pos_control_params.c

@@ -48,19 +48,6 @@
  * Controller parameters, accessible via MAVLink
  */
 
-/**
- * Distance from front axle to rear axle
- *
- * A value of 0.31 is typical for 1/10 RC cars.
- *
- * @unit m
- * @min 0.0
- * @decimal 3
- * @increment 0.01
- * @group Rover Position Control
- */
-PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f);
-
 /**
  * L1 distance
  *
@@ -247,6 +234,19 @@ PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f);
  */
 PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
 
+/**
+ * Minimum ground speed
+ *
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 40
+ * @decimal 1
+ * @increment 0.5
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_SPEED_MIN, 1.0f);
+
 /**
  * Maximum ground speed
  *
@@ -275,6 +275,19 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
  */
 PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
 
+/**
+ * Attitude control P gain
+ *
+ *
+ * @unit rad
+ * @min 0.0
+ * @max 5.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_ATT_P, 1.0f);
+
 /**
  * Max manual yaw rate
  *
@@ -285,3 +298,87 @@ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
  * @group Rover Position Control
  */
 PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f);
+
+/**
+ * Rover Rate Proportional Gain
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 10.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_P, 5.0f);
+
+/**
+ * Rover Rate Integral Gain
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 1.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_I, 0.5f);
+
+/**
+ * Rover Rate Integral Gain
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 1.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_D, 0.0f);
+
+/**
+ * Rover Rate Proportional Gain
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 10.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_FF, 1.6f);
+
+/**
+ * Rover Rate Maximum Integral Gain
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 50.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_IMAX, 1.0f);
+
+/**
+ * Rover Rate Maximum Rate
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 1.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_MAX, 0.5f);
+
+/**
+ * Rover Rate Integral Minimum speed
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 50.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_IMINSPD, 1.0f);