uuv_att_control: added bodyrate setpoint to the geometric controller

* added bodyrate setpoint to the geometric controller
* started switching to new uORB API

src/modules/uuv_att_control/uuv_att_control.cpp

@@ -112,6 +112,11 @@ void UUVAttitudeControl::vehicle_attitude_setpoint_poll()
 	}
 }
 
+void UUVAttitudeControl::vehicle_rates_setpoint_poll()
+{
+	_vehicle_rates_setpoint_sub.update(&_vehicle_rates_sp);
+}
+
 
 void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_u)
 {
@@ -186,6 +191,10 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, con
 	float pitch_body = _vehicle_attitude_sp.pitch_body;
 	float yaw_body = _vehicle_attitude_sp.yaw_body;
 
+	float roll_rate_desired = _vehicle_rates_sp.roll;
+	float pitch_rate_desired = _vehicle_rates_sp.pitch;
+	float yaw_rate_desired = _vehicle_rates_sp.yaw;
+
 	/* get attitude setpoint rotational matrix */
 	Dcmf _rot_des = Eulerf(roll_body, pitch_body, yaw_body);
 
@@ -205,9 +214,9 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, con
 	e_R_vec(1) = e_R(0, 2);  /**< Pitch */
 	e_R_vec(2) = e_R(1, 0);  /**< Yaw   */
 
-	omega(0) = _angular_velocity.xyz[0] - 0.0f;
-	omega(1) = _angular_velocity.xyz[1] - 0.0f;
-	omega(2) = _angular_velocity.xyz[2] - 0.0f;
+	omega(0) = _angular_velocity.xyz[0] - roll_rate_desired;
+	omega(1) = _angular_velocity.xyz[1] - pitch_rate_desired;
+	omega(2) = _angular_velocity.xyz[2] - yaw_rate_desired;
 
 	/**< P-Control */
 	torques(0) = - e_R_vec(0) * _param_roll_p.get();	/**< Roll  */
@@ -306,6 +315,7 @@ void UUVAttitudeControl::run()
 			orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
 
 			vehicle_attitude_setpoint_poll();
+			vehicle_rates_setpoint_poll();
 			vehicle_control_mode_poll();
 			manual_control_setpoint_poll();
 
@@ -364,6 +374,8 @@ void UUVAttitudeControl::run()
 	}
 
 	orb_unsubscribe(_vcontrol_mode_sub);
+	orb_unsubscribe(_vehicle_attitude_sp_sub);
+	orb_unsubscribe(_angular_velocity_sub);
 	orb_unsubscribe(_manual_control_setpoint_sub);
 	orb_unsubscribe(_vehicle_attitude_sub);
 	orb_unsubscribe(_local_pos_sub);

src/modules/uuv_att_control/uuv_att_control.hpp

@@ -122,6 +122,7 @@ private:
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};
 
 	int	_vehicle_attitude_sp_sub{-1};	/**< vehicle attitude setpoint */
+	uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle bodyrates setpoint subscriber */
 	int	_battery_status_sub{-1};	/**< battery status subscription */
 	int	_vehicle_attitude_sub{-1};	/**< control state subscription */
 	int	_angular_velocity_sub{-1};	/**< vehicle angular velocity subscription */
@@ -135,20 +136,18 @@ private:
 	vehicle_attitude_s		_vehicle_attitude {};	/**< control state */
 	vehicle_angular_velocity_s	_angular_velocity{};	/**< angular velocity */
 	vehicle_attitude_setpoint_s	_vehicle_attitude_sp {};/**< vehicle attitude setpoint */
+	vehicle_rates_setpoint_s _vehicle_rates_sp {}; /**< vehicle bodyrates setpoint */
 	vehicle_control_mode_s		_vcontrol_mode {};	/**< vehicle control mode */
 	sensor_combined_s		_sensor_combined{};
 	vehicle_local_position_s	_local_pos{};		/**< vehicle local position */
 
-
 	SubscriptionData<vehicle_acceleration_s>		_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	hrt_abstime _control_position_last_called{0}; 	/**<last call of control_position  */
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-
 	// estimator reset counters
 	uint8_t _pos_reset_counter{0};		// captures the number of times the estimator has reset the horizontal position
 
-
 	bool _debug{false};	/**< if set to true, print debug output */
 	int loop_counter = 0;
 
@@ -171,14 +170,15 @@ private:
 	/**
 	 * Update our local parameter cache.
 	 */
-	void	parameters_update(bool force = false);
+	void parameters_update(bool force = false);
 
-	void	manual_control_setpoint_poll();
-	void	position_setpoint_triplet_poll();
-	void	vehicle_control_mode_poll();
-	void 	vehicle_attitude_poll();
-	void	vehicle_attitude_setpoint_poll();
-	void	vehicle_local_position_poll();
+	void manual_control_setpoint_poll();
+	void position_setpoint_triplet_poll();
+	void vehicle_control_mode_poll();
+	void vehicle_attitude_poll();
+	void vehicle_attitude_setpoint_poll();
+	void vehicle_rates_setpoint_poll();
+	void vehicle_local_position_poll();
 
 	/**
 	 * Control Attitude