mc_rate_control: publish angular acceleration setpoint

msg/CMakeLists.txt

@@ -136,6 +136,7 @@ set(msg_files
 	vehicle_acceleration.msg
 	vehicle_air_data.msg
 	vehicle_angular_acceleration.msg
+	vehicle_angular_acceleration_setpoint.msg
 	vehicle_angular_velocity.msg
 	vehicle_attitude.msg
 	vehicle_attitude_setpoint.msg

msg/tools/uorb_rtps_message_ids.yaml

@@ -279,6 +279,8 @@ rtps:
     id: 124
   - msg: vehicle_angular_acceleration
     id: 125
+  - msg: vehicle_angular_acceleration_setpoint
+    id: 126
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

msg/vehicle_angular_acceleration_setpoint.msg

@@ -0,0 +1,4 @@
+
+uint64 timestamp        # time since system start (microseconds)
+
+float32[3] xyz          # angular acceleration about X, Y, Z body axis in rad/s^2

src/modules/logger/logged_topics.cpp

@@ -119,6 +119,8 @@ void LoggedTopics::add_default_topics()
 	add_topic("fw_virtual_attitude_setpoint");
 	add_topic("mc_virtual_attitude_setpoint");
 	add_topic("time_offset");
+	add_topic("vehicle_angular_acceleration", 10);
+	add_topic("vehicle_angular_acceleration_setpoint", 10);
 	add_topic("vehicle_angular_velocity", 10);
 	add_topic("vehicle_attitude_groundtruth", 10);
 	add_topic("vehicle_global_position_groundtruth", 100);
@@ -134,6 +136,8 @@ void LoggedTopics::add_high_rate_topics()
 	add_topic("manual_control_setpoint");
 	add_topic("rate_ctrl_status");
 	add_topic("sensor_combined");
+	add_topic("vehicle_angular_acceleration");
+	add_topic("vehicle_angular_acceleration_setpoint");
 	add_topic("vehicle_angular_velocity");
 	add_topic("vehicle_attitude");
 	add_topic("vehicle_attitude_setpoint");

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -277,6 +277,12 @@ MulticopterRateControl::Run()
 			// run rate controller
 			const Vector3f att_control = _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed);
 
+			// publish angular acceleration setpoint
+			vehicle_angular_acceleration_setpoint_s v_angular_accel_sp;
+			_rate_control.getAngularAccelerationSetpoint().copyTo(v_angular_accel_sp.xyz);
+			v_angular_accel_sp.timestamp = hrt_absolute_time();
+			_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
+
 			// publish rate controller status
 			rate_ctrl_status_s rate_ctrl_status{};
 			_rate_control.getRateControlStatus(rate_ctrl_status);

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -54,6 +54,7 @@
 #include <uORB/topics/multirotor_motor_limits.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_land_detected.h>
@@ -107,6 +108,7 @@ class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public
 
 	uORB::Publication<actuator_controls_s>		_actuators_0_pub;
 	uORB::PublicationMulti<rate_ctrl_status_s>	_controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT};	/**< controller status publication */
+	uORB::Publication<vehicle_angular_acceleration_setpoint_s> _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)};
 	uORB::Publication<landing_gear_s>		_landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};			/**< rate setpoint publication */
 

src/modules/mc_rate_control/RateControl/RateControl.cpp

@@ -79,9 +79,13 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
 		rate_d = (rate_filtered - _rate_prev_filtered) / dt;
 	}
 
-	// PID control with feed forward
-	const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp);
+	// P + D control
+	_angular_accel_sp = _gain_p.emult(rate_error) - _gain_d.emult(rate_d);
 
+	// I + FF control
+	const Vector3f torque_feedforward = _rate_int + _gain_ff.emult(rate_sp);
+
+	// save states
 	_rate_prev = rate;
 	_rate_prev_filtered = rate_filtered;
 
@@ -90,7 +94,7 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
 		updateIntegral(rate_error, dt);
 	}
 
-	return torque;
+	return _angular_accel_sp + torque_feedforward;
 }
 
 void RateControl::updateIntegral(Vector3f &rate_error, const float dt)

src/modules/mc_rate_control/RateControl/RateControl.hpp

@@ -96,6 +96,12 @@ class RateControl
 	matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt,
 				const bool landed);
 
+	/**
+	 * Get the desired angular acceleration
+	 * @see _angular_accel_sp
+	 */
+	const matrix::Vector3f &getAngularAccelerationSetpoint() const { return _angular_accel_sp; };
+
 	/**
 	 * Set the integral term to 0 to prevent windup
 	 * @see _rate_int
@@ -125,4 +131,7 @@ class RateControl
 	math::LowPassFilter2pVector3f _lp_filters_d{0.f, 0.f}; ///< low-pass filters for D-term (roll, pitch & yaw)
 	bool _mixer_saturation_positive[3] {};
 	bool _mixer_saturation_negative[3] {};
+
+	// Output
+	matrix::Vector3f _angular_accel_sp{0.f, 0.f, 0.f}; //< Angular acceleration setpoint computed using P and D gains
 };