Enable rover attitude setpoint

src/modules/mavlink/mavlink_receiver.cpp

@@ -1302,6 +1302,7 @@ void MavlinkReceiver::fill_thrust(float *thrust_body_array, uint8_t vehicle_type
 		break;
 
 	case MAV_TYPE_FIXED_WING:
+	case MAV_TYPE_GROUND_ROVER:
 		thrust_body_array[0] = thrust;
 		break;
 
@@ -1313,10 +1314,6 @@ void MavlinkReceiver::fill_thrust(float *thrust_body_array, uint8_t vehicle_type
 		thrust_body_array[2] = -thrust;
 		break;
 
-	case MAV_TYPE_GROUND_ROVER:
-		thrust_body_array[0] = thrust;
-		break;
-
 	case MAV_TYPE_VTOL_DUOROTOR:
 	case MAV_TYPE_VTOL_QUADROTOR:
 	case MAV_TYPE_VTOL_TILTROTOR:

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -128,6 +128,17 @@ RoverPositionControl::position_setpoint_triplet_poll()
 	}
 }
 
+void
+RoverPositionControl::attitude_setpoint_poll()
+{
+	bool att_sp_updated;
+	orb_check(_att_sp_sub, &att_sp_updated);
+
+	if (att_sp_updated) {
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+	}
+}
+
 void
 RoverPositionControl::vehicle_attitude_poll()
 {
@@ -304,6 +315,24 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity,
 	}
 }
 
+void
+RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp)
+{
+	// 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;
+
+	float control_effort = euler_sp(2) / _param_max_turn_angle.get();
+	control_effort = math::constrain(control_effort, -1.0f, 1.0f);
+
+	_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
+
+	const float control_throttle = att_sp.thrust_body[0];
+
+	_act_controls.control[actuator_controls_s::INDEX_THROTTLE] =  math::constrain(control_throttle, 0.0f, 1.0f);
+
+}
+
 void
 RoverPositionControl::run()
 {
@@ -312,6 +341,8 @@ RoverPositionControl::run()
 	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_manual_control_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));
+
 	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 
@@ -348,6 +379,7 @@ 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();
@@ -426,6 +458,10 @@ RoverPositionControl::run()
 
 				control_velocity(current_velocity, _pos_sp_triplet);
 
+			} else if (!manual_mode && _control_mode.flag_control_attitude_enabled) {
+
+				control_attitude(_vehicle_att, _att_sp);
+
 			}
 
 

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -106,13 +106,15 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod
 	int		_local_pos_sub{-1};
 	int		_manual_control_sub{-1};		/**< notification of manual control updates */
 	int		_pos_sp_triplet_sub{-1};
+	int 		_att_sp_sub{-1};
 	int     _vehicle_attitude_sub{-1};
 	int		_sensor_combined_sub{-1};
 
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};
 
 	manual_control_setpoint_s		_manual{};			    /**< r/c channel data */
 	position_setpoint_triplet_s		_pos_sp_triplet{};		/**< triplet of mission items */
+	vehicle_attitude_setpoint_s		_att_sp{};			/**< attitude 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 */
@@ -172,6 +174,7 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod
 
 	void		manual_control_setpoint_poll();
 	void		position_setpoint_triplet_poll();
+	void		attitude_setpoint_poll();
 	void		vehicle_control_mode_poll();
 	void 		vehicle_attitude_poll();
 
@@ -181,5 +184,6 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod
 	bool		control_position(const matrix::Vector2f &global_pos, const matrix::Vector3f &ground_speed,
 					 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);
 
 };