Disable mTECS, but retain code for now (#4407)

* Disable mTECS, but retain code for now

* astyle fw_pos_control_l1

* fw_pos remove pitch_max_special only used by mtecs

* move FW_T_CLMB_MAX to FW TECS param group

* fw_pos initialize tecs_status_s

src/modules/fw_pos_control_l1/CMakeLists.txt

@@ -35,13 +35,10 @@ px4_add_module(
 	MAIN fw_pos_control_l1
 	STACK_MAIN 1200
 	COMPILE_FLAGS
-		-Wno-float-equal
 		-Os
 	SRCS
 		fw_pos_control_l1_main.cpp
 		landingslope.cpp
-		mtecs/mTecs.cpp
-		mtecs/limitoverride.cpp
 	DEPENDS
 		platforms__common
 		git_ecl

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 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
@@ -52,53 +52,54 @@
  * @author Andreas Antener <andreas@uaventure.com>
  */
 
+#include <errno.h>
+#include <fcntl.h>
+#include <math.h>
+#include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <unistd.h>
+
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <px4_tasks.h>
 #include <px4_posix.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_accel.h>
+
+#include "landingslope.h"
 #include <arch/board/board.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/fw_virtual_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/control_state.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/navigation_capabilities.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/tecs_status.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/mavlink_log.h>
-#include <geo/geo.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
-#include <mathlib/mathlib.h>
-#include <launchdetection/LaunchDetector.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_hrt.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
-#include "landingslope.h"
-#include "mtecs/mTecs.h"
+#include <geo/geo.h>
+#include <launchdetection/LaunchDetector.h>
+#include <mathlib/mathlib.h>
 #include <platforms/px4_defines.h>
 #include <runway_takeoff/RunwayTakeoff.h>
+#include <systemlib/err.h>
+#include <systemlib/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/pid/pid.h>
+#include <systemlib/systemlib.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/control_state.h>
+#include <uORB/topics/fw_virtual_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/tecs_status.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>
 
 static int	_control_task = -1;			/**< task handle for sensor task */
@@ -249,7 +250,6 @@ private:
 
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
-	fwPosctrl::mTecs				_mTecs;
 	enum FW_POSCTRL_MODE {
 		FW_POSCTRL_MODE_AUTO,
 		FW_POSCTRL_MODE_POSITION,
@@ -486,8 +486,7 @@ private:
 	void		reset_landing_state();
 
 	/*
-	 * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
-	 * XXX need to clean up/remove this function once mtecs fully replaces TECS
+	 * Call TECS : a wrapper function to call the TECS implementation
 	 */
 	void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
 					float pitch_min_rad, float pitch_max_rad,
@@ -495,8 +494,7 @@ private:
 					bool climbout_mode, float climbout_pitch_min_rad,
 					float altitude,
 					const math::Vector<3> &ground_speed,
-					unsigned mode = tecs_status_s::TECS_MODE_NORMAL,
-					bool pitch_max_special = false);
+					unsigned mode = tecs_status_s::TECS_MODE_NORMAL);
 
 };
 
@@ -593,7 +591,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_asp_after_transition(0.0f),
 	_was_in_transition(false),
 	_l1_control(),
-	_mTecs(),
 	_control_mode_current(FW_POSCTRL_MODE_OTHER)
 {
 	_nav_capabilities = {};
@@ -780,9 +777,6 @@ FixedwingPositionControl::parameters_update()
 	/* Update Launch Detector Parameters */
 	launchDetector.updateParams();
 
-	/* Update the mTecs */
-	_mTecs.updateParams();
-
 	_runway_takeoff.updateParams();
 
 	return OK;
@@ -927,18 +921,21 @@ float
 FixedwingPositionControl::get_demanded_airspeed()
 {
 	float altctrl_airspeed = 0;
+
 	// neutral throttle corresponds to trim airspeed
 	if (_manual.z < 0.5f) {
 		// lower half of throttle is min to trim airspeed
 		altctrl_airspeed = _parameters.airspeed_min +
-							(_parameters.airspeed_trim - _parameters.airspeed_min) *
-							_manual.z * 2;
+				   (_parameters.airspeed_trim - _parameters.airspeed_min) *
+				   _manual.z * 2;
+
 	} else {
 		// upper half of throttle is trim to max airspeed
 		altctrl_airspeed = _parameters.airspeed_trim +
-							(_parameters.airspeed_max - _parameters.airspeed_trim) *
-							(_manual.z * 2 - 1);
+				   (_parameters.airspeed_max - _parameters.airspeed_trim) *
+				   (_manual.z * 2 - 1);
 	}
+
 	return altctrl_airspeed;
 }
 
@@ -1232,10 +1229,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				    _control_mode.flag_control_altitude_enabled));
 
 	/* update TECS filters */
-	if (!_mTecs.getEnabled()) {
-		_tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb,
-				   accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
-	}
+	_tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb,
+			   accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
 
 	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
 	calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
@@ -1265,13 +1260,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
 		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
 			/* reset integrators */
-			if (_mTecs.getEnabled()) {
-				_mTecs.resetIntegrators();
-				_mTecs.resetDerivatives(_ctrl_state.airspeed);
-
-			} else {
-				_tecs.reset_state();
-			}
+			_tecs.reset_state();
 		}
 
 		_control_mode_current = FW_POSCTRL_MODE_AUTO;
@@ -1316,8 +1305,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}
 
 		float mission_airspeed = _parameters.airspeed_trim;
+
 		if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
-			_pos_sp_triplet.current.cruising_speed > 0.1f) {
+		    _pos_sp_triplet.current.cruising_speed > 0.1f) {
 			mission_airspeed = _pos_sp_triplet.current.cruising_speed;
 		}
 
@@ -1663,8 +1653,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 									   _parameters.pitch_limit_min)),
 							   _global_pos.alt,
 							   ground_speed,
-							   tecs_status_s::TECS_MODE_TAKEOFF,
-							   takeoff_pitch_max_deg != _parameters.pitch_limit_max);
+							   tecs_status_s::TECS_MODE_TAKEOFF);
 
 				// assign values
 				_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll());
@@ -1737,8 +1726,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 										     math::radians(10.0f)),
 									   _global_pos.alt,
 									   ground_speed,
-									   tecs_status_s::TECS_MODE_TAKEOFF,
-									   takeoff_pitch_max_deg != _parameters.pitch_limit_max);
+									   tecs_status_s::TECS_MODE_TAKEOFF);
 
 						/* limit roll motion to ensure enough lift */
 						_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
@@ -1806,13 +1794,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
 		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
 			/* reset integrators */
-			if (_mTecs.getEnabled()) {
-				_mTecs.resetIntegrators();
-				_mTecs.resetDerivatives(_ctrl_state.airspeed);
-
-			} else {
-				_tecs.reset_state();
-			}
+			_tecs.reset_state();
 		}
 
 		_control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -1921,13 +1903,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
 		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
 			/* reset integrators */
-			if (_mTecs.getEnabled()) {
-				_mTecs.resetIntegrators();
-				_mTecs.resetDerivatives(_ctrl_state.airspeed);
-
-			} else {
-				_tecs.reset_state();
-			}
+			_tecs.reset_state();
 		}
 
 		_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
@@ -2002,7 +1978,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons.
 		   the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
-		_att_sp.thrust = (launchDetector.launchDetectionEnabled()) ? launchDetector.getThrottlePreTakeoff() : _parameters.throttle_idle;
+		_att_sp.thrust = (launchDetector.launchDetectionEnabled()) ? launchDetector.getThrottlePreTakeoff() :
+				 _parameters.throttle_idle;
 
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
 		   pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
@@ -2050,9 +2027,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 }
 
 float
-FixedwingPositionControl::get_tecs_pitch() {
+FixedwingPositionControl::get_tecs_pitch()
+{
 	if (_is_tecs_running) {
-		return _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
+		return _tecs.get_pitch_demand();
 
 	} else {
 		// return 0 to prevent stale tecs state when it's not running
@@ -2061,9 +2039,10 @@ FixedwingPositionControl::get_tecs_pitch() {
 }
 
 float
-FixedwingPositionControl::get_tecs_thrust() {
+FixedwingPositionControl::get_tecs_thrust()
+{
 	if (_is_tecs_running) {
-		return _mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand();
+		return _tecs.get_throttle_demand();
 
 	} else {
 		// return 0 to prevent stale tecs state when it's not running
@@ -2238,7 +2217,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		bool climbout_mode, float climbout_pitch_min_rad,
 		float altitude,
 		const math::Vector<3> &ground_speed,
-		unsigned mode, bool pitch_max_special)
+		unsigned mode)
 {
 	bool run_tecs = true;
 	float dt = 0.01f; // prevent division with 0
@@ -2263,7 +2242,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		_was_in_transition = true;
 		_asp_after_transition = _ctrl_state.airspeed;
 
-	// after transition we ramp up desired airspeed from the speed we had coming out of the transition
+		// after transition we ramp up desired airspeed from the speed we had coming out of the transition
+
 	} else if (_was_in_transition) {
 		_asp_after_transition += dt * 2; // increase 2m/s
 
@@ -2278,6 +2258,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 	}
 
 	_is_tecs_running = run_tecs;
+
 	if (!run_tecs) {
 		// next time we run TECS we should reinitialize states
 		_reinitialize_tecs = true;
@@ -2289,122 +2270,85 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		_reinitialize_tecs = false;
 	}
 
-	if (_mTecs.getEnabled()) {
-		/* Using mtecs library: prepare arguments for mtecs call */
-		float flightPathAngle = 0.0f;
-		float ground_speed_length = ground_speed.length();
+	if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
+		/* Force the slow downwards spiral */
+		pitch_min_rad = M_DEG_TO_RAD_F * -1.0f;
+		pitch_max_rad = M_DEG_TO_RAD_F * 5.0f;
+	}
 
-		if (ground_speed_length > FLT_EPSILON) {
-			flightPathAngle = -asinf(ground_speed(2) / ground_speed_length);
-		}
+	/* No underspeed protection in landing mode */
+	_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND
+					      || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
 
-		fwPosctrl::LimitOverride limitOverride;
+	/* Using tecs library */
+	float pitch_for_tecs = _pitch;
 
-		if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
-			/* Force the slow downwards spiral */
-			limitOverride.enablePitchMinOverride(-1.0f);
-			limitOverride.enablePitchMaxOverride(5.0f);
+	// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
+	// between multirotor and fixed wing flight
+	if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
+		math::Matrix<3, 3> R_offset;
+		R_offset.from_euler(0, M_PI_2_F, 0);
+		math::Matrix<3, 3> R_fixed_wing = _R_nb * R_offset;
+		math::Vector<3> euler = R_fixed_wing.to_euler();
+		pitch_for_tecs = euler(1);
+	}
 
-		} else if (climbout_mode) {
-			limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+	_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs, altitude, alt_sp, v_sp,
+				    _ctrl_state.airspeed, eas2tas,
+				    climbout_mode, climbout_pitch_min_rad,
+				    throttle_min, throttle_max, throttle_cruise,
+				    pitch_min_rad, pitch_max_rad);
 
-		} else {
-			limitOverride.disablePitchMinOverride();
-		}
+	struct TECS::tecs_state s;
+	_tecs.get_tecs_state(s);
 
-		if (pitch_max_special) {
-			/* Use the maximum pitch from the argument */
-			limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad);
+	struct tecs_status_s t = {};
 
-		} else {
-			/* use pitch max set by MT param */
-			limitOverride.disablePitchMaxOverride();
-		}
+	t.timestamp = s.timestamp;
 
-		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode,
-					   limitOverride);
+	switch (s.mode) {
+	case TECS::ECL_TECS_MODE_NORMAL:
+		t.mode = tecs_status_s::TECS_MODE_NORMAL;
+		break;
+
+	case TECS::ECL_TECS_MODE_UNDERSPEED:
+		t.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
+		break;
+
+	case TECS::ECL_TECS_MODE_BAD_DESCENT:
+		t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT;
+		break;
+
+	case TECS::ECL_TECS_MODE_CLIMBOUT:
+		t.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
+		break;
+	}
+
+	t.altitudeSp 		= s.altitude_sp;
+	t.altitude_filtered = s.altitude_filtered;
+	t.airspeedSp 		= s.airspeed_sp;
+	t.airspeed_filtered = s.airspeed_filtered;
+
+	t.flightPathAngleSp 		= s.altitude_rate_sp;
+	t.flightPathAngle 			= s.altitude_rate;
+	t.flightPathAngleFiltered 	= s.altitude_rate;
+
+	t.airspeedDerivativeSp 	= s.airspeed_rate_sp;
+	t.airspeedDerivative 	= s.airspeed_rate;
+
+	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.throttle_integ 	= s.throttle_integ;
+	t.pitch_integ 		= s.pitch_integ;
+
+	if (_tecs_status_pub != nullptr) {
+		orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
 
 	} else {
-		if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
-			/* Force the slow downwards spiral */
-			pitch_min_rad = M_DEG_TO_RAD_F * -1.0f;
-			pitch_max_rad = M_DEG_TO_RAD_F * 5.0f;
-		}
-
-		/* No underspeed protection in landing mode */
-		_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND
-						      || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
-
-		/* Using tecs library */
-		float pitch_for_tecs = _pitch;
-
-		// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
-		// between multirotor and fixed wing flight
-		if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
-			math::Matrix<3,3> R_offset;
-			R_offset.from_euler(0, M_PI_2_F, 0);
-			math::Matrix<3,3> R_fixed_wing = _R_nb * R_offset;
-			math::Vector<3> euler = R_fixed_wing.to_euler();
-			pitch_for_tecs = euler(1);
-		}
-
-		_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs, altitude, alt_sp, v_sp,
-					    _ctrl_state.airspeed, eas2tas,
-					    climbout_mode, climbout_pitch_min_rad,
-					    throttle_min, throttle_max, throttle_cruise,
-					    pitch_min_rad, pitch_max_rad);
-
-		struct TECS::tecs_state s;
-		_tecs.get_tecs_state(s);
-
-		struct tecs_status_s t;
-
-		t.timestamp = s.timestamp;
-
-		switch (s.mode) {
-		case TECS::ECL_TECS_MODE_NORMAL:
-			t.mode = tecs_status_s::TECS_MODE_NORMAL;
-			break;
-
-		case TECS::ECL_TECS_MODE_UNDERSPEED:
-			t.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
-			break;
-
-		case TECS::ECL_TECS_MODE_BAD_DESCENT:
-			t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT;
-			break;
-
-		case TECS::ECL_TECS_MODE_CLIMBOUT:
-			t.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
-			break;
-		}
-
-		t.altitudeSp 		= s.altitude_sp;
-		t.altitude_filtered = s.altitude_filtered;
-		t.airspeedSp 		= s.airspeed_sp;
-		t.airspeed_filtered = s.airspeed_filtered;
-
-		t.flightPathAngleSp 		= s.altitude_rate_sp;
-		t.flightPathAngle 			= s.altitude_rate;
-		t.flightPathAngleFiltered 	= s.altitude_rate;
-
-		t.airspeedDerivativeSp 	= s.airspeed_rate_sp;
-		t.airspeedDerivative 	= s.airspeed_rate;
-
-		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.throttle_integ 	= s.throttle_integ;
-		t.pitch_integ 		= s.pitch_integ;
-
-		if (_tecs_status_pub != nullptr) {
-			orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
-
-		} else {
-			_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
-		}
+		_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
 	}
 }
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 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
@@ -36,7 +36,7 @@
  *
  * Parameters defined by the L1 position control task
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 /*
@@ -223,7 +223,7 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f);
  * @unit m/s
  * @min 2.0
  * @max 10.0
- * @group FW L1 Control
+ * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 

src/modules/fw_pos_control_l1/landingslope.cpp

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *   Copyright (c) 2012-2016 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
@@ -33,8 +32,9 @@
  ****************************************************************************/
 
 /*
- * @file: landingslope.cpp
+ * @file landingslope.cpp
  *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include "landingslope.h"
@@ -47,9 +47,9 @@
 #include <mathlib/mathlib.h>
 
 void Landingslope::update(float landing_slope_angle_rad_new,
-		float flare_relative_alt_new,
-		float motor_lim_relative_alt_new,
-		float H1_virt_new)
+			  float flare_relative_alt_new,
+			  float motor_lim_relative_alt_new,
+			  float H1_virt_new)
 {
 
 	_landing_slope_angle_rad = landing_slope_angle_rad_new;
@@ -63,52 +63,65 @@ void Landingslope::update(float landing_slope_angle_rad_new,
 void Landingslope::calculateSlopeValues()
 {
 	_H0 =  _flare_relative_alt + _H1_virt;
-	_d1 = _flare_relative_alt/tanf(_landing_slope_angle_rad);
-	_flare_constant = (_H0 * _d1)/_flare_relative_alt;
-	_flare_length = - logf(_H1_virt/_H0) * _flare_constant;
+	_d1 = _flare_relative_alt / tanf(_landing_slope_angle_rad);
+	_flare_constant = (_H0 * _d1) / _flare_relative_alt;
+	_flare_length = - logf(_H1_virt / _H0) * _flare_constant;
 	_horizontal_slope_displacement = (_flare_length - _d1);
 }
 
 float  Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
 {
-	return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad);
+	return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement,
+			_landing_slope_angle_rad);
 }
 
-float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
+float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
+		float bearing_airplane_currwp)
 {
 	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
-	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) {
 		return getLandingSlopeRelativeAltitude(wp_landing_distance);
-	else
+
+	} else {
 		return 0.0f;
+	}
 }
 
 float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude)
 {
-	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
+	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement,
+			_landing_slope_angle_rad);
 }
 
-float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
+float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
+		float bearing_airplane_currwp, float wp_altitude)
 {
 	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
-	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) {
 		return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude);
-	else
+
+	} else {
 		return wp_altitude;
+	}
 }
 
-float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
+float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
+		float bearing_airplane_currwp)
 {
 	/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
-	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
-		return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
-	else
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) {
+		return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance) / _flare_constant) - _H1_virt;
+
+	} else {
 		return 0.0f;
+	}
 }
 
-float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
+float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
+		float bearing_airplane_currwp, float wp_landing_altitude)
 {
 
-	return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
+	return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp,
+			bearing_airplane_currwp);
 }
 

src/modules/fw_pos_control_l1/landingslope.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *   Copyright (c) 2012-2016 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
@@ -33,8 +32,9 @@
  ****************************************************************************/
 
 /*
- * @file: landingslope.h
+ * @file landingslope.h
  *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #ifndef LANDINGSLOPE_H_
@@ -75,7 +75,8 @@ public:
 	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
 	 * Performs check if aircraft is in front of waypoint to avoid climbout
 	 */
-	float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
+	float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
+			float bearing_airplane_currwp);
 
 
 	/**
@@ -89,44 +90,51 @@ public:
 	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
 	 * Performs check if aircraft is in front of waypoint to avoid climbout
 	 */
-	float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);
+	float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
+			float bearing_airplane_currwp, float wp_landing_altitude);
 
 	/**
 	 *
 	 * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
 	 */
-	__EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	__EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement,
+			float landing_slope_angle_rad)
 	{
-		return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative
+		return (wp_landing_distance - horizontal_slope_displacement) * tanf(
+			       landing_slope_angle_rad); //flare_relative_alt is negative
 	}
 
 	/**
 	 *
 	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
 	 */
-	__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude,
+			float horizontal_slope_displacement, float landing_slope_angle_rad)
 	{
-		return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude;
+		return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement,
+						       landing_slope_angle_rad) + wp_landing_altitude;
 	}
 
 	/**
 	 *
 	 * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
 	 */
-	__EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	__EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude,
+			float horizontal_slope_displacement, float landing_slope_angle_rad)
 	{
-		return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
+		return (slope_altitude - wp_landing_altitude) / tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
 
 	}
 
 	float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
 
-	float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
+	float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp,
+						float wp_altitude);
 
 	void update(float landing_slope_angle_rad_new,
-			float flare_relative_alt_new,
-			float motor_lim_relative_alt_new,
-			float H1_virt_new);
+		    float flare_relative_alt_new,
+		    float motor_lim_relative_alt_new,
+		    float H1_virt_new);
 
 
 	inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}