[WIP]: Rate control: introduce torque_setpoint, angular_accel_setpoint, and inertia matrix

ROMFS/px4fmu_common/init.d-posix/10016_iris

@@ -9,5 +9,18 @@
 
 sh /etc/init.d/rc.mc_defaults
 
+if [ $AUTOCNF = yes ]
+then
+	param set MC_ROLLRATE_P 18.0
+	param set MC_ROLLRATE_D 0.36
+	param set MC_PITCHRATE_P 18.0
+	param set MC_PITCHRATE_D 0.36
+	param set MC_YAWRATE_P 7.0
+	param set MC_YAWRATE_D 0.0
+
+	param set VM_INERTIA_XX 0.0083
+	param set VM_INERTIA_YY 0.0083
+	param set VM_INERTIA_ZZ 0.0286
+fi
 set MIXER quad_w
 

boards/intel/aerofc-v1/default.cmake

@@ -44,7 +44,7 @@ px4_add_board(
 		land_detector
 		landing_target_estimator
 		load_mon
-		local_position_estimator
+		#local_position_estimator
 		logger
 		mavlink
 		mc_att_control

boards/omnibus/f4sd/default.cmake

@@ -89,7 +89,7 @@ px4_add_board(
 		shutdown
 		#tests # tests and test runner
 		top
-		topic_listener
+		#topic_listener
 		tune_control
 		usb_connected
 		ver

boards/px4/fmu-v2/default.cmake

@@ -53,7 +53,7 @@ px4_add_board(
 		#pca9685
 		#protocol_splitter
 		#pwm_input
-		pwm_out_sim
+		#pwm_out_sim
 		px4fmu
 		px4io
 		#roboclaw

msg/CMakeLists.txt

@@ -130,6 +130,8 @@ set(msg_files
 	ulog_stream_ack.msg
 	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
@@ -148,6 +150,8 @@ set(msg_files
 	vehicle_roi.msg
 	vehicle_status.msg
 	vehicle_status_flags.msg
+	vehicle_thrust_setpoint.msg
+	vehicle_torque_setpoint.msg
 	vehicle_trajectory_waypoint.msg
 	vtol_vehicle_status.msg
 	wheel_encoders.msg

msg/tools/uorb_rtps_message_ids.yaml

@@ -265,6 +265,14 @@ rtps:
     id: 116
   - msg: cellular_status
     id: 117
+  - msg: vehicle_angular_acceleration
+    id: 118
+  - msg: vehicle_angular_acceleration_setpoint
+    id: 119
+  - msg: vehicle_torque_setpoint
+    id: 120
+  - msg: vehicle_thrust_setpoint
+    id: 121
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

msg/vehicle_angular_acceleration.msg

@@ -0,0 +1,7 @@
+
+uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz		# angular acceleration about X, Y, Z body axis in rad/s^2,
+			# computed by differentiating vehicle_angular_velocity and applying a low pass filter
+			# can be used to verify that the vehicle correctly tracks angular acceleration setpoints

msg/vehicle_angular_acceleration_setpoint.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz		# angular acceleration about X, Y, Z body axis in rad/s^2

msg/vehicle_thrust_setpoint.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz		# thrust setpoint along X, Y, Z body axis (in N)

msg/vehicle_torque_setpoint.msg

@@ -0,0 +1,5 @@
+
+uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz		# torque setpoint about X, Y, Z body axis (in N.m)

src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp

@@ -43,6 +43,7 @@ namespace math
 void LowPassFilter2pVector3f::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 {
 	_cutoff_freq = cutoff_freq;
+	_sample_freq = sample_freq;
 
 	// reset delay elements on filter change
 	_delay_element_1.zero();

src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp

@@ -74,12 +74,16 @@ class LowPassFilter2pVector3f
 	// Return the cutoff frequency
 	float get_cutoff_freq() const { return _cutoff_freq; }
 
+	// Return the sample frequency
+	float get_sample_freq() const { return _sample_freq; }
+
 	// Reset the filter state to this value
 	matrix::Vector3f reset(const matrix::Vector3f &sample);
 
 private:
 
 	float _cutoff_freq{0.0f};
+	float _sample_freq{0.0f};
 
 	float _a1{0.0f};
 	float _a2{0.0f};

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -655,6 +655,28 @@ void FixedwingAttitudeControl::Run()
 			}
 
 			_actuators_2_pub.publish(_actuators_airframe);
+
+			/* publish thrust and torque setpoint */
+			if (!_vehicle_status.is_vtol
+			    || (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING))
+			   ) {
+				vehicle_torque_setpoint_s v_torque_sp = {};
+				v_torque_sp.timestamp = hrt_absolute_time();
+				v_torque_sp.timestamp_sample = _att.timestamp;
+				v_torque_sp.xyz[0] = (PX4_ISFINITE(_actuators.control[0])) ? _actuators.control[0] : 0.0f;
+				v_torque_sp.xyz[1] = (PX4_ISFINITE(_actuators.control[1])) ? _actuators.control[1] : 0.0f;
+				v_torque_sp.xyz[2] = (PX4_ISFINITE(_actuators.control[2])) ? _actuators.control[2] : 0.0f;
+				_vehicle_torque_setpoint_pub.publish(v_torque_sp);
+
+				vehicle_thrust_setpoint_s v_thrust_sp = {};
+				v_thrust_sp.timestamp = hrt_absolute_time();
+				v_thrust_sp.timestamp_sample = _att.timestamp;
+				v_thrust_sp.xyz[0] = (PX4_ISFINITE(_actuators.control[3])) ? _actuators.control[3] : 0.0f;
+				v_thrust_sp.xyz[1] = 0.0f;
+				v_thrust_sp.xyz[2] = 0.0f;
+				_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
+
+			}
 		}
 	}
 

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -66,6 +66,8 @@
 #include <uORB/topics/vehicle_land_detected.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>
 
 using matrix::Eulerf;
 using matrix::Quatf;
@@ -115,6 +117,8 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 	uORB::Publication<actuator_controls_s>		_actuators_2_pub{ORB_ID(actuator_controls_2)};		/**< actuator control group 1 setpoint (Airframe) */
 	uORB::Publication<vehicle_rates_setpoint_s>	_rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};		/**< rate setpoint publication */
 	uORB::PublicationMulti<rate_ctrl_status_s>	_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};	/**< rate controller status publication */
+	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};		/**< vehicle_thrust_setpoint publication */
+	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};		/**< vehicle_torque_setpoint publication */
 
 	orb_id_t	_attitude_setpoint_id{nullptr};
 	orb_advert_t	_attitude_sp_pub{nullptr};	/**< attitude setpoint point */

src/modules/logger/logger.cpp

@@ -653,6 +653,9 @@ void Logger::add_system_identification_topics()
 	add_topic("actuator_controls_0");
 	add_topic("actuator_controls_1");
 	add_topic("sensor_combined");
+	add_topic("vehicle_angular_acceleration");
+	add_topic("vehicle_angular_acceleration_setpoint");
+	add_topic("vehicle_torque_setpoint");
 }
 
 int Logger::add_topics_from_file(const char *fname)

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -94,6 +94,14 @@ MulticopterRateControl::parameters_updated()
 	_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()),
 				  radians(_param_mc_acro_y_max.get()));
 
+	// inertia matrix
+	const float inertia[3][3] = {
+		{_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()},
+		{_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()},
+		{_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()}
+	};
+	_rate_control.setInertiaMatrix(matrix::Matrix3f(inertia));
+
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
 }
 
@@ -164,6 +172,7 @@ MulticopterRateControl::Run()
 
 	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
 		const hrt_abstime now = hrt_absolute_time();
+		_timestamp_sample = angular_velocity.timestamp_sample;
 
 		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
 		const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
@@ -271,7 +280,7 @@ MulticopterRateControl::Run()
 		}
 
 		// run the rate controller
-		if (_v_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
+		if (_v_control_mode.flag_control_rates_enabled) {
 
 			// reset integral if disarmed
 			if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
@@ -291,56 +300,126 @@ MulticopterRateControl::Run()
 			}
 
 			// run rate controller
-			const Vector3f att_control = _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed);
+			_rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed);
 
 			// publish rate controller status
 			rate_ctrl_status_s rate_ctrl_status{};
 			_rate_control.getRateControlStatus(rate_ctrl_status);
 			rate_ctrl_status.timestamp = hrt_absolute_time();
 			_controller_status_pub.publish(rate_ctrl_status);
 
-			// publish actuator controls
-			actuator_controls_s actuators{};
-			actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
-			actuators.timestamp_sample = angular_velocity.timestamp_sample;
-
-			// scale effort by battery status if enabled
-			if (_param_mc_bat_scale_en.get()) {
-				if (_battery_status_sub.updated()) {
-					battery_status_s battery_status;
-
-					if (_battery_status_sub.copy(&battery_status)) {
-						_battery_status_scale = battery_status.scale;
-					}
-				}
-
-				if (_battery_status_scale > 0.0f) {
-					for (int i = 0; i < 4; i++) {
-						actuators.control[i] *= _battery_status_scale;
-					}
-				}
+			// publish controller output
+			if (!_vehicle_status.is_vtol
+			    || (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING))
+			    || (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode)) {
+				publish_actuator_controls();
+				publish_angular_acceleration_setpoint();
+				publish_torque_setpoint();
+				publish_thrust_setpoint();
 			}
 
-			actuators.timestamp = hrt_absolute_time();
-			orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
-
 		} else if (_v_control_mode.flag_control_termination_enabled) {
 			if (!_vehicle_status.is_vtol) {
-				// publish actuator controls
-				actuator_controls_s actuators{};
-				actuators.timestamp = hrt_absolute_time();
-				orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
+				// reset controller
+				_rate_control.reset();
+
+				// publish controller output
+				publish_actuator_controls();
+				publish_angular_acceleration_setpoint();
+				publish_torque_setpoint();
+				publish_thrust_setpoint();
 			}
 		}
 	}
 
 	perf_end(_loop_perf);
 }
 
+void
+MulticopterRateControl::publish_actuator_controls()
+{
+	Vector3f act_control = _rate_control.getTorqueSetpoint();
+
+	actuator_controls_s actuators{};
+	actuators.timestamp = hrt_absolute_time();
+	actuators.timestamp_sample = _timestamp_sample;
+	actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(act_control(0)) ? act_control(0) : 0.0f;
+	actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(act_control(1)) ? act_control(1) : 0.0f;
+	actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(act_control(2)) ? act_control(2) : 0.0f;
+	actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
+	actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
+
+	// scale effort by battery status if enabled
+	if (_param_mc_bat_scale_en.get()) {
+		if (_battery_status_sub.updated()) {
+			battery_status_s battery_status;
+
+			if (_battery_status_sub.copy(&battery_status)) {
+				_battery_status_scale = battery_status.scale;
+			}
+		}
+
+		if (_battery_status_scale > 0.0f) {
+			for (int i = 0; i < 4; i++) {
+				actuators.control[i] *= _battery_status_scale;
+			}
+		}
+	}
+
+	if (!_actuators_0_circuit_breaker_enabled) {
+		orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
+	}
+}
+
+void
+MulticopterRateControl::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
+MulticopterRateControl::publish_torque_setpoint()
+{
+	Vector3f torque_sp = _rate_control.getTorqueSetpoint();
+
+	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
+MulticopterRateControl::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);
+	}
+}
+
 int MulticopterRateControl::task_spawn(int argc, char *argv[])
 {
 	MulticopterRateControl *instance = new MulticopterRateControl();