update angular accelerations

Rebase fixes
PX4 · Mar 4, 2023 · 193858d · 193858d
1 parent 96f5edb
commit 193858d
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 22 deletions.
diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp
@@ -159,8 +159,8 @@ RoverPositionControl::manual_control_setpoint_poll()
 					// STABILIZED mode generate the attitude setpoint from manual user inputs
 					_rates_sp.roll = 0.0;
 					_rates_sp.pitch = 0.0;
-					_rates_sp.yaw = _manual_control_setpoint.y;
-					_rates_sp.thrust_body[0] = _manual_control_setpoint.z;
+					_rates_sp.yaw = _manual_control_setpoint.roll;
+					_rates_sp.thrust_body[0] = _manual_control_setpoint.throttle;
 
 					_rates_sp.timestamp = hrt_absolute_time();
 
@@ -218,14 +218,6 @@ RoverPositionControl::vehicle_attitude_poll()
 	}
 }
 
-void
-RoverPositionControl::vehicle_angular_acceleration_poll()
-{
-	if (_vehicle_angular_acceleration_sub.updated()) {
-		_vehicle_angular_acceleration_sub.copy(&_vehicle_angular_acceleration);
-	}
-}
-
 bool
 RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 				       const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
@@ -431,8 +423,7 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi
 }
 
 void
-RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const  vehicle_angular_acceleration_s &acc,
-				    const vehicle_local_position_s &local_pos,
+RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const vehicle_local_position_s &local_pos,
 				    const vehicle_rates_setpoint_s &rates_sp)
 {
 	float dt = (_control_rates_last_called > 0) ? hrt_elapsed_time(&_control_rates_last_called) * 1e-6f : 0.01f;
@@ -444,7 +435,7 @@ RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, con
 	const matrix::Vector3f current_velocity(local_pos.vx, local_pos.vy, local_pos.vz);
 	bool lock_integrator = bool(current_velocity.norm() < _param_rate_i_minspeed.get());
 
-	const matrix::Vector3f angular_acceleration{acc.xyz};
+	const matrix::Vector3f angular_acceleration{rates.xyz_derivative};
 	const matrix::Vector3f torque = _rate_control.update(vehicle_rates, rates_setpoint, angular_acceleration, dt,
 					lock_integrator);
 	///TODO: Handle mimimum speed constraints
@@ -468,8 +459,6 @@ RoverPositionControl::Run()
 
 	/* run controller on gyro changes */
 	if (_vehicle_angular_velocity_sub.update(&_vehicle_rates)) {
-		// grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy
-		vehicle_angular_acceleration_poll();
 		/* check vehicle control mode for changes to publication state */
 		vehicle_control_mode_poll();
 		attitude_setpoint_poll();
@@ -567,7 +556,7 @@ RoverPositionControl::Run()
 
 		//Body Rate control
 		if (_control_mode.flag_control_rates_enabled) {
-			control_rates(_vehicle_rates, _vehicle_angular_acceleration, _local_pos, _rates_sp);
+			control_rates(_vehicle_rates, _local_pos, _rates_sp);
 		}
 
 		/* Only publish if any of the proper modes are enabled */

diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp
@@ -69,7 +69,6 @@
 #include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
-#include <uORB/topics/vehicle_angular_acceleration.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
@@ -124,7 +123,6 @@ class RoverPositionControl final : public ModuleBase<RoverPositionControl>, publ
 	uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
 	uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
-	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
 	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 > */
@@ -134,7 +132,6 @@ class RoverPositionControl final : public ModuleBase<RoverPositionControl>, publ
 	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_acceleration_s		_vehicle_angular_acceleration{};
 	vehicle_angular_velocity_s _vehicle_rates{};
 
 	trajectory_setpoint_s _trajectory_setpoint{};
@@ -229,7 +226,6 @@ class RoverPositionControl final : public ModuleBase<RoverPositionControl>, publ
 	void		rates_setpoint_poll();
 	void		vehicle_control_mode_poll();
 	void 		vehicle_attitude_poll();
-	void		vehicle_angular_acceleration_poll();
 	void		manual_control_setpoint_poll();
 
 	/**
@@ -239,8 +235,7 @@ class RoverPositionControl final : public ModuleBase<RoverPositionControl>, publ
 					 const position_setpoint_triplet_s &_pos_sp_triplet);
 	void		control_velocity(const matrix::Vector3f &current_velocity);
 	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  vehicle_angular_acceleration_s &acc,
-				      const vehicle_local_position_s &local_pos,
+	void		control_rates(const vehicle_angular_velocity_s &rates, const vehicle_local_position_s &local_pos,
 				      const vehicle_rates_setpoint_s &rates_sp);
 
 };