Add Rover Rates Control

This commit adds a angular rates controller controlling the yaw rate of the rover. This is also used for the previous attitude controller in order to stabilize the vehicles.

This is important for vehicles operating in low friction surfaces or boats

src/modules/rover_pos_control/CMakeLists.txt

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

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -102,6 +102,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()));
 	}
 }
 
@@ -151,6 +155,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.y;
+					_rates_sp.thrust_body[0] = _manual_control_setpoint.z;
+
+					_rates_sp.timestamp = hrt_absolute_time();
+
+					_rates_sp_pub.publish(_rates_sp);
+
 				} else {
 					_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y;
 					_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x;
@@ -187,6 +202,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()
 {
@@ -195,6 +218,14 @@ RoverPositionControl::vehicle_attitude_poll()
 	}
 }
 
+void
+RoverPositionControl::vehicle_angular_acceleration_poll()
+{
+	if (_vehicle_angular_acceleration_sub.updated()) {
+		_vehicle_angular_acceleration_sub.copy(&_vehicle_angular_acceleration);
+	}
+}
+
 bool
 RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 				       const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
@@ -380,31 +411,59 @@ 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);
+	_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);
+	control_rates(_vehicle_rates, _vehicle_angular_acceleration, _local_pos, _rates_sp);
+}
 
-	_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
+void
+RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const  vehicle_angular_acceleration_s &acc,
+				    const vehicle_local_position_s &local_pos,
+				    const vehicle_rates_setpoint_s &rates_sp)
+{
+	float dt = 0.01; // Using non zero value to a avoid division by zero
 
-	const float control_throttle = att_sp.thrust_body[0];
+	if (_control_rates_last_called > 0) {
+		dt = hrt_elapsed_time(&_control_rates_last_called) * 1e-6f;
+	}
+
+	const matrix::Vector3f current_velocity(local_pos.vx, local_pos.vy, local_pos.vz);
+	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 angular_acceleration{acc.xyz};
+	bool lock_integrator = bool(current_velocity.norm() < _param_rate_i_minspeed.get());
+	const matrix::Vector3f torque = _rate_control.update(vehicle_rates, rates_setpoint, angular_acceleration, dt,
+					lock_integrator);
+
+	_steering_input = math::constrain(_steering_input + torque(2), -1.0f, 1.0f);
+
+	_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)) {
+		// grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy
+		vehicle_angular_acceleration_poll();
 		/* 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();
 
@@ -413,6 +472,10 @@ RoverPositionControl::Run()
 		/* update parameters from storage */
 		parameters_update();
 
+		if (!is_finite(_steering_input)) {
+			_steering_input = 0.0;
+		}
+
 		/* only run controller if position changed */
 		if (_local_pos_sub.update(&_local_pos)) {
 
@@ -492,6 +555,14 @@ RoverPositionControl::Run()
 
 		}
 
+		if (_control_mode.flag_control_rates_enabled
+		    && !_control_mode.flag_control_attitude_enabled
+		    && !_control_mode.flag_control_position_enabled
+		    && !_control_mode.flag_control_velocity_enabled) {
+			//Body Rate control
+			control_rates(_vehicle_rates, _vehicle_angular_acceleration, _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,8 +41,9 @@
  * @author Marco Zorzi <mzorzi@student.ethz.ch>
  */
 
-#include <float.h>
+#include <RoverRateControl.hpp>
 
+#include <float.h>
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
@@ -67,6 +68,9 @@
 #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_angular_acceleration.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>
@@ -106,6 +110,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 */
 	uORB::Publication<actuator_controls_s>		_actuator_controls_pub{ORB_ID(actuator_controls_0)};  /**< actuator controls publication */
@@ -118,10 +123,12 @@ 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)};
+	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
 	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 */
@@ -136,6 +143,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{};
@@ -149,6 +157,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;
+	RoverRateControl				_rate_control;
+	float _steering_input{0.0};
 
 	enum UGV_POSCTRL_MODE {
 		UGV_POSCTRL_MODE_AUTO,
@@ -187,12 +197,21 @@ 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 */
 	)
@@ -204,8 +223,10 @@ private:
 
 	void		position_setpoint_triplet_poll();
 	void		attitude_setpoint_poll();
+	void		rates_setpoint_poll();
 	void		vehicle_control_mode_poll();
 	void 		vehicle_attitude_poll();
+	void		vehicle_angular_acceleration_poll();
 	void		manual_control_setpoint_poll();
 
 	/**
@@ -215,5 +236,8 @@ 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_angular_acceleration_s &acc,
+				      const vehicle_local_position_s &local_pos,
+				      const vehicle_rates_setpoint_s &rates_sp);
 
 };

src/modules/rover_pos_control/RoverRateControl/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(RoverRateControl
+	RoverRateControl.cpp
+	RoverRateControl.hpp
+)
+target_compile_options(RoverRateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(RoverRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(RoverRateControl PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC RoverRateControlTest.cpp LINKLIBS RoverRateControl)

src/modules/rover_pos_control/RoverRateControl/RoverRateControl.cpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RoverRateControl.cpp
+ */
+
+#include <RoverRateControl.hpp>
+#include <px4_platform_common/defines.h>
+
+using namespace matrix;
+
+void RoverRateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
+{
+	_gain_p = P;
+	_gain_i = I;
+	_gain_d = D;
+}
+
+Vector3f RoverRateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel,
+				  const float dt, const bool landed)
+{
+	// angular rates error
+	Vector3f rate_error = rate_sp - rate;
+
+	// PID control with feed forward
+	const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp);
+
+	// update integral only if we are not landed
+	if (!landed) {
+		updateIntegral(rate_error, dt);
+	}
+
+	return torque;
+}
+
+void RoverRateControl::updateIntegral(Vector3f &rate_error, const float dt)
+{
+	for (int i = 0; i < 3; i++) {
+		// prevent further positive control saturation
+		if (_mixer_saturation_positive[i]) {
+			rate_error(i) = math::min(rate_error(i), 0.f);
+		}
+
+		// prevent further negative control saturation
+		if (_mixer_saturation_negative[i]) {
+			rate_error(i) = math::max(rate_error(i), 0.f);
+		}
+
+		// I term factor: reduce the I gain with increasing rate error.
+		// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
+		// change (noticeable in a bounce-back effect after a flip).
+		// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
+		// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
+		// and up to 200 deg error leads to <25% reduction of I.
+		float i_factor = rate_error(i) / math::radians(400.f);
+		i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
+
+		// Perform the integration using a first order method
+		float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt;
+
+		// do not propagate the result if out of range or invalid
+		if (PX4_ISFINITE(rate_i)) {
+			_rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i));
+		}
+	}
+}
+
+void RoverRateControl::getRoverRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
+{
+	rate_ctrl_status.rollspeed_integ = _rate_int(0);
+	rate_ctrl_status.pitchspeed_integ = _rate_int(1);
+	rate_ctrl_status.yawspeed_integ = _rate_int(2);
+}

src/modules/rover_pos_control/RoverRateControl/RoverRateControl.hpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RoverRateControl.hpp
+ *
+ * PID 3 axis angular rate / angular velocity control.
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <uORB/topics/rate_ctrl_status.h>
+
+class RoverRateControl
+{
+public:
+	RoverRateControl() = default;
+	~RoverRateControl() = default;
+
+	/**
+	 * Set the rate control gains
+	 * @param P 3D vector of proportional gains for body x,y,z axis
+	 * @param I 3D vector of integral gains
+	 * @param D 3D vector of derivative gains
+	 */
+	void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
+
+	/**
+	 * Set the mximum absolute value of the integrator for all axes
+	 * @param integrator_limit limit value for all axes x, y, z
+	 */
+	void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
+
+	/**
+	 * Set direct rate to torque feed forward gain
+	 * @see _gain_ff
+	 * @param FF 3D vector of feed forward gains for body x,y,z axis
+	 */
+	void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
+
+	/**
+	 * Run one control loop cycle calculation
+	 * @param rate estimation of the current vehicle angular rate
+	 * @param rate_sp desired vehicle angular rate setpoint
+	 * @param dt desired vehicle angular rate setpoint
+	 * @return [-1,1] normalized torque vector to apply to the vehicle
+	 */
+	matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp,
+				const matrix::Vector3f &angular_accel, const float dt, const bool landed);
+
+	/**
+	 * Set the integral term to 0 to prevent windup
+	 * @see _rate_int
+	 */
+	void resetIntegral() { _rate_int.zero(); }
+
+	/**
+	 * Get status message of controller for logging/debugging
+	 * @param rate_ctrl_status status message to fill with internal states
+	 */
+	void getRoverRateControlStatus(rate_ctrl_status_s &rate_ctrl_status);
+
+private:
+	void updateIntegral(matrix::Vector3f &rate_error, const float dt);
+
+	// Gains
+	matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z
+	matrix::Vector3f _gain_i; ///< rate control integral gain
+	matrix::Vector3f _gain_d; ///< rate control derivative gain
+	matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
+	matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
+
+	// States
+	matrix::Vector3f _rate_int; ///< integral term of the rate controller
+
+	bool _mixer_saturation_positive[3] {};
+	bool _mixer_saturation_negative[3] {};
+};

src/modules/rover_pos_control/RoverRateControl/RoverRateControlTest.cpp

@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <RoverRateControl.hpp>
+
+using namespace matrix;
+
+TEST(RoverRateControlTest, AllZeroCase)
+{
+	RoverRateControl rate_control;
+	Vector3f torque = rate_control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
+	EXPECT_EQ(torque, Vector3f());
+}

src/modules/rover_pos_control/rover_pos_control_params.c

@@ -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 50.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_MAX, 5.0f);
+
+/**
+ * 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, 0.3f);