Fixup vehicle_model

This module estimates the vehicle mass and inertia matrix.

It uses a least mean square algorithm between
delayed, low pass filtered, and time differentiated setpoints
(thrust and torque, respectively),
and low pass filtered and time differentiated accelerations
(linear and angular, respectively).

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

@@ -9,5 +9,19 @@
 
 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
 
+vehicle_model_estimator start

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

@@ -88,7 +88,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

@@ -48,12 +48,12 @@ px4_add_board(
 		magnetometer/hmc5883
 		#mkblctrl
 		#optical_flow # all available optical flow drivers
-		optical_flow/px4flow
+		#optical_flow/px4flow
 		#osd
 		#pca9685
 		#protocol_splitter
 		#pwm_input
-		pwm_out_sim
+		#pwm_out_sim
 		px4fmu
 		px4io
 		#roboclaw

boards/px4/sitl/default.cmake

@@ -43,6 +43,7 @@ px4_add_board(
 		sensors
 		#sih
 		simulator
+		vehicle_model_estimator
 		vmount
 		vtol_att_control
 	SYSTEMCMDS

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
@@ -143,11 +145,14 @@ set(msg_files
 	vehicle_local_position.msg
 	vehicle_local_position_setpoint.msg
 	vehicle_magnetometer.msg
+	vehicle_model.msg
 	vehicle_odometry.msg
 	vehicle_rates_setpoint.msg
 	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_model.msg

@@ -0,0 +1,6 @@
+# Vehicle model, mass and inertia matrix
+
+uint64 timestamp		# time since system start (microseconds)
+
+float32 mass			# vehicle mass
+float32[9] inertia		# vehicle inertia matrix

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 @@ public:
 	// 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 @@ private:
 	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,10 @@ 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");
+	add_topic("vehicle_model");
 }
 
 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,7 +300,7 @@ 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{};
@@ -299,41 +308,26 @@ MulticopterRateControl::Run()
 			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();
 			}
 		}
 	}
@@ -341,6 +335,91 @@ MulticopterRateControl::Run()
 	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();

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -54,11 +54,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_control_mode.h>
 #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>
+
 
 class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public ModuleParams, public px4::WorkItem
 {
@@ -98,6 +102,11 @@ private:
 	 */
 	float		get_landing_gear_state();
 
+	void		publish_angular_acceleration_setpoint();
+	void		publish_torque_setpoint();
+	void		publish_thrust_setpoint();
+	void		publish_actuator_controls();
+
 	RateControl _rate_control; ///< class for rate control calculations
 
 	uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};		/**< vehicle rates setpoint subscription */
@@ -115,6 +124,9 @@ private:
 	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<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_id_t _actuators_id{nullptr};	/**< pointer to correct actuator controls0 uORB metadata structure */
@@ -143,6 +155,7 @@ private:
 
 	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};
 
@@ -184,7 +197,14 @@ private:
 
 		(ParamBool<px4::params::MC_BAT_SCALE_EN>) _param_mc_bat_scale_en,
 
-		(ParamInt<px4::params::CBRK_RATE_CTRL>) _param_cbrk_rate_ctrl
+		(ParamInt<px4::params::CBRK_RATE_CTRL>) _param_cbrk_rate_ctrl,
+
+		(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx,			/**< vehicle inertia */
+		(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
+		(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
+		(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
+		(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
+		(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz
 	)
 
 	matrix::Vector3f _acro_rate_max;	/**< max attitude rates in acro mode */

src/modules/mc_rate_control/RateControl/RateControl.cpp

@@ -66,7 +66,7 @@ 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)
+void RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const float dt, const bool landed)
 {
 	// angular rates error
 	Vector3f rate_error = rate_sp - rate;
@@ -79,9 +79,28 @@ 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
+	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;
 
@@ -89,8 +108,6 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
 	if (!landed) {
 		updateIntegral(rate_error, dt);
 	}
-
-	return torque;
 }
 
 void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
@@ -125,6 +142,15 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
 	}
 }
 
+void RateControl::reset()
+{
+	_rate_int.zero();
+	_rate_prev.zero();
+	_rate_prev_filtered.zero();
+	_torque_sp.zero();
+	_angular_accel_sp.zero();
+}
+
 void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
 {
 	rate_ctrl_status.rollspeed_integ = _rate_int(0);

src/modules/mc_rate_control/RateControl/RateControl.hpp

@@ -80,6 +80,13 @@ public:
 	 */
 	void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
 
+	/**
+	 * Set inertia matrix
+	 * @see _inertia
+	 * @param inertia inertia matrix
+	 */
+	void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; };
+
 	/**
 	 * Set saturation status
 	 * @param status message from mixer reporting about saturation
@@ -91,10 +98,20 @@ public:
 	 * @param rate estimation of the current vehicle angular rate
 	 * @param rate_sp desired vehicle angular rate setpoint
 	 * @param dt desired vehicle angular rate setpoint
-	 * @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);
+	void 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() {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
@@ -102,6 +119,11 @@ public:
 	 */
 	void resetIntegral() { _rate_int.zero(); }
 
+	/**
+	 * ReSet the controller state
+	 */
+	void reset();
+
 	/**
 	 * Get status message of controller for logging/debugging
 	 * @param rate_ctrl_status status message to fill with internal states
@@ -117,6 +139,7 @@ private:
 	matrix::Vector3f _gain_d; ///< rate control derivative gain
 	matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
 	matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
+	matrix::Matrix3f _inertia{matrix::eye<float, 3>()}; ///< inertia matrix
 
 	// States
 	matrix::Vector3f _rate_prev; ///< angular rates of previous update
@@ -125,4 +148,8 @@ private:
 	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; 	//< Angular acceleration setpoint computed using P and D gains
+	matrix::Vector3f _torque_sp;		//< Torque setpoint to apply to the vehicle
 };

src/modules/mc_rate_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);
+	rate_control.update(Vector3f(), Vector3f(), 0.f, false);
+	Vector3f torque = rate_control.getTorqueSetpoint();
 	EXPECT_EQ(torque, Vector3f());
 }

src/modules/mc_rate_control/mc_rate_control_params.c

@@ -412,4 +412,3 @@ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
  * @group Multicopter Rate Control
  */
 PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f);
-

src/modules/sensors/CMakeLists.txt

@@ -33,6 +33,7 @@
 
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
+add_subdirectory(vehicle_angular_acceleration)
 
 px4_add_module(
 	MODULE modules__sensors
@@ -54,4 +55,5 @@ px4_add_module(
 		mathlib
 		vehicle_acceleration
 		vehicle_angular_velocity
+		vehicle_angular_acceleration
 	)

src/modules/sensors/parameters.cpp

@@ -155,6 +155,8 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
 	parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
 	parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
 
+	parameter_handles.imu_dgyro_en = param_find("IMU_DGYRO_EN");
+
 	// These are parameters for which QGroundControl always expects to be returned in a list request.
 	// We do a param_find here to force them into the list.
 	(void)param_find("RC_CHAN_CNT");
@@ -385,6 +387,8 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
 	param_get(parameter_handles.air_tube_length, &parameters.air_tube_length);
 	param_get(parameter_handles.air_tube_diameter_mm, &parameters.air_tube_diameter_mm);
 
+	param_get(parameter_handles.imu_dgyro_en, &parameters.imu_dgyro_en);
+
 	return ret;
 }
 

src/modules/sensors/parameters.h

@@ -143,6 +143,8 @@ struct Parameters {
 	int32_t air_cmodel;
 	float air_tube_length;
 	float air_tube_diameter_mm;
+
+	int32_t imu_dgyro_en;
 };
 
 struct ParameterHandles {
@@ -222,6 +224,7 @@ struct ParameterHandles {
 	param_t air_tube_length;
 	param_t air_tube_diameter_mm;
 
+	param_t imu_dgyro_en;
 };
 
 /**

src/modules/sensors/sensor_params.c

@@ -227,6 +227,31 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
 */
 PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
 
+/**
+* Enable computation of angular acceleration by gyro differentiation.
+*
+* @reboot_required true
+*
+* @boolean
+* @group Sensors
+*/
+PARAM_DEFINE_INT32(IMU_DGYRO_EN, 1);
+
+/**
+* Cutoff frequency for angular acceleration
+*
+* The cutoff frequency for the 2nd order butterworth filter used on
+* the time derivative of the measured angular velocity.
+* Set to 0 to disable the filter.
+*
+* @min 0
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);
+
 /**
 * Gyro control data maximum publication rate
 *

src/modules/sensors/sensors.cpp

@@ -92,6 +92,7 @@
 
 #include "vehicle_acceleration/VehicleAcceleration.hpp"
 #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
+#include "vehicle_angular_acceleration/VehicleAngularAcceleration.hpp"
 
 using namespace DriverFramework;
 using namespace sensors;
@@ -170,8 +171,9 @@ private:
 	VotedSensorsUpdate _voted_sensors_update;
 
 
-	VehicleAcceleration	_vehicle_acceleration;
-	VehicleAngularVelocity	_vehicle_angular_velocity;
+	VehicleAcceleration		_vehicle_acceleration;
+	VehicleAngularVelocity		_vehicle_angular_velocity;
+	VehicleAngularAcceleration	_vehicle_angular_acceleration;
 
 
 	/**
@@ -215,18 +217,24 @@ Sensors::Sensors(bool hil_enabled) :
 	_voted_sensors_update(_parameters, hil_enabled)
 {
 	initialize_parameter_handles(_parameter_handles);
+	parameters_update();
 
 	_airspeed_validator.set_timeout(300000);
 	_airspeed_validator.set_equal_value_threshold(100);
 
 	_vehicle_acceleration.Start();
 	_vehicle_angular_velocity.Start();
+
+	if (_parameters.imu_dgyro_en) {
+		_vehicle_angular_acceleration.Start();
+	}
 }
 
 Sensors::~Sensors()
 {
 	_vehicle_acceleration.Stop();
 	_vehicle_angular_velocity.Stop();
+	_vehicle_angular_acceleration.Stop();
 }
 
 int
@@ -573,6 +581,7 @@ int Sensors::print_status()
 
 	_vehicle_acceleration.PrintStatus();
 	_vehicle_angular_velocity.PrintStatus();
+	_vehicle_angular_acceleration.PrintStatus();
 
 	return 0;
 }

src/modules/sensors/vehicle_angular_acceleration/CMakeLists.txt

@@ -0,0 +1,37 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(vehicle_angular_acceleration
+	VehicleAngularAcceleration.cpp
+)
+target_link_libraries(vehicle_angular_acceleration PRIVATE px4_work_queue)

src/modules/sensors/vehicle_angular_acceleration/VehicleAngularAcceleration.cpp

@@ -0,0 +1,156 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "VehicleAngularAcceleration.hpp"
+
+#include <px4_log.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+VehicleAngularAcceleration::VehicleAngularAcceleration() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
+	_cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_acceleration: cycle time")),
+	_sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_acceleration: sensor latency"))
+{
+}
+
+VehicleAngularAcceleration::~VehicleAngularAcceleration()
+{
+	Stop();
+
+	perf_free(_cycle_perf);
+	perf_free(_sensor_latency_perf);
+}
+
+bool
+VehicleAngularAcceleration::Start()
+{
+	_angular_velocity_prev.zero();
+
+	// force initial updates
+	ParametersUpdate(true);
+
+	// Register callbacks
+	_vehicle_angular_velocity_sub.registerCallback();
+
+	return true;
+}
+
+void
+VehicleAngularAcceleration::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	_vehicle_angular_velocity_sub.unregisterCallback();
+}
+
+void
+VehicleAngularAcceleration::ParametersUpdate(bool force)
+{
+	// Check if parameters have changed
+	if (_params_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_params_sub.copy(&param_update);
+
+		updateParams();
+
+		// Low pass filter
+		if (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f) {
+			_lp_filter.set_cutoff_frequency(_loop_update_rate_hz, _param_imu_dgyro_cutoff.get());
+			_lp_filter.reset(_angular_velocity_prev);
+		}
+	}
+}
+
+void
+VehicleAngularAcceleration::Run()
+{
+	perf_begin(_cycle_perf);
+
+	vehicle_angular_velocity_s v_angular_velocity;
+
+	if (_vehicle_angular_velocity_sub.update(&v_angular_velocity)) {
+		perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&v_angular_velocity.timestamp));
+
+		ParametersUpdate();
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt = math::constrain(((v_angular_velocity.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f,
+						 0.02f);
+		_timestamp_sample_prev = v_angular_velocity.timestamp_sample;
+
+		// Differentiate angular velocity
+		Vector3f angular_velocity(v_angular_velocity.xyz);
+		Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt;
+		_angular_velocity_prev = angular_velocity;
+
+		// Apply low pass filter
+		Vector3f angular_acceleration(_lp_filter.apply(angular_acceleration_raw));
+
+		// Publish
+		vehicle_angular_acceleration_s v_angular_acceleration;
+		v_angular_acceleration.timestamp = hrt_absolute_time();
+		v_angular_acceleration.timestamp_sample = v_angular_velocity.timestamp_sample;
+		angular_acceleration.copyTo(v_angular_acceleration.xyz);
+
+		_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
+
+		// Calculate loop rate
+		_dt_accumulator += dt;
+		++_loop_counter;
+
+		if (_dt_accumulator > 1.f) {
+			const float loop_update_rate = (float)_loop_counter / _dt_accumulator;
+			_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
+			_dt_accumulator = 0;
+			_loop_counter = 0;
+
+			if (fabsf(_lp_filter.get_sample_freq() - _loop_update_rate_hz) > 1.0f) {
+				_lp_filter.set_cutoff_frequency(_loop_update_rate_hz, _param_imu_dgyro_cutoff.get());
+			}
+		}
+	}
+
+	perf_end(_cycle_perf);
+}
+
+void
+VehicleAngularAcceleration::PrintStatus()
+{
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_sensor_latency_perf);
+}

src/modules/sensors/vehicle_angular_acceleration/VehicleAngularAcceleration.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <lib/mathlib/math/Limits.hpp>
+#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
+
+class VehicleAngularAcceleration : public ModuleParams, public px4::WorkItem
+{
+public:
+
+	VehicleAngularAcceleration();
+	virtual ~VehicleAngularAcceleration();
+
+	void	Run() override;
+
+	bool	Start();
+	void	Stop();
+
+	void	PrintStatus();
+
+private:
+
+	void	ParametersUpdate(bool force = false);
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
+	)
+
+	uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
+
+	uORB::SubscriptionCallbackWorkItem 	_vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};	/**< angular velocity subscription */
+	uORB::Subscription			_params_sub{ORB_ID(parameter_update)};					/**< parameter updates subscription */
+
+	math::LowPassFilter2pVector3f _lp_filter{initial_update_rate_hz, 10.f};	/**< low-pass filter for angular acceleration */
+	static constexpr const float initial_update_rate_hz = 1000.f; 		/**< loop update rate used for initialization */
+	float _loop_update_rate_hz{initial_update_rate_hz};           		/**< current rate-controller loop update rate in [Hz] */
+
+	matrix::Vector3f			_angular_velocity_prev;
+
+	hrt_abstime _timestamp_sample_prev{0};
+	float _dt_accumulator{0.0f};
+	int _loop_counter{0};
+
+	perf_counter_t				_cycle_perf;
+	perf_counter_t				_sensor_latency_perf;
+};

src/modules/vehicle_model_estimator/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__vehicle_model_estimator
+	MAIN vehicle_model_estimator
+	COMPILE_FLAGS
+	SRCS
+		VehicleModelEstimator.cpp
+		VehicleModelEstimator.hpp
+	DEPENDS
+		mathlib
+		px4_work_queue
+		ecl_EKF
+	)

src/modules/vehicle_model_estimator/VehicleModelEstimator.cpp

@@ -0,0 +1,519 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file VehicleModelEstimator.cpp
+ *
+ * Vehicle model estimator.
+ *
+ * To estimate the vehicle inertia, the estimator low-pass filters and
+ * computes the time derivative of the vehicle angular acceleration
+ * and of the vehicle torque setpoint, then applies a least-mean-square.
+ * Similarly, to estimate the vehicle mass, the estimator low-pass
+ * filters and computes the time derivative of the vehicle acceleration
+ * and of the vehicle thrust setpoint, then applies a least-mean-square.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "VehicleModelEstimator.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <matrix/matrix/math.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+
+VehicleModelEstimator::VehicleModelEstimator() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	parameters_updated();
+	_I_est_inv = inv(_I);
+	_mass_est_inv = 1.0f / _mass;
+}
+
+VehicleModelEstimator::~VehicleModelEstimator()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+VehicleModelEstimator::init()
+{
+	if (!_vehicle_angular_acceleration_sub.registerCallback()) {
+		PX4_ERR("vehicle_angular_acceleration callback registration failed!");
+		return false;
+	}
+
+	if (!_vehicle_acceleration_sub.registerCallback()) {
+		PX4_ERR("vehicle_acceleration callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void
+VehicleModelEstimator::parameters_updated()
+{
+	// vehicle 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()}
+	};
+	_I = matrix::Matrix3f(inertia);
+
+	// vehicle mass
+	_mass = _param_vm_mass.get();
+
+	// low pass filters
+	if (fabsf(_lpf_ang_accel.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
+		_lpf_ang_accel.set_cutoff_frequency(_loop_update_rate_hz_ang_accel, _param_vm_est_cutoff.get());
+		_lpf_ang_accel.reset(_ang_accel_filtered);
+	}
+
+	if (fabsf(_lpf_torque_sp.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
+		_lpf_torque_sp.set_cutoff_frequency(_loop_update_rate_hz_torque_sp, _param_vm_est_cutoff.get());
+		_lpf_torque_sp.reset(_torque_sp_filtered);
+	}
+
+	if (fabsf(_lpf_lin_accel.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
+		_lpf_lin_accel.set_cutoff_frequency(_loop_update_rate_hz_lin_accel, _param_vm_est_cutoff.get());
+		_lpf_lin_accel.reset(_lin_accel_filtered);
+	}
+
+	if (fabsf(_lpf_thrust_sp.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
+		_lpf_thrust_sp.set_cutoff_frequency(_loop_update_rate_hz_thrust_sp, _param_vm_est_cutoff.get());
+		_lpf_thrust_sp.reset(_thrust_sp_filtered);
+	}
+}
+
+void
+VehicleModelEstimator::Run()
+{
+	if (should_exit()) {
+		_vehicle_angular_acceleration_sub.unregisterCallback();
+		_vehicle_acceleration_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+		parameters_updated();
+	}
+
+	// check for updates in other topics
+	if (_vehicle_land_detected_sub.updated()) {
+		vehicle_land_detected_s vehicle_land_detected;
+
+		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+			_landed = vehicle_land_detected.landed;
+			_maybe_landed = vehicle_land_detected.maybe_landed;
+		}
+	}
+
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_armed = vehicle_control_mode.flag_armed;
+		}
+	}
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	/* run inertia estimator on angular acceleration and torque setpoint changes */
+	bool inertia_update = false;
+	vehicle_angular_acceleration_s angular_acceleration;
+	vehicle_torque_setpoint_s torque_setpoint;
+
+	if (_vehicle_angular_acceleration_sub.update(&angular_acceleration)) {
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt_ang_accel = math::constrain(((angular_acceleration.timestamp_sample - _timestamp_ang_accel) / 1e6f), 0.0002f, 0.02f);
+		_timestamp_ang_accel = angular_acceleration.timestamp_sample;
+
+		// Calculate loop update rate
+		// (while disarmed or at least a few times beacause updating the filter is expensive)
+		if (!_armed || (now - _task_start) < 3300000) {
+			_dt_accumulator_ang_accel += dt_ang_accel;
+			++_loop_counter_ang_accel;
+
+			if (_dt_accumulator_ang_accel > 1.0f) {
+				const float loop_update_rate = (float)_loop_counter_ang_accel / _dt_accumulator_ang_accel;
+				_loop_update_rate_hz_ang_accel = _loop_update_rate_hz_ang_accel * 0.5f + loop_update_rate * 0.5f;
+				_dt_accumulator_ang_accel = 0.0f;
+				_loop_counter_ang_accel = 0;
+
+				if (fabsf(_lpf_ang_accel.get_sample_freq() - _loop_update_rate_hz_ang_accel) > 1.0f) {
+					_lpf_ang_accel.set_cutoff_frequency(_loop_update_rate_hz_ang_accel, _param_vm_est_cutoff.get());
+				}
+			}
+		}
+
+		// Apply low pass filter
+		Vector3f ang_accel_filtered_new(_lpf_ang_accel.apply(Vector3f(angular_acceleration.xyz)));
+
+		// Time derivative
+		_ang_accel_filtered_diff = (ang_accel_filtered_new - _ang_accel_filtered) / dt_ang_accel;
+		_ang_accel_filtered = ang_accel_filtered_new;
+
+		// Indicate that new data is available to estimate the vehicle inertia
+		inertia_update = true;
+	}
+
+	if (_vehicle_torque_setpoint_sub.update(&torque_setpoint)) {
+
+		// Buffer incoming data
+		_torque_sp_buffer.push(torque_setpoint);
+		torque_setpoint = _torque_sp_buffer.get_oldest();
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt_torque_sp = math::constrain(((torque_setpoint.timestamp_sample - _timestamp_torque_sp) / 1e6f), 0.0002f, 0.02f);
+		_timestamp_torque_sp = torque_setpoint.timestamp_sample;
+
+		// Calculate loop update rate
+		// (while disarmed or at least a few times beacause updating the filter is expensive)
+		if (!_armed || (now - _task_start) < 3300000) {
+			_dt_accumulator_torque_sp += dt_torque_sp;
+			++_loop_counter_torque_sp;
+
+			if (_dt_accumulator_torque_sp > 1.0f) {
+				const float loop_update_rate = (float)_loop_counter_torque_sp / _dt_accumulator_torque_sp;
+				_loop_update_rate_hz_torque_sp = _loop_update_rate_hz_torque_sp * 0.5f + loop_update_rate * 0.5f;
+				_dt_accumulator_torque_sp = 0.0f;
+				_loop_counter_torque_sp = 0;
+
+				if (fabsf(_lpf_torque_sp.get_sample_freq() - _loop_update_rate_hz_torque_sp) > 1.0f) {
+					_lpf_torque_sp.set_cutoff_frequency(_loop_update_rate_hz_torque_sp, _param_vm_est_cutoff.get());
+				}
+			}
+		}
+
+		// Compute required buffer size
+		if (!_armed || (now - _task_start) < 3300000 || _torque_sp_buffer.get_length() < 2) {
+			int _torque_sp_buffer_length = (_param_vm_est_delay.get() * _loop_update_rate_hz_torque_sp) + 1;
+
+			if (_torque_sp_buffer.get_length() != _torque_sp_buffer_length) {
+				if(!_torque_sp_buffer.allocate(_torque_sp_buffer_length)) {
+					PX4_ERR("Torque setpoint buffer allocation failed!");
+				}
+			}
+		}
+
+		// Apply low pass filter
+		Vector3f torque_sp_filtered_new(_lpf_torque_sp.apply(Vector3f(torque_setpoint.xyz)));
+
+		// Time derivative
+		_torque_sp_filtered_diff = (torque_sp_filtered_new - _torque_sp_filtered) / dt_torque_sp;
+		_torque_sp_filtered = torque_sp_filtered_new;
+
+		// Indicate that new data is available to estimate the vehicle inertia
+		inertia_update = true;
+	}
+
+	/* run mass estimator on linear acceleration and thrust setpoint changes */
+	bool mass_update = false;
+	vehicle_acceleration_s acceleration;
+	vehicle_thrust_setpoint_s thrust_setpoint;
+
+	if (_vehicle_acceleration_sub.update(&acceleration)) {
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt_lin_accel = math::constrain(((acceleration.timestamp_sample - _timestamp_lin_accel) / 1e6f), 0.0002f, 0.02f);
+		_timestamp_lin_accel = acceleration.timestamp_sample;
+
+		// Calculate loop update rate
+		// (while disarmed or at least a few times beacause updating the filter is expensive)
+		if (!_armed || (now - _task_start) < 3300000) {
+			_dt_accumulator_lin_accel += dt_lin_accel;
+			++_loop_counter_lin_accel;
+
+			if (_dt_accumulator_lin_accel > 1.0f) {
+				const float loop_update_rate = (float)_loop_counter_lin_accel / _dt_accumulator_lin_accel;
+				_loop_update_rate_hz_lin_accel = _loop_update_rate_hz_lin_accel * 0.5f + loop_update_rate * 0.5f;
+				_dt_accumulator_lin_accel = 0.0f;
+				_loop_counter_lin_accel = 0;
+
+				if (fabsf(_lpf_lin_accel.get_sample_freq() - _loop_update_rate_hz_lin_accel) > 1.0f) {
+					_lpf_lin_accel.set_cutoff_frequency(_loop_update_rate_hz_lin_accel, _param_vm_est_cutoff.get());
+				}
+			}
+		}
+
+		// Apply low pass filter
+		Vector3f lin_accel_filtered_new(_lpf_lin_accel.apply(Vector3f(acceleration.xyz)));
+
+		// Time derivative
+		_lin_accel_filtered_diff = (lin_accel_filtered_new - _lin_accel_filtered) / dt_lin_accel;
+		_lin_accel_filtered = lin_accel_filtered_new;
+
+		// Indicate that new data is available to estimate the vehicle mass
+		mass_update = true;
+	}
+
+	if (_vehicle_thrust_setpoint_sub.update(&thrust_setpoint)) {
+
+		// Buffer incoming data
+		_thrust_sp_buffer.push(thrust_setpoint);
+		thrust_setpoint = _thrust_sp_buffer.get_oldest();
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt_thrust_sp = math::constrain(((thrust_setpoint.timestamp_sample - _timestamp_thrust_sp) / 1e6f), 0.0002f, 0.02f);
+		_timestamp_thrust_sp = thrust_setpoint.timestamp_sample;
+
+		// Calculate loop update rate
+		// (while disarmed or at least a few times beacause updating the filter is expensive)
+		if (!_armed || (now - _task_start) < 3300000) {
+			_dt_accumulator_thrust_sp += dt_thrust_sp;
+			++_loop_counter_thrust_sp;
+
+			if (_dt_accumulator_thrust_sp > 1.0f) {
+				const float loop_update_rate = (float)_loop_counter_thrust_sp / _dt_accumulator_thrust_sp;
+				_loop_update_rate_hz_thrust_sp = _loop_update_rate_hz_thrust_sp * 0.5f + loop_update_rate * 0.5f;
+				_dt_accumulator_thrust_sp = 0.0f;
+				_loop_counter_thrust_sp = 0;
+
+				if (fabsf(_lpf_thrust_sp.get_sample_freq() - _loop_update_rate_hz_thrust_sp) > 1.0f) {
+					_lpf_thrust_sp.set_cutoff_frequency(_loop_update_rate_hz_thrust_sp, _param_vm_est_cutoff.get());
+				}
+			}
+		}
+
+		// Compute required buffer size
+		if (!_armed || (now - _task_start) < 3300000 || _thrust_sp_buffer.get_length() < 2) {
+			int _thrust_sp_buffer_length = (_param_vm_est_delay.get() * _loop_update_rate_hz_thrust_sp) + 1;
+
+			if (_thrust_sp_buffer.get_length() != _thrust_sp_buffer_length) {
+				if(!_thrust_sp_buffer.allocate(_thrust_sp_buffer_length)) {
+					PX4_ERR("Thrust setpoint buffer allocation failed!");
+				}
+			}
+		}
+
+		// Apply low pass filter
+		Vector3f thrust_sp_filtered_new(_lpf_thrust_sp.apply(Vector3f(thrust_setpoint.xyz)));
+
+		// Time derivative
+		_thrust_sp_filtered_diff = (thrust_sp_filtered_new - _thrust_sp_filtered) / dt_thrust_sp;
+		_thrust_sp_filtered = thrust_sp_filtered_new;
+
+		// Indicate that new data is available to estimate the vehicle mass
+		mass_update = true;
+	}
+
+	// Update inertia estimate only if we have new data and we are flying
+	if (inertia_update && _armed && !_landed && !_maybe_landed) {
+		// Predict angular jerk from time derivative of torque setpoint and estimated inertia
+		Matrix<float, 3, 1> jerk_pred = _I_est_inv * _torque_sp_filtered_diff;
+
+		// Compute prediction error
+		matrix::Matrix<float, 3, 1> error = _ang_accel_filtered_diff - jerk_pred;
+
+		// Update estimate
+		_I_est_inv += error * _torque_sp_filtered_diff.T() * _param_vm_est_gain.get();
+
+		// Sanity check
+		bool reset_needed = false;
+		for (size_t i = 0; i < 3; i++) {
+			for (size_t j = 0; j < 3; j++) {
+				if (not PX4_ISFINITE(_I_est_inv(i, j))) {
+					reset_needed = true;
+				}
+			}
+		}
+
+		if (reset_needed) {
+			_I_est_inv = inv(_I);
+		}
+
+		// Enforce positive values on diagonal
+		for (size_t i = 0; i < 3; i++) {
+			if (_I_est_inv(i, i) < FLT_EPSILON) {
+				_I_est_inv(i, i) = FLT_EPSILON;
+			}
+		}
+
+		// Enforce symmetry
+		_I_est_inv(1, 0) = 0.5f * (_I_est_inv(0, 1) + _I_est_inv(1, 0));
+		_I_est_inv(0, 1) = _I_est_inv(1, 0);
+		_I_est_inv(2, 0) = 0.5f * (_I_est_inv(0, 2) + _I_est_inv(2, 0));
+		_I_est_inv(0, 2) = _I_est_inv(2, 0);
+		_I_est_inv(2, 1) = 0.5f * (_I_est_inv(1, 2) + _I_est_inv(2, 1));
+		_I_est_inv(1, 2) = _I_est_inv(2, 1);
+	}
+
+	// Update mass estimate only if we have new data and we are flying
+	if (mass_update && _armed && !_landed && !_maybe_landed) {
+		// Predict jerk from time derivative of thrust setpoint and estimated mass
+		Vector3f jerk_pred = _mass_est_inv * _thrust_sp_filtered_diff;
+
+		// Compute prediction error
+		Vector3f error = _lin_accel_filtered_diff - jerk_pred;
+
+		// Update estimate
+		_mass_est_inv += error.dot(_torque_sp_filtered_diff) * _param_vm_est_gain.get();
+
+		// Sanity check
+		if (not PX4_ISFINITE(_mass_est_inv)) {
+			_mass_est_inv = 1.0f / _mass;
+		}
+
+		// Enforce positive value
+		if (_mass_est_inv < FLT_EPSILON) {
+			_mass_est_inv = FLT_EPSILON;
+		}
+
+		if ((1.0f / _mass_est_inv) < FLT_EPSILON) {
+			_mass_est_inv = 1.0f / _mass;
+		}
+	}
+
+	// Publish estimated model
+	if ((now - _prev_publish_time) > (1e6f / _param_vm_est_pubrate.get())) {
+		_prev_publish_time = now;
+
+		// Compute inertia matrix and mass
+		Matrix3f I = _I;
+		float mass = _mass;
+		if (_param_vm_est_en.get()) {
+			// Invert mass estimation
+			mass = 1.0f / _mass_est_inv;
+
+			// Try to invert inertia estimation
+			bool invertible = inv(_I_est_inv, I);
+
+			if (not invertible) {
+				// Fallback to default value
+				I = _I;
+				_I_est_inv = inv(_I);
+			}
+		} else {
+			// reset estimation
+			_I_est_inv = inv(_I);
+			_mass_est_inv = 1.0f / _mass;
+		}
+
+		// Publish
+		vehicle_model_s vehicle_model;
+		vehicle_model.timestamp = hrt_absolute_time();
+		vehicle_model.mass = mass;
+		I.copyTo(vehicle_model.inertia);
+		_vehicle_model_pub.publish(vehicle_model);
+	}
+
+	perf_end(_loop_perf);
+}
+
+int VehicleModelEstimator::task_spawn(int argc, char *argv[])
+{
+	VehicleModelEstimator *instance = new VehicleModelEstimator();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int VehicleModelEstimator::print_status()
+{
+	PX4_INFO("Running");
+
+	perf_print_counter(_loop_perf);
+
+	return 0;
+}
+
+int VehicleModelEstimator::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int VehicleModelEstimator::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the estimation of vehicle model (mass & inertia).
+To estimate the vehicle inertia, the estimator low-pass filters and
+computes the time derivative of the vehicle angular acceleration
+and of the vehicle torque setpoint, then applies a least-mean-square.
+Similarly, to estimate the vehicle mass, the estimator low-pass
+filters and computes the time derivative of the vehicle acceleration
+and of the vehicle thrust setpoint, then applies a least-mean-square.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+/**
+ * Multicopter rate control app start / stop handling function
+ */
+extern "C" __EXPORT int vehicle_model_estimator_main(int argc, char *argv[]);
+
+int vehicle_model_estimator_main(int argc, char *argv[])
+{
+	return VehicleModelEstimator::main(argc, argv);
+}

src/modules/vehicle_model_estimator/VehicleModelEstimator.hpp

@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file VehicleModelEstimator.hpp
+ *
+ * Vehicle model estimator.
+ *
+ * To estimate the vehicle inertia, the estimator low-pass filters and
+ * computes the time derivative of the vehicle angular acceleration
+ * and of the vehicle torque setpoint, then applies a least-mean-square.
+ * Similarly, to estimate the vehicle mass, the estimator low-pass
+ * filters and computes the time derivative of the vehicle acceleration
+ * and of the vehicle thrust setpoint, then applies a least-mean-square.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/ecl/EKF/ekf.h>	// included for RingBuffer
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_model.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
+
+
+class VehicleModelEstimator : public ModuleBase<VehicleModelEstimator>, public ModuleParams, public px4::WorkItem
+{
+public:
+	VehicleModelEstimator();
+
+	virtual ~VehicleModelEstimator();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void Run() override;
+
+	bool init();
+
+private:
+
+	/**
+	 * initialize some vectors/matrices from parameters
+	 */
+	void		parameters_updated();
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};			/**< parameter updates subscription */
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
+	uORB::Subscription _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint)}; 	/**< vehicle torque setpoint subscription */
+	uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; 	/**< vehicle thrust setpoint subscription */
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};	  	/**< vehicle land detected subscription */
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_acceleration_sub{this, ORB_ID(vehicle_angular_acceleration)};
+	uORB::SubscriptionCallbackWorkItem _vehicle_acceleration_sub{this, ORB_ID(vehicle_acceleration)};
+
+	uORB::Publication<vehicle_model_s>		   _vehicle_model_pub{ORB_ID(vehicle_model)};				/**< vehicle model publication */
+
+	bool _armed{false};
+	bool _landed{true};
+	bool _maybe_landed{true};
+
+	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
+
+	static constexpr const float initial_update_rate_hz = 1000.f; /**< loop update rate used for initialization */
+	hrt_abstime _task_start{hrt_absolute_time()};
+	hrt_abstime _prev_publish_time{0};
+
+	// Inertia estimation data
+	matrix::SquareMatrix3f _I;  		/**< Inertia matrix entered via parameters */
+	matrix::SquareMatrix3f _I_est_inv;      /**< Inverse of estimated inertia matrix */
+
+	// Angular acceleration data
+	math::LowPassFilter2pVector3f _lpf_ang_accel{0.f, 0.f}; 	/**< low-pass filters for angular acceleration */
+	matrix::Vector3f _ang_accel_filtered;				/**< low-pass filtered angular acceleration */
+	matrix::Vector3f _ang_accel_filtered_diff;			/**< time derivative of the low-pass filtered angular acceleration */
+	hrt_abstime _timestamp_ang_accel{0};
+	float _loop_update_rate_hz_ang_accel{initial_update_rate_hz};
+	int _loop_counter_ang_accel{0};
+	float _dt_accumulator_ang_accel{0.0f};
+
+	// Torque setpoint data
+	RingBuffer<vehicle_torque_setpoint_s> _torque_sp_buffer;
+	math::LowPassFilter2pVector3f _lpf_torque_sp{0.f, 0.f}; 	/**< low-pass filters for torque setpoint */
+	matrix::Vector3f _torque_sp_filtered;				/**< low-pass filtered torque setpoint */
+	matrix::Vector3f _torque_sp_filtered_diff;			/**< time derivative of the low-pass filtered torque setpoint */
+	hrt_abstime _timestamp_torque_sp{0};
+	float _loop_update_rate_hz_torque_sp{initial_update_rate_hz};
+	int _loop_counter_torque_sp{0};
+	float _dt_accumulator_torque_sp{0.0f};
+
+	// Mass estimation data
+	float _mass{0.0f};		/**< Mass entered via parameters */
+	float _mass_est_inv{1.0f};	/**< Inverse of estimated mass */
+
+	// Linear acceleration data
+	math::LowPassFilter2pVector3f _lpf_lin_accel{0.f, 0.f}; 	/**< low-pass filters for linear acceleration */
+	matrix::Vector3f _lin_accel_filtered;				/**< low-pass filtered linear acceleration */
+	matrix::Vector3f _lin_accel_filtered_diff;			/**< time derivative of the low-pass filtered linear acceleration */
+	hrt_abstime _timestamp_lin_accel{0};
+	float _loop_update_rate_hz_lin_accel{initial_update_rate_hz};
+	int _loop_counter_lin_accel{0};
+	float _dt_accumulator_lin_accel{0.0f};
+
+	// Thrust setpoint data
+	RingBuffer<vehicle_thrust_setpoint_s> _thrust_sp_buffer;
+	math::LowPassFilter2pVector3f _lpf_thrust_sp{0.f, 0.f}; 	/**< low-pass filters for thrust setpoint */
+	matrix::Vector3f _thrust_sp_filtered;				/**< low-pass filtered thrust setpoint */
+	matrix::Vector3f _thrust_sp_filtered_diff;			/**< time derivative of the low-pass filtered thrust setpoint */
+	hrt_abstime _timestamp_thrust_sp{0};
+	float _loop_update_rate_hz_thrust_sp{initial_update_rate_hz};
+	int _loop_counter_thrust_sp{0};
+	float _dt_accumulator_thrust_sp{0.0f};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::VM_MASS>) _param_vm_mass,			/**< vehicle mass */
+
+		(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx,		/**< vehicle inertia */
+		(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
+		(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
+		(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
+		(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
+		(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz,
+
+		(ParamInt<px4::params::VM_EST_EN>) _param_vm_est_en,
+		(ParamFloat<px4::params::VM_EST_PUBRATE>) _param_vm_est_pubrate,
+		(ParamFloat<px4::params::VM_EST_CUTOFF>) _param_vm_est_cutoff,
+		(ParamFloat<px4::params::VM_EST_GAIN>) _param_vm_est_gain,
+		(ParamFloat<px4::params::VM_EST_DELAY>) _param_vm_est_delay
+	)
+};
+

src/modules/vehicle_model_estimator/vehicle_model_estimator_params.c

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_model_estimator_params.c
+ * Parameters for the estimation of the vehicle model.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+
+/**
+ * Enables the vehicle model estimation
+ *
+ * @boolean
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_INT32(VM_EST_EN, 1);
+
+/**
+ * Estimated vehicle model publishing rate
+ *
+ * The estimator will always run at maximum rate, updating its
+ * estimate at full rate.
+ * This parameter may be used to limit the rate at which the
+ * resulting estimation is published.
+ *
+ * @unit Hz
+ * @min 0
+ * @max 1000
+ * @decimal 0
+ * @increment 1
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_EST_PUBRATE, 10.0f);
+
+/**
+ * Estimation gain
+ *
+ * A larger value will lead to faster update of the estimated model
+ * but an excessively large value may cause faulty estimation.
+ *
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_EST_GAIN, 0.1f);
+
+/**
+ * Vehicle model estimator cutoff frequency
+ *
+ * Determines the cutoff frequency of the 2nd order low pass filter
+ * applied on control setpoints and sensor data before estimation.
+ *
+ * @min 0
+ * @max 30
+ * @decimal 2
+ * @increment 1
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_EST_CUTOFF, 5.0f);
+
+/**
+ * Vehicle model estimator actuator delay
+ *
+ * Determines the delay applied to the torque and thrust setpoints
+ * in order to take into account the time required for the
+ * setpoints to take effect.
+ * This delay may vary depending on
+ * - whether the setpoints go through an IO board
+ * - the latency of the protocol used to communicate with actuators
+ * - the intrinsic latency of actuators
+ *
+ * @unit s
+ * @min 0
+ * @max 0.5
+ * @decimal 4
+ * @increment 1
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_EST_DELAY, 0.080f);

src/modules/vehicle_model_estimator/vehicle_model_params.c

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_model_params.c
+ * Parameters for vehicle model.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+/**
+ * Mass
+ *
+ * @unit kg
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_MASS, 1.f);
+
+/**
+ * Inertia matrix, XX component
+ *
+ * @unit kg.m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 1.f);
+
+/**
+ * Inertia matrix, YY component
+ *
+ * @unit kg.m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 1.f);
+
+/**
+ * Inertia matrix, ZZ component
+ *
+ * @unit kg.m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 1.f);
+
+/**
+ * Inertia matrix, XY component
+ *
+ * @unit kg.m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f);
+
+/**
+ * Inertia matrix, XZ component
+ *
+ * @unit kg.m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f);
+
+/**
+ * Inertia matrix, YZ component
+ *
+ * @unit kg.m^2
+ * @decimal 5
+ * @increment 0.00001
+ * @group Vehicle Model
+ */
+PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f);