TECS: Use version in ECL library

This change updates a number of interfaces to use the new TECS implementation from the ECL library.

msg/tecs_status.msg

@@ -11,7 +11,6 @@ float32 altitudeSp
 float32 altitude_filtered
 float32 flightPathAngleSp
 float32 flightPathAngle
-float32 flightPathAngleFiltered
 float32 airspeedSp
 float32 airspeed_filtered
 float32 airspeedDerivativeSp

msg/vehicle_local_position.msg

@@ -29,6 +29,10 @@ uint8 vxy_reset_counter
 
 float32 delta_vz
 uint8 vz_reset_counter
+# Acceleration in NED frame
+float32 ax        # North velocity derivative in NED earth-fixed frame, (metres/sec^2)
+float32 ay        # East velocity derivative in NED earth-fixed frame, (metres/sec^2)
+float32 az        # Down velocity derivative in NED earth-fixed frame, (metres/sec^2)
 
 # Heading
 float32 yaw				# Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI,  (radians) 

src/lib/ecl

@@ -1 +1 @@
-Subproject commit 2bae55753dc3cb83ae8bd92be5dcc76f27cf8948
+Subproject commit f640dbcb538c9d033385ad454fd3288cfd0efd4f

src/modules/ekf2/ekf2_main.cpp

@@ -904,6 +904,13 @@ void Ekf2::run()
 			_ekf.get_pos_d_deriv(&pos_d_deriv);
 			lpos.z_deriv = pos_d_deriv; // vertical position time derivative (m/s)
 
+			// Acceleration of body origin in local NED frame
+			float vel_deriv[3] = {};
+			_ekf.get_vel_deriv_ned(vel_deriv);
+			lpos.ax = vel_deriv[0];
+			lpos.ay = vel_deriv[1];
+			lpos.az = vel_deriv[2];
+
 			// TODO: better status reporting
 			lpos.xy_valid = _ekf.local_position_is_valid() && !_vel_innov_preflt_fail;
 			lpos.z_valid = !_vel_innov_preflt_fail;

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -668,29 +668,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 	_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
 	_att_sp.apply_flaps = false;		// by default we don't use flaps
 
-	/* filter speed and altitude for controller */
-	math::Vector<3> accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z);
-
-	// tailsitters use the multicopter frame as reference, in fixed wing
-	// we need to use the fixed wing frame
-	if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
-		float tmp = accel_body(0);
-		accel_body(0) = -accel_body(2);
-		accel_body(2) = tmp;
-	}
-
-	math::Vector<3> accel_earth{_R_nb * accel_body};
-
-	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
-	bool in_air_alt_control = (!_vehicle_land_detected.landed &&
-				   (_control_mode.flag_control_auto_enabled ||
-				    _control_mode.flag_control_velocity_enabled ||
-				    _control_mode.flag_control_altitude_enabled));
-
-	/* update TECS filters */
-	_tecs.update_state(_global_pos.alt, _airspeed, _R_nb,
-			   accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
-
 	calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
 
 	// l1 navigation logic breaks down when wind speed exceeds max airspeed
@@ -1474,7 +1451,7 @@ float
 FixedwingPositionControl::get_tecs_pitch()
 {
 	if (_is_tecs_running) {
-		return _tecs.get_pitch_demand();
+		return _tecs.get_pitch_setpoint();
 	}
 
 	// return 0 to prevent stale tecs state when it's not running
@@ -1485,7 +1462,7 @@ float
 FixedwingPositionControl::get_tecs_thrust()
 {
 	if (_is_tecs_running) {
-		return _tecs.get_throttle_demand();
+		return _tecs.get_throttle_setpoint();
 	}
 
 	// return 0 to prevent stale tecs state when it's not running
@@ -1514,6 +1491,7 @@ FixedwingPositionControl::task_main()
 	 * do subscriptions
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -1589,6 +1567,7 @@ FixedwingPositionControl::task_main()
 
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+			orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
 
 			// handle estimator reset events. we only adjust setpoins for manual modes
 			if (_control_mode.flag_control_manual_enabled) {
@@ -1821,6 +1800,28 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 		pitch_for_tecs = euler(1);
 	}
 
+	/* filter speed and altitude for controller */
+	math::Vector<3> accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z);
+
+	// tailsitters use the multicopter frame as reference, in fixed wing
+	// we need to use the fixed wing frame
+	if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
+		float tmp = accel_body(0);
+		accel_body(0) = -accel_body(2);
+		accel_body(2) = tmp;
+	}
+
+	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
+	bool in_air_alt_control = (!_vehicle_land_detected.landed &&
+				   (_control_mode.flag_control_auto_enabled ||
+				    _control_mode.flag_control_velocity_enabled ||
+				    _control_mode.flag_control_altitude_enabled));
+
+	/* update TECS vehicle state estimates */
+	_tecs.update_vehicle_state_estimates(_airspeed, _R_nb,
+					     accel_body, (_global_pos.timestamp > 0), in_air_alt_control,
+					     _global_pos.alt, _local_pos.v_z_valid, _local_pos.vz, _local_pos.az);
+
 	_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs,
 				    _global_pos.alt, alt_sp,
 				    airspeed_sp, _airspeed, _eas2tas,
@@ -1828,13 +1829,10 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 				    throttle_min, throttle_max, throttle_cruise,
 				    pitch_min_rad, pitch_max_rad);
 
-	TECS::tecs_state s {};
-	_tecs.get_tecs_state(s);
-
-	tecs_status_s t {};
-	t.timestamp = s.timestamp;
+	tecs_status_s t;
+	t.timestamp = _tecs.timestamp();
 
-	switch (s.mode) {
+	switch (_tecs.tecs_mode()) {
 	case TECS::ECL_TECS_MODE_NORMAL:
 		t.mode = tecs_status_s::TECS_MODE_NORMAL;
 		break;
@@ -1852,25 +1850,24 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 		break;
 	}
 
-	t.altitudeSp 		= s.altitude_sp;
-	t.altitude_filtered = s.altitude_filtered;
-	t.airspeedSp 		= s.airspeed_sp;
-	t.airspeed_filtered = s.airspeed_filtered;
+	t.altitudeSp			= _tecs.hgt_setpoint_adj();
+	t.altitude_filtered		= _tecs.vert_pos_state();
+	t.airspeedSp			= _tecs.TAS_setpoint_adj();
+	t.airspeed_filtered 	= _tecs.tas_state();
 
-	t.flightPathAngleSp 		= s.altitude_rate_sp;
-	t.flightPathAngle 			= s.altitude_rate;
-	t.flightPathAngleFiltered 	= s.altitude_rate;
+	t.flightPathAngleSp		= _tecs.hgt_rate_setpoint();
+	t.flightPathAngle		= _tecs.vert_vel_state();
 
-	t.airspeedDerivativeSp 	= s.airspeed_rate_sp;
-	t.airspeedDerivative 	= s.airspeed_rate;
+	t.airspeedDerivativeSp	= _tecs.TAS_rate_setpoint();
+	t.airspeedDerivative	= _tecs.speed_derivative();
 
-	t.totalEnergyError 				= s.total_energy_error;
-	t.totalEnergyRateError 			= s.total_energy_rate_error;
-	t.energyDistributionError 		= s.energy_distribution_error;
-	t.energyDistributionRateError 	= s.energy_distribution_rate_error;
+	t.totalEnergyError				= _tecs.STE_error();
+	t.totalEnergyRateError			= _tecs.STE_rate_error();
+	t.energyDistributionError		= _tecs.SEB_error();
+	t.energyDistributionRateError	= _tecs.SEB_rate_error();
 
-	t.throttle_integ 	= s.throttle_integ;
-	t.pitch_integ 		= s.pitch_integ;
+	t.throttle_integ	= _tecs.throttle_integ_state();
+	t.pitch_integ		= _tecs.pitch_integ_state();
 
 	if (_tecs_status_pub != nullptr) {
 		orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -33,21 +33,14 @@
 
 
 /**
- * @file fw_pos_control_l1_main.c
+ * @file fw_pos_control_l1_main.hpp
  * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll
  * angle, equivalent to a lateral motion (for copters and rovers).
  *
- * Original publication for horizontal control class:
- *    S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
- *    Proceedings of the AIAA Guidance, Navigation and Control
- *    Conference, Aug 2004. AIAA-2004-4900.
+ * The implementation for the controllers is in the ECL library. This class only
+ * interfaces to the library.
  *
- * Original implementation for total energy control class:
- *    Paul Riseborough, ECL Library, 2017
- *
- * More details and acknowledgements in the referenced library headers.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Andreas Antener <andreas@uaventure.com>
  */
@@ -85,6 +78,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/uORB.h>
@@ -152,6 +146,7 @@ private:
 	bool		_task_running{false};			///< if true, task is running in its mainloop */
 
 	int		_global_pos_sub{-1};
+	int		_local_pos_sub{-1};
 	int		_pos_sp_triplet_sub{-1};
 	int		_control_mode_sub{-1};			///< control mode subscription */
 	int		_vehicle_attitude_sub{-1};		///< vehicle attitude subscription */
@@ -175,6 +170,7 @@ private:
 	vehicle_command_s		_vehicle_command {};		///< vehicle commands */
 	vehicle_control_mode_s		_control_mode {};		///< control mode */
 	vehicle_global_position_s	_global_pos {};			///< global vehicle position */
+	vehicle_local_position_s	_local_pos {};			///< vehicle local position */
 	vehicle_land_detected_s		_vehicle_land_detected {};	///< vehicle land detected */
 	vehicle_status_s		_vehicle_status {};		///< vehicle status */