From 97e9f44d357f35c31a313812acec699f38e74a18 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jul 2015 22:22:41 +0200 Subject: [PATCH 1/6] TECS: Reset integrator states on first run or when DT is large --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index d13673ec9b..33beac14bb 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -45,7 +45,7 @@ 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) { + if (_update_50hz_last_usec == 0 || DT > 1.0f) { _integ3_state = baro_altitude; _integ2_state = 0.0f; _integ1_state = 0.0f; From e84d97b387fdbdb041194fcf2fbe58028f9cc364 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jul 2015 22:23:13 +0200 Subject: [PATCH 2/6] Only update TECS filter if auto is enabled. This forces the internal TECS logic to reset between auto runs --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 8856dd829d..2f915c711f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1054,8 +1054,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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); + /* update TECS filters, but only if in auto mode. */ + if (_control_mode.flag_control_auto_enabled && (_global_pos.timestamp > 0) && !_mTecs.getEnabled() && !_vehicle_status.condition_landed) { + _tecs.update_50hz(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); } /* define altitude error */ From 509b8c1c2497bcb52b67638d68063a2cf3883fd8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jul 2015 10:52:09 +0200 Subject: [PATCH 3/6] MC: Move all multicopter configs to PWM min/max params --- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 17 ++-------------- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 17 ++-------------- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 24 ++++++++++++++++++++--- 3 files changed, 25 insertions(+), 33 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 50b8320613..a530d75ba9 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 1075a5376f..dafa3bf7bf 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 3482078165..eccee07e7b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/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 From 09da389558fa82965334c9e699b5c5dd84788482 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jul 2015 10:10:15 +0200 Subject: [PATCH 4/6] TECS: Add in-air state and trigger filter state resets when not flying to ensure correct filter state initialization --- src/lib/external_lgpl/tecs/tecs.cpp | 41 +++++++++++++++++++++-------- src/lib/external_lgpl/tecs/tecs.h | 10 ++++++- 2 files changed, 39 insertions(+), 12 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 33beac14bb..53380d75da 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/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 (_update_50hz_last_usec == 0 || 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; + } diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index eb45237b7d..81a5e2d691 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/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; From 75085dc5d610f03b53ccbd89fcd93048d748657b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jul 2015 10:16:48 +0200 Subject: [PATCH 5/6] Condition TECS properly on any altitude control mode --- .../fw_pos_control_l1_main.cpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 2f915c711f..33a78f3141 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1037,7 +1037,7 @@ bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_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,20 +1045,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_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; - /* update TECS filters, but only if in auto mode. */ - if (_control_mode.flag_control_auto_enabled && (_global_pos.timestamp > 0) && !_mTecs.getEnabled() && !_vehicle_status.condition_landed) { - _tecs.update_50hz(_global_pos.alt, _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; From 455c449a02d59d2c72b5b86c95409bd1d2f6b4ae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jul 2015 23:45:30 +0200 Subject: [PATCH 6/6] Build system: Add support for airframes config file --- Tools/airframes.xml | 12 ++++++++++++ Tools/px_mkfw.py | 6 ++++++ makefiles/firmware.mk | 3 +++ 3 files changed, 21 insertions(+) create mode 100644 Tools/airframes.xml diff --git a/Tools/airframes.xml b/Tools/airframes.xml new file mode 100644 index 0000000000..f167c51dd7 --- /dev/null +++ b/Tools/airframes.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index 152444f84b..2f07fa1e73 100755 --- a/Tools/px_mkfw.py +++ b/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() diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index ebe7a09c20..5163a4c9f3 100644 --- a/makefiles/firmware.mk +++ b/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 \