From 56f1ea9266eacd689a7b4065231d94a78ea96de6 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:25:49 +0100 Subject: Added Bottle Drop app --- makefiles/config_px4fmu-v2_default.mk | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'makefiles/config_px4fmu-v2_default.mk') diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d..a2fb63422 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -114,6 +114,11 @@ MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion +# +# OBC challenge +# +MODULES += modules/bottle_drop + # # Demo apps # -- cgit v1.2.3 From ae2de675014c701b45710b646f3a816ad2222b73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:24:11 +0200 Subject: Revert "Remove old TECS implementation - we can really only decently flight-test and support one." This reverts commit 503ded05394767d58359834e73bc63672b701dbe. Conflicts: mavlink/include/mavlink/v1.0 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp --- makefiles/config_aerocore_default.mk | 1 + makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 74 +++++++++++++++++----- 4 files changed, 61 insertions(+), 16 deletions(-) (limited to 'makefiles/config_px4fmu-v2_default.mk') diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 53a2ad1ab..0d3934bd0 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -80,6 +80,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cc0958c29..a7c10f52f 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -108,6 +108,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a498a1b40..d0a40445d 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -121,6 +121,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion 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 a1bc5c24e..78e91bbfd 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 @@ -42,8 +42,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Implementation for total energy control class: - * Thomas Gubler + * Original implementation for total energy control class: + * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) * * More details and acknowledgements in the referenced library headers. * @@ -87,6 +87,7 @@ #include #include #include +#include #include #include "landingslope.h" #include "mtecs/mTecs.h" @@ -192,6 +193,7 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; + TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; @@ -557,6 +559,23 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); + _tecs.set_time_const(_parameters.time_const); + _tecs.set_min_sink_rate(_parameters.min_sink_rate); + _tecs.set_max_sink_rate(_parameters.max_sink_rate); + _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_integrator_gain(_parameters.integrator_gain); + _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); + _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); + _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); + _tecs.set_speed_weight(_parameters.speed_weight); + _tecs.set_pitch_damping(_parameters.pitch_damping); + _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); + _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_p); + /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -618,6 +637,9 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); + return airspeed_updated; } @@ -790,6 +812,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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()) { + _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + } + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -815,6 +845,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -1123,9 +1156,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getPitchSetpoint(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1305,20 +1338,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } else { - limitOverride.disablePitchMinOverride(); + /* Using tecs library */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); } int -- cgit v1.2.3 From 73ecbbe13d1f231bf2a9c2ccaafe29065352d75c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Aug 2014 11:12:01 +0200 Subject: config_px4fmu-v2_default: include px4flow driver by default --- makefiles/config_px4fmu-v2_default.mk | 1 + 1 file changed, 1 insertion(+) (limited to 'makefiles/config_px4fmu-v2_default.mk') diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d0a40445d..5f146686c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -41,6 +41,7 @@ MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/pca8574 +MODULES += drivers/px4flow # Needs to be burned to the ground and re-written; for now, -- cgit v1.2.3 From d4a5f345aabc83506b05277b9dcf71e24700c70d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:50:38 +0100 Subject: Remove unneeded apps from build --- makefiles/config_px4fmu-v2_default.mk | 17 +---------------- makefiles/config_px4fmu-v2_test.mk | 7 +++++++ 2 files changed, 8 insertions(+), 16 deletions(-) (limited to 'makefiles/config_px4fmu-v2_default.mk') diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d17dff5bb..3c65b19e0 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -40,14 +40,8 @@ MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl -MODULES += drivers/pca8574 MODULES += drivers/px4flow - -# Needs to be burned to the ground and re-written; for now, -# just don't build it. -#MODULES += drivers/mkblctrl - # # System commands # @@ -61,7 +55,6 @@ MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top -MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd @@ -81,10 +74,8 @@ MODULES += modules/uavcan # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator # # Vehicle Control @@ -100,12 +91,6 @@ MODULES += modules/mc_pos_control # MODULES += modules/sdlog2 -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - # # Library modules # @@ -139,7 +124,7 @@ MODULES += modules/bottle_drop #MODULES += examples/math_demo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/hello_sky -MODULES += examples/px4_simple_app +#MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/daemon diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 2f4d9d6a4..6f54b960c 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -49,6 +49,13 @@ MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/conversion +# +# Modules to test-build, but not useful for test environment +# +MODULES += modules/attitude_estimator_so3 +MODULES += drivers/pca8574 +MODULES += examples/flow_position_estimator + # # Libraries # -- cgit v1.2.3 From f3f7f08e0d98be7eaba589fb6466f1d411f56b33 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 18 Nov 2014 17:20:50 -0800 Subject: Parameter xml metadata in .px4 --- Tools/px_mkfw.py | 5 +++++ makefiles/config_px4fmu-v1_default.mk | 3 +++ makefiles/config_px4fmu-v2_default.mk | 3 +++ makefiles/firmware.mk | 9 +++++++++ 4 files changed, 20 insertions(+) (limited to 'makefiles/config_px4fmu-v2_default.mk') diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index b598a65a1..c2da8a203 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -73,6 +73,7 @@ parser.add_argument("--version", action="store", help="set a version string") 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("--image", action="store", help="the firmware image") args = parser.parse_args() @@ -101,6 +102,10 @@ if args.git_identity != None: p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout desc['git_identity'] = str(p.read().strip()) p.close() +if args.parameter_xml != None: + f = open(args.parameter_xml, "rb") + bytes = f.read() + desc['parameter_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/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 9fe16fbb6..4507b506c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -132,6 +132,9 @@ MODULES += lib/launchdetection # Hardware test #MODULES += examples/hwtest +# Generate parameter XML file +GEN_PARAM_XML = 1 + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3c65b19e0..d3b8ee93e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -141,6 +141,9 @@ MODULES += modules/bottle_drop # Hardware test #MODULES += examples/hwtest +# Generate parameter XML file +GEN_PARAM_XML = 1 + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 60602e76f..21e8739aa 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -467,6 +467,7 @@ endif PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 PRODUCT_BIN = $(WORK_DIR)firmware.bin PRODUCT_ELF = $(WORK_DIR)firmware.elf +PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml .PHONY: firmware firmware: $(PRODUCT_BUNDLE) @@ -497,9 +498,17 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ +ifdef GEN_PARAM_XML + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ + --parameter_xml $(PRODUCT_PARAMXML) \ --image $< > $@ +else + $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ + --git_identity $(PX4_BASE) \ + --image $< > $@ +endif $(PRODUCT_BIN): $(PRODUCT_ELF) $(call SYM_TO_BIN,$<,$@) -- cgit v1.2.3