Merge branch 'beta' into stable

ROMFS/px4fmu_common/init.d/4010_dji_f330

@@ -21,25 +21,12 @@ then
 	param set MC_YAWRATE_P 0.2
 	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
-	param set PWM_DISARMED 900
+	# DJI ESCs do not support calibration and need a higher min
 	param set PWM_MIN 1230
-	param set PWM_MAX 1950
 fi
 
 # Transitional support: ensure suitable PWM min/max param values
-if param compare PWM_MIN 1000
+if param compare PWM_MIN 1075
 then
 	param set PWM_MIN 1230
 fi
-if param compare PWM_MAX 2000
-then
-	param set PWM_MAX 1950
-fi
-if param compare PWM_DISARMED 0
-then
-	param set PWM_DISARMED 900
-fi
-
-set PWM_DISARMED p:PWM_DISARMED
-set PWM_MIN p:PWM_MIN
-set PWM_MAX p:PWM_MAX

ROMFS/px4fmu_common/init.d/4011_dji_f450

@@ -22,25 +22,12 @@ then
 	param set MC_YAWRATE_P 0.3
 	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
-	param set PWM_DISARMED 900
+	# DJI ESCs do not support calibration and need a higher min
 	param set PWM_MIN 1230
-	param set PWM_MAX 1950
 fi
 
 # Transitional support: ensure suitable PWM min/max param values
-if param compare PWM_MIN 1000
+if param compare PWM_MIN 1075
 then
 	param set PWM_MIN 1230
 fi
-if param compare PWM_MAX 2000
-then
-	param set PWM_MAX 1950
-fi
-if param compare PWM_DISARMED 0
-then
-	param set PWM_DISARMED 900
-fi
-
-set PWM_DISARMED p:PWM_DISARMED
-set PWM_MIN p:PWM_MIN
-set PWM_MAX p:PWM_MAX

ROMFS/px4fmu_common/init.d/rc.mc_defaults

@@ -14,13 +14,31 @@ then
 	param set NAV_ACC_RAD 2.0
 	param set RTL_RETURN_ALT 30.0
 	param set RTL_DESCEND_ALT 10.0
+	param set PWM_DISARMED 900
+	param set PWM_MIN 1075
+	param set PWM_MAX 1950
+fi
+
+# Transitional support: ensure suitable PWM min/max param values
+if param compare PWM_MIN 1000
+then
+	param set PWM_MIN 1075
+fi
+if param compare PWM_MAX 2000
+then
+	param set PWM_MAX 1950
+fi
+if param compare PWM_DISARMED 0
+then
+	param set PWM_DISARMED 900
 fi
 
 # set environment variables (!= parameters)
 set PWM_RATE 400
-set PWM_DISARMED 900
-set PWM_MIN 1075
-set PWM_MAX 2000
+# tell the mixer to use parameters for these instead
+set PWM_DISARMED p:PWM_DISARMED
+set PWM_MIN p:PWM_MIN
+set PWM_MAX p:PWM_MAX
 
 # This is the gimbal pass mixer
 set MIXER_AUX pass

Tools/airframes.xml

@@ -0,0 +1,12 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<airframes>
+  <airframe_group name="StandardPlane">
+  	<airframe name="Multiplex Easystar 1/2" id="2100"/>
+  	<airframe name="Generic AERT" id="2101"/>
+  </airframe_group>
+  <airframe_group name="QuadRotorX">
+  	<airframe name="Generic 10 inch Quad" id="4001"/>
+  	<airframe name="DJI F330 8 inch Quad" id="4010"/>
+  	<airframe name="DJI F450 10 inch Quad" id="4011"/>
+  </airframe_group>
+</airframes>

Tools/px_mkfw.py

@@ -74,6 +74,7 @@ parser.add_argument("--summary",	action="store", help="set a brief description")
 parser.add_argument("--description",	action="store", help="set a longer description")
 parser.add_argument("--git_identity",	action="store", help="the working directory to check for git identity")
 parser.add_argument("--parameter_xml",	action="store", help="the parameters.xml file")
+parser.add_argument("--airframe_xml",	action="store", help="the airframes.xml file")
 parser.add_argument("--image",		action="store", help="the firmware image")
 args = parser.parse_args()
 
@@ -107,6 +108,11 @@ if args.parameter_xml != None:
 	bytes = f.read()
 	desc['parameter_xml_size'] = len(bytes)
 	desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8')
+if args.airframe_xml != None:
+	f = open(args.airframe_xml, "rb")
+	bytes = f.read()
+	desc['airframe_xml_size'] = len(bytes)
+	desc['airframe_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8')
 if args.image != None:
 	f = open(args.image, "rb")
 	bytes = f.read()

makefiles/firmware.mk

@@ -463,6 +463,7 @@ PRODUCT_BUNDLE		 = $(WORK_DIR)firmware.px4
 PRODUCT_BIN		 = $(WORK_DIR)firmware.bin
 PRODUCT_ELF		 = $(WORK_DIR)firmware.elf
 PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml
+PRODUCT_AIRFRAMEXML = $(WORK_DIR)/airframes.xml
 
 .PHONY:			firmware
 firmware:		$(PRODUCT_BUNDLE)
@@ -495,9 +496,11 @@ $(PRODUCT_BUNDLE):	$(PRODUCT_BIN)
 	@$(ECHO) %% Generating $@
 ifdef GEN_PARAM_XML
 	$(Q) $(PYTHON) $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml
+	$(Q) $(COPY) $(PX4_BASE)/Tools/airframes.xml $(WORK_DIR)/airframes.xml
 	$(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \
 		--git_identity $(PX4_BASE) \
 		--parameter_xml $(PRODUCT_PARAMXML) \
+		--airframe_xml $(PRODUCT_AIRFRAMEXML) \
 		--image $< > $@
 else
 	$(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \

src/lib/external_lgpl/tecs/tecs.cpp

@@ -27,7 +27,8 @@ using namespace math;
  *
  */
 
-void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth)
+void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat,
+	const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air)
 {
 	// Implement third order complementary filter for height and height rate
 	// estimted height rate = _integ2_state
@@ -45,12 +46,24 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3
 	// 	DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2),
 	// 	accel_earth(0), accel_earth(1), accel_earth(2));
 
-	if (DT > 1.0f) {
+	bool reset_altitude = false;
+
+	_in_air = in_air;
+
+	if (_update_50hz_last_usec == 0 || DT > DT_MAX) {
+		DT            = 0.02f; // when first starting TECS, use a
+		// small time constant
+		reset_altitude = true;
+	}
+
+	if (!altitude_lock || !in_air) {
+		reset_altitude = true;
+	}
+
+	if (reset_altitude) {
 		_integ3_state = baro_altitude;
 		_integ2_state = 0.0f;
 		_integ1_state = 0.0f;
-		DT            = 0.02f; // when first starting TECS, use a
-		// small time constant
 	}
 
 	_update_50hz_last_usec = now;
@@ -70,7 +83,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3
 
 	// If more than 1 second has elapsed since last update then reset the integrator state
 	// to the measured height
-	if (DT > 1.0f) {
+	if (reset_altitude) {
 		_integ3_state = baro_altitude;
 
 	} else {
@@ -95,6 +108,8 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3
 		_vel_dot = 0.0f;
 	}
 
+	_states_initalized = true;
+
 }
 
 void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
@@ -103,7 +118,6 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
 	// Calculate time in seconds since last update
 	uint64_t now = ecl_absolute_time();
 	float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f;
-	_update_speed_last_usec = now;
 
 	// Convert equivalent airspeeds to true airspeeds
 
@@ -113,7 +127,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
 	_TASmin   = indicated_airspeed_min * EAS2TAS;
 
 	// Reset states of time since last update is too large
-	if (DT > 1.0f) {
+	if (_update_speed_last_usec == 0 || DT > 1.0f || !_in_air) {
 		_integ5_state = (_EAS * EAS2TAS);
 		_integ4_state = 0.0f;
 		DT            = 0.1f; // when first starting TECS, use a
@@ -146,7 +160,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
 	_integ5_state = _integ5_state + integ5_input * DT;
 	// limit the airspeed to a minimum of 3 m/s
 	_integ5_state = max(_integ5_state, 3.0f);
-
+	_update_speed_last_usec = now;
 }
 
 void TECS::_update_speed_demand(void)
@@ -471,7 +485,7 @@ void TECS::_update_pitch(void)
 void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad)
 {
 	// Initialise states and variables if DT > 1 second or in climbout
-	if (_DT > 1.0f) {
+	if (_update_pitch_throttle_last_usec == 0 || _DT > 1.0f || !_in_air) {
 		_integ6_state      = 0.0f;
 		_integ7_state      = 0.0f;
 		_last_throttle_dem = throttle_cruise;
@@ -484,7 +498,7 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
 		_TAS_dem_adj       = _TAS_dem;
 		_underspeed        = false;
 		_badDescent        = false;
-		_DT                = 0.1f; // when first starting TECS, use a
+		_DT                = DT_MIN; // when first starting TECS, use a
 		// small time constant
 
 	} else if (_climbOutDem) {
@@ -512,10 +526,13 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 				 float throttle_min, float throttle_max, float throttle_cruise,
 				 float pitch_limit_min, float pitch_limit_max)
 {
+	if (!_states_initalized) {
+		return;
+	}
+
 	// Calculate time in seconds since last update
 	uint64_t now = ecl_absolute_time();
 	_DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f;
-	_update_pitch_throttle_last_usec = now;
 
 	// printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n",
 	// 	_DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max);
@@ -583,4 +600,6 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 	_tecs_state.ptch     = _pitch_dem;
 	_tecs_state.dspd_dem = _TAS_rate_dem;
 
+	_update_pitch_throttle_last_usec = now;
+
 }

src/lib/external_lgpl/tecs/tecs.h

@@ -80,6 +80,8 @@ public:
 		_SPEdot(0.0f),
 		_SKEdot(0.0f),
 		_airspeed_enabled(false),
+		_states_initalized(false),
+		_in_air(false),
 		_throttle_slewrate(0.0f)
 	{
 	}
@@ -95,7 +97,8 @@ public:
 	// Update of the estimated height and height rate internal state
 	// Update of the inertial speed rate internal state
 	// Should be called at 50Hz or greater
-	void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth);
+	void update_state(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat,
+		const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air);
 
 	// Update the control loop calculations
 	void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
@@ -376,7 +379,12 @@ private:
 	// Time since last update of main TECS loop (seconds)
 	float _DT;
 
+	static constexpr float DT_MIN = 0.1f;
+	static constexpr float DT_MAX = 1.0f;
+
 	bool _airspeed_enabled;
+	bool _states_initalized;
+	bool _in_air;
 	float _throttle_slewrate;
 	float _indicated_airspeed_min;
 	float _indicated_airspeed_max;

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1037,7 +1037,7 @@ bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
-	float dt = FLT_MIN; // Using non zero value to a avoid division by zero
+	float dt = 0.01; // Using non zero value to a avoid division by zero
 	if (_control_position_last_called > 0) {
 		dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
 	}
@@ -1045,19 +1045,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	bool setpoint = true;
 
-	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
-	calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
-
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
 	/* filter speed and altitude for controller */
 	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
-	if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) {
-		_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
+	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
+	bool in_air_alt_control = (!_vehicle_status.condition_landed &&
+		(_control_mode.flag_control_auto_enabled ||
+		 _control_mode.flag_control_velocity_enabled ||
+		 _control_mode.flag_control_altitude_enabled));
+
+	/* update TECS filters */
+	if (!_mTecs.getEnabled()) {
+		_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _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);
+
 	/* define altitude error */
 	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;