uuv_att_control: removed redundant code, switched to new uORB API

src/modules/uuv_att_control/uuv_att_control.cpp

@@ -59,8 +59,9 @@ extern "C" __EXPORT int uuv_att_control_main(int argc, char *argv[]);
 
 UUVAttitudeControl::UUVAttitudeControl():
 	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::attitude_ctrl),
 	/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "gnda_dt"))
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
 }
 
@@ -69,6 +70,16 @@ UUVAttitudeControl::~UUVAttitudeControl()
 	perf_free(_loop_perf);
 }
 
+bool UUVAttitudeControl::init()
+{
+	if (!_vehicle_attitude_sub.registerCallback()) {
+		PX4_ERR("vehicle_attitude callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
 void UUVAttitudeControl::parameters_update(bool force)
 {
 	// check for parameter updates
@@ -82,42 +93,6 @@ void UUVAttitudeControl::parameters_update(bool force)
 	}
 }
 
-void UUVAttitudeControl::vehicle_control_mode_poll()
-{
-	bool updated = false;
-	orb_check(_vcontrol_mode_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
-	}
-}
-
-void UUVAttitudeControl::manual_control_setpoint_poll()
-{
-	bool updated = false;
-	orb_check(_manual_control_setpoint_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
-	}
-}
-
-void UUVAttitudeControl::vehicle_attitude_setpoint_poll()
-{
-	bool updated = false;
-	orb_check(_vehicle_attitude_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_attitude_setpoint), _vehicle_attitude_sp_sub, &_vehicle_attitude_sp);
-	}
-}
-
-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)
 {
 	if (PX4_ISFINITE(roll_u)) {
@@ -126,10 +101,6 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
 
 	} else {
 		_actuators.control[actuator_controls_s::INDEX_ROLL] = 0.0f;
-
-		if (loop_counter % 10 == 0) {
-			PX4_INFO("roll_u %.4f", (double)roll_u);
-		}
 	}
 
 	if (PX4_ISFINITE(pitch_u)) {
@@ -138,10 +109,6 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
 
 	} else {
 		_actuators.control[actuator_controls_s::INDEX_PITCH] = 0.0f;
-
-		if (loop_counter % 10 == 0) {
-			PX4_INFO("pitch_u %.4f", (double)pitch_u);
-		}
 	}
 
 	if (PX4_ISFINITE(yaw_u)) {
@@ -150,10 +117,6 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
 
 	} else {
 		_actuators.control[actuator_controls_s::INDEX_YAW] = 0.0f;
-
-		if (loop_counter % 10 == 0) {
-			PX4_INFO("yaw_u %.4f", (double)yaw_u);
-		}
 	}
 
 	if (PX4_ISFINITE(thrust_u)) {
@@ -162,15 +125,12 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
 
 	} else {
 		_actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
-
-		if (loop_counter % 10 == 0) {
-			PX4_INFO("thrust_u %.4f", (double)thrust_u);
-		}
 	}
 }
 
-
+void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude,
-void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp)
+		const vehicle_attitude_setpoint_s &attitude_setpoint, const vehicle_angular_velocity_s &angular_velocity,
+		const vehicle_rates_setpoint_s &rates_setpoint)
 {
 	/** Geometric Controller
 	 *
@@ -178,45 +138,43 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, con
 	 * D. Mellinger, V. Kumar, "Minimum Snap Trajectory Generation and Control for Quadrotors", IEEE ICRA 2011, pp. 2520-2525.
 	 * D. A. Duecker, A. Hackbarth, T. Johannink, E. Kreuzer, and E. Solowjow, “Micro Underwater Vehicle Hydrobatics: A SubmergedFuruta Pendulum,” IEEE ICRA 2018, pp. 7498–7503.
 	 */
-
+	Eulerf euler_angles(matrix::Quatf(attitude.q));
-
-	Eulerf euler_angles(matrix::Quatf(att.q));
 
 	float roll_u;
 	float pitch_u;
 	float yaw_u;
 	float thrust_u;
 
-	float roll_body = _vehicle_attitude_sp.roll_body;
+	float roll_body = attitude_setpoint.roll_body;
-	float pitch_body = _vehicle_attitude_sp.pitch_body;
+	float pitch_body = attitude_setpoint.pitch_body;
-	float yaw_body = _vehicle_attitude_sp.yaw_body;
+	float yaw_body = attitude_setpoint.yaw_body;
 
-	float roll_rate_desired = _vehicle_rates_sp.roll;
+	float roll_rate_desired = rates_setpoint.roll;
-	float pitch_rate_desired = _vehicle_rates_sp.pitch;
+	float pitch_rate_desired = rates_setpoint.pitch;
-	float yaw_rate_desired = _vehicle_rates_sp.yaw;
+	float yaw_rate_desired = rates_setpoint.yaw;
 
 	/* get attitude setpoint rotational matrix */
-	Dcmf _rot_des = Eulerf(roll_body, pitch_body, yaw_body);
+	Dcmf rot_des = Eulerf(roll_body, pitch_body, yaw_body);
 
 	/* get current rotation matrix from control state quaternions */
-	Quatf q_att(att.q[0], att.q[1], att.q[2], att.q[3]);
+	Quatf q_att(attitude.q);
-	Matrix3f _rot_att =  matrix::Dcm<float>(q_att);
+	Matrix3f rot_att =  matrix::Dcm<float>(q_att);
 
 	Vector3f e_R_vec;
 	Vector3f torques;
-	Vector3f omega;
 
 	/* Compute matrix: attitude error */
-	Matrix3f e_R = (_rot_des.transpose() * _rot_att - _rot_att.transpose() * _rot_des) * 0.5;
+	Matrix3f e_R = (rot_des.transpose() * rot_att - rot_att.transpose() * rot_des) * 0.5;
 
 	/* vee-map the error to get a vector instead of matrix e_R */
 	e_R_vec(0) = e_R(2, 1);  /**< Roll  */
 	e_R_vec(1) = e_R(0, 2);  /**< Pitch */
 	e_R_vec(2) = e_R(1, 0);  /**< Yaw   */
 
-	omega(0) = _angular_velocity.xyz[0] - roll_rate_desired;
+	Vector3f omega{angular_velocity.xyz};
-	omega(1) = _angular_velocity.xyz[1] - pitch_rate_desired;
+	omega(0) -= roll_rate_desired;
-	omega(2) = _angular_velocity.xyz[2] - yaw_rate_desired;
+	omega(1) -= pitch_rate_desired;
+	omega(2) -= yaw_rate_desired;
 
 	/**< P-Control */
 	torques(0) = - e_R_vec(0) * _param_roll_p.get();	/**< Roll  */
@@ -232,93 +190,33 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, con
 	pitch_u = torques(1);
 	yaw_u = torques(2);
 
-	//Quatf q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]);
-	//Matrix3f _rot_att = q_att.to_dcm();
-	/** Vector3f current_velocity_boat;
-
-	current_velocity_boat(0) = _local_pos.vx;
-	current_velocity_boat(1) = _local_pos.vy;
-	current_velocity_boat(2) = _local_pos.vz;
-
-	current_velocity_boat = q_att.to_dcm() * current_velocity_boat;
-	*/
-
 	// take thrust as
-	thrust_u = _vehicle_attitude_sp.thrust_body[0];
+	thrust_u = attitude_setpoint.thrust_body[0];
 
 	constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_u);
 	/* Geometric Controller END*/
 }
 
-
+void UUVAttitudeControl::Run()
-void UUVAttitudeControl::run()
 {
-	_vehicle_attitude_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	if (should_exit()) {
-	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+		_vehicle_attitude_sub.unregisterCallback();
-	_angular_velocity_sub = orb_subscribe(ORB_ID(vehicle_angular_velocity));
+		exit_and_cleanup();
-	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+		return;
-	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-
-	_manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-
-	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
-
-
-	/* rate limit control mode updates to 5Hz */
-	orb_set_interval(_vcontrol_mode_sub, 200);
-
-	parameters_update(true);
-
-	/* wakeup source(s) */
-	px4_pollfd_struct_t fds[3];
-
-	/* Setup of loop */
-	fds[0].fd = _vehicle_attitude_sub;
-	fds[0].events = POLLIN;
-	fds[1].fd = _manual_control_setpoint_sub;
-	fds[1].events = POLLIN;
-	fds[2].fd = _sensor_combined_sub;
-	fds[2].events = POLLIN;
-
-
-	while (!should_exit()) {
-		/* wait for up to 500ms for data */
-		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
-
-		/* this is undesirable but not much we can do - might want to flag unhappy status */
-		if (pret < 0) {
-			warn("poll error %d, %d", pret, errno);
-			continue;
 	}
 
 	/* check vehicle control mode for changes to publication state */
-		vehicle_control_mode_poll();
+	_vcontrol_mode_sub.update(&_vcontrol_mode);
-		vehicle_attitude_setpoint_poll();
 
 	/* update parameters from storage */
 	parameters_update();
 
+	vehicle_attitude_s attitude;
+
 	/* only run controller if attitude changed */
-		if (fds[0].revents & POLLIN) {
+	if (_vehicle_attitude_sub.update(&attitude)) {
-			static uint64_t last_run = 0;
+		vehicle_angular_velocity_s angular_velocity {};
-			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+		_angular_velocity_sub.copy(&angular_velocity);
-			last_run = hrt_absolute_time();
-
-			/* guard against too large deltaT's */
-			if (deltaT > 1.0f || fabsf(deltaT) < 0.00001f || !PX4_ISFINITE(deltaT)) {
-				deltaT = 0.01f;
-			}
-
-			/* load local copies */
-			orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_attitude);
-			orb_copy(ORB_ID(vehicle_angular_velocity), _angular_velocity_sub, &_angular_velocity);
-			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();
-
 
 		/* Run geometric attitude controllers if NOT manual mode*/
 		if (!_vcontrol_mode.flag_control_manual_enabled
@@ -327,28 +225,27 @@ void UUVAttitudeControl::run()
 
 			int input_mode = _param_input_mode.get();
 
-				// if (input_mode == 0) // process incoming vehicles setpoint data --> nothing to do
+			_vehicle_attitude_setpoint_sub.update(&_attitude_setpoint);
+			_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
+
 			if (input_mode == 1) { // process manual data
-					_vehicle_attitude_sp.roll_body = _param_direct_roll.get();
+				_attitude_setpoint.roll_body = _param_direct_roll.get();
-					_vehicle_attitude_sp.pitch_body = _param_direct_pitch.get();
+				_attitude_setpoint.pitch_body = _param_direct_pitch.get();
-					_vehicle_attitude_sp.yaw_body = _param_direct_yaw.get();
+				_attitude_setpoint.yaw_body = _param_direct_yaw.get();
-					_vehicle_attitude_sp.thrust_body[0] = _param_direct_thrust.get();
+				_attitude_setpoint.thrust_body[0] = _param_direct_thrust.get();
 			}
 
 			/* Geometric Control*/
-				control_attitude_geo(_vehicle_attitude, _vehicle_attitude_sp);
+			control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint);
 		}
 	}
 
-		loop_counter++;
 	perf_end(_loop_perf);
 
 	/* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */
-		if (fds[1].revents & POLLIN) {
+	if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) {
 		// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
 		// returning immediately and this loop will eat up resources.
-			orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
-
 		if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
 			/* manual/direct control */
 			constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x,
@@ -357,10 +254,6 @@ void UUVAttitudeControl::run()
 
 	}
 
-		if (fds[2].revents & POLLIN) {
-
-			orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
-
 	_actuators.timestamp = hrt_absolute_time();
 
 	/* Only publish if any of the proper modes are enabled */
@@ -370,16 +263,6 @@ void UUVAttitudeControl::run()
 		_actuator_controls_pub.publish(_actuators);
 
 	}
-		}
-	}
-
-	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);
-	orb_unsubscribe(_sensor_combined_sub);
 
 	warnx("exiting.\n");
 }

src/modules/uuv_att_control/uuv_att_control.hpp

@@ -59,26 +59,21 @@
 #include <px4_platform_common/tasks.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/position_controller_status.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/uORB.h>
 
-
-
-
 using matrix::Eulerf;
 using matrix::Quatf;
 using matrix::Matrix3f;
@@ -87,7 +82,7 @@ using matrix::Dcmf;
 
 using uORB::SubscriptionData;
 
-class UUVAttitudeControl: public ModuleBase<UUVAttitudeControl>, public ModuleParams
+class UUVAttitudeControl: public ModuleBase<UUVAttitudeControl>, public ModuleParams, public px4::WorkItem
 {
 public:
 	UUVAttitudeControl();
@@ -108,49 +103,29 @@ public:
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
 
-	/** @see ModuleBase::run() */
+	bool init();
-	void run() override;
-
-
-//	int start();
-//	bool task_running() { return _task_running; }
 
 private:
-	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};
 	uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)};
 
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 
-	int	_vehicle_attitude_sp_sub{-1};	/**< vehicle attitude setpoint */
+	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};	/**< 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 */
+	uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};	/**< vehicle angular velocity subscription */
-	int	_vehicle_attitude_sub{-1};	/**< control state subscription */
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};	/**< notification of manual control updates */
-	int	_angular_velocity_sub{-1};	/**< vehicle angular velocity subscription */
+	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle status subscription */
-	int	_local_pos_sub{-1};		/**< local position subscription */
+
-	int	_manual_control_setpoint_sub{-1};	/**< notification of manual control updates */
+	uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
-	int	_vcontrol_mode_sub{-1};		/**< vehicle status subscription */
-	int	_sensor_combined_sub{-1};	/**< sensor combined subscription */
 
 	actuator_controls_s _actuators {}; /**< actuator control inputs */
 	manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
-	vehicle_attitude_s		_vehicle_attitude {};	/**< control state */
+	vehicle_attitude_setpoint_s _attitude_setpoint {}; /**< vehicle attitude setpoint */
-	vehicle_angular_velocity_s	_angular_velocity{};	/**< angular velocity */
+	vehicle_rates_setpoint_s _rates_setpoint {}; /**< vehicle bodyrates setpoint */
-	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;
-
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::UUV_ROLL_P>) _param_roll_p,
 		(ParamFloat<px4::params::UUV_ROLL_D>) _param_roll_d,
@@ -167,26 +142,16 @@ private:
 		(ParamFloat<px4::params::UUV_DIRCT_THRUST>) _param_direct_thrust
 	)
 
+	void Run() override;
 	/**
 	 * Update our local parameter cache.
 	 */
 	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_rates_setpoint_poll();
-	void vehicle_local_position_poll();
-
 	/**
 	 * Control Attitude
 	 */
-	void control_attitude_geo(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
+	void control_attitude_geo(const vehicle_attitude_s &attitude, const vehicle_attitude_setpoint_s &attitude_setpoint,
-
+				  const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_s &rates_setpoint);
-	void control_attitude_pid(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp, float deltaT);
 	void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_u);
-
-
 };