Enable Offboard Bodyrate control for Rovers

src/modules/rover_pos_control/CMakeLists.txt

@@ -30,13 +30,17 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
+add_subdirectory(RoverRateControl)
+
 px4_add_module(
 	MODULE modules__rover_pos_control
 	MAIN rover_pos_control
 	SRCS
 		RoverPositionControl.cpp
 		RoverPositionControl.hpp
 	DEPENDS
+		RoverRateControl
 		l1
 		pid
 	)

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -90,6 +90,12 @@ void RoverPositionControl::parameters_update(bool force)
 				   _param_speed_d.get(),
 				   _param_speed_imax.get(),
 				   _param_gndspeed_max.get());
+
+		// _rate_control.set_k_p(_param_rate_p.get());
+		// _rate_control.set_k_i(_param_rate_i.get());
+		// _rate_control.set_k_ff(_param_rate_ff.get());
+		// _rate_control.set_integrator_max(_param_rate_imax.get());
+		// _rate_control.set_max_rate(_param_rate_max.get());
 	}
 }
 
@@ -148,6 +154,28 @@ RoverPositionControl::vehicle_attitude_poll()
 	}
 }
 
+void
+RoverPositionControl::rates_setpoint_poll()
+{
+	bool rates_sp_updated;
+	orb_check(_rates_sp_sub, &rates_sp_updated);
+
+	if (rates_sp_updated) {
+		orb_copy(ORB_ID(vehicle_rates_setpoint), _rates_sp_sub, &_rates_sp);
+	}
+}
+
+void
+RoverPositionControl::vehicle_angular_velocity_poll()
+{
+	bool rates_updated;
+	orb_check(_vehicle_angular_velocity_sub, &rates_updated);
+
+	if (rates_updated) {
+		orb_copy(ORB_ID(vehicle_angular_velocity), _vehicle_angular_velocity_sub, &_vehicle_rates);
+	}
+}
+
 bool
 RoverPositionControl::control_position(const matrix::Vector2f &current_position,
 				       const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
@@ -334,7 +362,12 @@ 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;
-
+	//TODO: Switch to rate controller
+	// struct ECL_ControlData control_input = {};
+	// control_input.yaw = euler_sp(2);
+	// _rate_control.control_attitude(control_input);
+	// control_input.yaw_rate_setpoint = _rate_control.get_desired_rate();
+	// float control_effort = att_control.control_bodyrate(control_input);
 	float control_effort = euler_sp(2) / _param_max_turn_angle.get();
 	control_effort = math::constrain(control_effort, -1.0f, 1.0f);
 
@@ -346,15 +379,44 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi
 
 }
 
+void
+RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const matrix::Vector3f &current_velocity,
+				    const vehicle_rates_setpoint_s &rates_sp)
+{
+	// struct ECL_ControlData control_input = {};
+	// const float current_speed = current_velocity.norm();
+
+	// Set scaling factor with local velocity
+	// control_input.groundspeed = current_speed;
+	// // Lock integrator when local velocity is small
+	// control_input.lock_integrator = (current_speed < _param_rate_i_minspeed.get());
+
+	// control_input.body_z_rate = rates.xyz[2];
+	// _rate_control.set_bodyrate_setpoint(rates_sp.yaw);
+
+	float control_effort = 0.0f;
+	// float control_effort = _rate_control.control_bodyrate(control_input);
+	control_effort = math::constrain(control_effort, -1.0f, 1.0f);
+
+	_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
+
+	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()
 {
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	_vehicle_angular_velocity_sub = orb_subscribe(ORB_ID(vehicle_angular_velocity));
 	_manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
 	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -369,7 +431,7 @@ RoverPositionControl::run()
 	parameters_update(true);
 
 	/* wakeup source(s) */
-	px4_pollfd_struct_t fds[5];
+	px4_pollfd_struct_t fds[6];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
@@ -382,6 +444,8 @@ RoverPositionControl::run()
 	fds[3].events = POLLIN;
 	fds[4].fd = _local_pos_sub;  // Added local position as source of position
 	fds[4].events = POLLIN;
+	fds[5].fd = _vehicle_angular_velocity_sub;  // Added local position as source of position
+	fds[5].events = POLLIN;
 
 	while (!should_exit()) {
 
@@ -397,6 +461,8 @@ RoverPositionControl::run()
 		/* check vehicle control mode for changes to publication state */
 		vehicle_control_mode_poll();
 		attitude_setpoint_poll();
+		rates_setpoint_poll();
+		vehicle_angular_velocity_poll();
 		//manual_control_setpoint_poll();
 
 		_vehicle_acceleration_sub.update();
@@ -480,14 +546,28 @@ RoverPositionControl::run()
 
 			vehicle_attitude_poll();
 
-			if (!manual_mode && _control_mode.flag_control_attitude_enabled
+			if (manual_mode && _control_mode.flag_control_attitude_enabled
 			    && !_control_mode.flag_control_position_enabled
 			    && !_control_mode.flag_control_velocity_enabled) {
-
 				control_attitude(_vehicle_att, _att_sp);
+				PX4_INFO("Control Attitude");
+
+			}
+		}
+
+		if (fds[5].revents & POLLIN) {
+			vehicle_angular_velocity_poll();
 
+			if (!manual_mode && _control_mode.flag_control_rates_enabled
+			    && !_control_mode.flag_control_attitude_enabled
+			    && !_control_mode.flag_control_position_enabled
+			    && !_control_mode.flag_control_velocity_enabled) {
+				//Offboard rate control
+				matrix::Vector3f vehicle_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz);
+				control_rates(_vehicle_rates, vehicle_velocity, _rates_sp);
 			}
 
+			//TODO: Add stabilized mode for rovers
 		}
 
 		if (fds[1].revents & POLLIN) {
@@ -517,6 +597,7 @@ RoverPositionControl::run()
 			if (_control_mode.flag_control_velocity_enabled ||
 			    _control_mode.flag_control_attitude_enabled ||
 			    _control_mode.flag_control_position_enabled ||
+			    _control_mode.flag_control_rates_enabled ||
 			    manual_mode) {
 				/* publish the actuator controls */
 				_actuator_controls_pub.publish(_act_controls);
@@ -530,7 +611,9 @@ RoverPositionControl::run()
 	orb_unsubscribe(_local_pos_sub);
 	orb_unsubscribe(_manual_control_setpoint_sub);
 	orb_unsubscribe(_pos_sp_triplet_sub);
+	orb_unsubscribe(_rates_sp_sub);
 	orb_unsubscribe(_vehicle_attitude_sub);
+	orb_unsubscribe(_vehicle_angular_velocity_sub);
 	orb_unsubscribe(_sensor_combined_sub);
 
 	warnx("exiting.\n");

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -41,8 +41,9 @@
  * @author Marco Zorzi <[email protected]>
  */
 
-#include <float.h>
+#include <RoverRateControl.hpp>
 
+#include <float.h>
 #include <drivers/drv_hrt.h>
 #include <lib/ecl/geo/geo.h>
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
@@ -65,6 +66,8 @@
 #include <uORB/topics/sensor_combined.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_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -108,19 +111,23 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod
 	int		_manual_control_setpoint_sub{-1};		/**< notification of manual control updates */
 	int		_pos_sp_triplet_sub{-1};
 	int		_att_sp_sub{-1};
+	int		_rates_sp_sub{-1};
 	int		_vehicle_attitude_sub{-1};
+	int		_vehicle_angular_velocity_sub{-1};
 	int		_sensor_combined_sub{-1};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	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 */
 	actuator_controls_s				_act_controls{};		/**< direct control of actuators */
 	vehicle_attitude_s				_vehicle_att{};
+	vehicle_angular_velocity_s 		_vehicle_rates{};
 	sensor_combined_s				_sensor_combined{};
 
 	uORB::SubscriptionData<vehicle_acceleration_s>		_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
@@ -137,6 +144,7 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod
 	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;
 
 	enum UGV_POSCTRL_MODE {
 		UGV_POSCTRL_MODE_AUTO,
@@ -167,6 +175,13 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod
 		(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_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,
@@ -184,8 +199,10 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod
 	void		manual_control_setpoint_poll();
 	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_velocity_poll();
 
 	/**
 	 * Control position.
@@ -194,5 +211,7 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod
 					 const position_setpoint_triplet_s &_pos_sp_triplet);
 	void		control_velocity(const matrix::Vector3f &current_velocity, const position_setpoint_triplet_s &pos_sp_triplet);
 	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 matrix::Vector3f &current_velocity,
+				      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 RateControl)