Enable rover attitude setpoint

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);
+
 			}