rate ctrl: use inertia, ang.accel sp and torque sp

src/modules/mc_att_control/RateControl/RateControl.cpp

@@ -72,8 +72,8 @@ void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &
 	_mixer_saturation_negative[2] = status.flags.yaw_neg;
 }
 
-Vector3f RateControl::update(const Vector3f rate, const Vector3f rate_sp, const float dt, const bool landed,
-			     const float thrust_sp)
+void RateControl::update(const Vector3f rate, const Vector3f rate_sp, const float dt, const bool landed,
+			 const float thrust_sp)
 {
 	Vector3f gain_p_tpa = _gain_p.emult(tpa_attenuations(_tpa_breakpoint(0), _tpa_rate(0), thrust_sp));
 	Vector3f gain_i_tpa = _gain_i.emult(tpa_attenuations(_tpa_breakpoint(1), _tpa_rate(1), thrust_sp));
@@ -90,18 +90,35 @@ Vector3f RateControl::update(const Vector3f rate, const Vector3f rate_sp, const
 		rate_d = (rate_filtered - _rate_prev_filtered) / dt;
 	}
 
-	// PID control with feed forward
-	Vector3f torque = gain_p_tpa.emult(rate_error) + _rate_int - gain_d_tpa.emult(rate_d) + _gain_ff.emult(rate_sp);
-
+	// P + D control
+	_angular_accel_sp = gain_p_tpa.emult(rate_error) - gain_d_tpa.emult(rate_d);
+
+	// I + FF control
+	Vector3f torque_feedforward = _rate_int + _gain_ff.emult(rate_sp);
+
+	// compute torque setpoint
+	// Note: this may go into a separate module for general usage with FW and VTOLs
+	// if so, TODO:
+	// - [x] publish accel sp
+	// - [ ] publish torque ff sp
+	// - [ ] add dynamic model module
+	// - [ ] move params for inertia to that module
+	// - [ ] poll vehicle_angular_velocity & vehicle_angular_acceleration_setpoint & vehicle_torque_feedforward_setpoint => compute and publish vehicle_torque_setpoint
+	// - [ ] (eventually) add param for mass, poll vehicle_linear_acceleration_setpoint + vehicle_attitude => compute and publish vehicle_thrust_setpoint
+	_torque_sp = (
+			     _inertia * _angular_accel_sp
+			     + torque_feedforward
+			     + rate.cross(_inertia * rate)
+		     );
+
+	// save states
 	_rate_prev = rate;
 	_rate_prev_filtered = rate_filtered;
 
 	// update integral only if we are not landed
 	if (!landed) {
 		updateIntegral(rate_error, dt, gain_i_tpa);
 	}
-
-	return torque;
 }
 
 void RateControl::updateIntegral(Vector3f &rate_error, const float dt, const Vector3f &gain_i_tpa)
@@ -136,6 +153,7 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt, const Vec
 	}
 }
 
+
 void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
 {
 	rate_ctrl_status.rollspeed_integ = _rate_int(0);

src/modules/mc_att_control/RateControl/RateControl.hpp

@@ -106,10 +106,21 @@ class RateControl
 	 * @param rate_sp desired vehicle angular rate setpoint
 	 * @param dt desired vehicle angular rate setpoint
 	 * @param thrust_sp total thrust setpoint to be used for TPA
-	 * @return [-1,1] normalized torque vector to apply to the vehicle
 	 */
-	matrix::Vector3f update(const matrix::Vector3f rate, const matrix::Vector3f rate_sp, const float dt, const bool landed,
-				const float thrust_sp);
+	void update(const matrix::Vector3f rate, const matrix::Vector3f rate_sp, const float dt, const bool landed,
+		    const float thrust_sp);
+
+	/**
+	 * Get the desired angular acceleration
+	 * @see _angular_accel_sp
+	 */
+	const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;};
+
+	/**
+	 * Get the torque vector to apply to the vehicle
+	 * @see _torque_sp
+	 */
+	const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;};
 
 	/**
 	 * Set the integral term to 0 to prevent windup
@@ -142,6 +153,10 @@ class RateControl
 	bool _mixer_saturation_positive[3] {};
 	bool _mixer_saturation_negative[3] {};
 
+	// Output
+	matrix::Vector3f _angular_accel_sp; 	//< Angular acceleration setpoint computed using P and D gains
+	matrix::Vector3f _torque_sp;		//< Torque setpoint to apply to the vehicle
+
 	/*
 	* Throttle PID attenuation
 	* Lowers the overall gain of the PID controller linearly depending on total thrust.

src/modules/mc_att_control/RateControl/RateControlTest.cpp

@@ -39,6 +39,7 @@ using namespace matrix;
 TEST(RateControlTest, AllZeroCase)
 {
 	RateControl rate_control;
-	Vector3f torque = rate_control.update(Vector3f(), Vector3f(), 0.f, false, 0.f);
+	rate_control.update(Vector3f(), Vector3f(), 0.f, false, 0.f);
+	Vector3f torque = rate_control.getTorqueSetpoint();
 	EXPECT_EQ(torque, Vector3f());
 }

src/modules/mc_att_control/mc_att_control.hpp

@@ -50,12 +50,15 @@
 #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_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/landing_gear.h>
 #include <vtol_att_control/vtol_type.h>
@@ -107,6 +110,9 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 	void		vehicle_motor_limits_poll();
 	void		vehicle_status_poll();
 
+	void		publish_angular_acceleration_setpoint();
+	void		publish_torque_setpoint();
+	void		publish_thrust_setpoint();
 	void		publish_actuator_controls();
 	void		publish_rates_setpoint();
 	void		publish_rate_controller_status();
@@ -158,8 +164,11 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
 
 	uORB::PublicationMulti<rate_ctrl_status_s>	_controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT};	/**< controller status publication */
-	uORB::Publication<landing_gear_s>		_landing_gear_pub{ORB_ID(landing_gear)};
+	uORB::Publication<landing_gear_s>		_landing_gear_pub{ORB_ID(landing_gear)};				/**< landing gear publication */
 	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};			/**< rate setpoint publication */
+	uORB::Publication<vehicle_angular_acceleration_setpoint_s> _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)};	/**< angular acceleration setpoint publication */
+	uORB::Publication<vehicle_thrust_setpoint_s>		   _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};				/**< thrust setpoint publication */
+	uORB::Publication<vehicle_torque_setpoint_s>		   _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};				/**< torque setpoint publication */
 
 	orb_advert_t	_actuators_0_pub{nullptr};		/**< attitude actuator controls publication */
 	orb_advert_t	_vehicle_attitude_setpoint_pub{nullptr};
@@ -188,15 +197,15 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 	float _loop_update_rate_hz{initial_update_rate_hz};          /**< current rate-controller loop update rate in [Hz] */
 
 	matrix::Vector3f _rates_sp;			/**< angular rates setpoint */
-
-	matrix::Vector3f _att_control;			/**< attitude control vector */
-	float		_thrust_sp{0.0f};		/**< thrust setpoint */
+	matrix::Vector3f _torque_sp;			/**< torque setpoint */
+	float		 _thrust_sp{0.0f};		/**< thrust setpoint */
 
 	float _man_yaw_sp{0.f};				/**< current yaw setpoint in manual mode */
 	bool _gear_state_initialized{false};		/**< true if the gear state has been initialized */
 
 	hrt_abstime _task_start{hrt_absolute_time()};
 	hrt_abstime _last_run{0};
+	hrt_abstime _timestamp_sample{0};
 	float _dt_accumulator{0.0f};
 	int _loop_counter{0};
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -66,8 +66,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_v_att_sp.q_d[0] = 1.f;
 
 	_rates_sp.zero();
+	_torque_sp.zero();
 	_thrust_sp = 0.0f;
-	_att_control.zero();
 
 	parameters_updated();
 }
@@ -373,7 +373,7 @@ MulticopterAttitudeControl::control_attitude()
 /*
  * Attitude rates controller.
  * Input: '_rates_sp' vector, '_thrust_sp'
- * Output: '_att_control' vector
+ * Output: '_torque_sp' vector
  */
 void
 MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rates)
@@ -385,7 +385,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rat
 
 	const bool landed = _vehicle_land_detected.maybe_landed || _vehicle_land_detected.landed;
 	_rate_control.setSaturationStatus(_saturation_status);
-	_att_control = _rate_control.update(rates, _rates_sp, dt, landed, _thrust_sp);
+	_rate_control.update(rates, _rates_sp, dt, landed, _thrust_sp);
+
+	_torque_sp = _rate_control.getTorqueSetpoint();
 }
 
 void
@@ -414,13 +416,15 @@ MulticopterAttitudeControl::publish_rate_controller_status()
 void
 MulticopterAttitudeControl::publish_actuator_controls()
 {
-	_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
-	_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
-	_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
+	matrix::Vector3f act_controls =  _torque_sp;
+
+	_actuators.timestamp = hrt_absolute_time();
+	_actuators.timestamp_sample = _timestamp_sample;
+	_actuators.control[0] = (PX4_ISFINITE(act_controls(0))) ? act_controls(0) : 0.0f;
+	_actuators.control[1] = (PX4_ISFINITE(act_controls(1))) ? act_controls(1) : 0.0f;
+	_actuators.control[2] = (PX4_ISFINITE(act_controls(2))) ? act_controls(2) : 0.0f;
 	_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
 	_actuators.control[7] = (float)_landing_gear.landing_gear;
-	// note: _actuators.timestamp_sample is set in MulticopterAttitudeControl::Run()
-	_actuators.timestamp = hrt_absolute_time();
 
 	/* scale effort by battery status */
 	if (_param_mc_bat_scale_en.get() && _battery_status.scale > 0.0f) {
@@ -434,6 +438,53 @@ MulticopterAttitudeControl::publish_actuator_controls()
 	}
 }
 
+void
+MulticopterAttitudeControl::publish_angular_acceleration_setpoint()
+{
+	Vector3f angular_accel_sp = _rate_control.getAngularAccelerationSetpoint();
+
+	vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
+	v_angular_accel_sp.timestamp = hrt_absolute_time();
+	v_angular_accel_sp.timestamp_sample = _timestamp_sample;
+	v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
+	v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
+	v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
+
+	if (!_vehicle_status.is_vtol) {
+		_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::publish_torque_setpoint()
+{
+	vehicle_torque_setpoint_s v_torque_sp = {};
+	v_torque_sp.timestamp = hrt_absolute_time();
+	v_torque_sp.timestamp_sample = _timestamp_sample;
+	v_torque_sp.xyz[0] = (PX4_ISFINITE(_torque_sp(0))) ? _torque_sp(0) : 0.0f;
+	v_torque_sp.xyz[1] = (PX4_ISFINITE(_torque_sp(1))) ? _torque_sp(1) : 0.0f;
+	v_torque_sp.xyz[2] = (PX4_ISFINITE(_torque_sp(2))) ? _torque_sp(2) : 0.0f;
+
+	if (!_vehicle_status.is_vtol) {
+		_vehicle_torque_setpoint_pub.publish(v_torque_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::publish_thrust_setpoint()
+{
+	vehicle_thrust_setpoint_s v_thrust_sp = {};
+	v_thrust_sp.timestamp = hrt_absolute_time();
+	v_thrust_sp.timestamp_sample = _timestamp_sample;
+	v_thrust_sp.xyz[0] = 0.0f;
+	v_thrust_sp.xyz[1] = 0.0f;
+	v_thrust_sp.xyz[2] = (PX4_ISFINITE(-_thrust_sp)) ? (-_thrust_sp) : 0.0f;
+
+	if (!_vehicle_status.is_vtol) {
+		_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
+	}
+}
+
 void
 MulticopterAttitudeControl::Run()
 {
@@ -457,13 +508,18 @@ MulticopterAttitudeControl::Run()
 
 		const Vector3f rates{angular_velocity.xyz};
 
-		_actuators.timestamp_sample = angular_velocity.timestamp_sample;
+		_timestamp_sample = angular_velocity.timestamp_sample;
 
 		/* run the rate controller immediately after a gyro update */
 		if (_v_control_mode.flag_control_rates_enabled) {
 			control_attitude_rates(dt, rates);
 
 			publish_actuator_controls();
+
+			publish_angular_acceleration_setpoint();
+			publish_torque_setpoint();
+			publish_thrust_setpoint();
+
 			publish_rate_controller_status();
 		}
 
@@ -552,7 +608,7 @@ MulticopterAttitudeControl::Run()
 				_rates_sp.zero();
 				_rate_control.resetIntegral();
 				_thrust_sp = 0.0f;
-				_att_control.zero();
+				_torque_sp.zero();
 				publish_actuator_controls();
 			}
 		}