From a61a89f339394dd87de5c48951fb12d118b14e7c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Feb 2014 00:25:30 +0100 Subject: ROMFS: ignore comments and newlines in startup files, text in mixer files --- Tools/px_romfs_pruner.py | 83 ++++++++++++++++++++++++++++++++++++++++++++++++ makefiles/firmware.mk | 4 +++ 2 files changed, 87 insertions(+) create mode 100644 Tools/px_romfs_pruner.py diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py new file mode 100644 index 000000000..f6ed0a8e4 --- /dev/null +++ b/Tools/px_romfs_pruner.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2014 PX4 Development Team. All rights reserved. +# Author: Julian Oes + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + + +""" +px_romfs_pruner.py: +Delete all comments and newlines before ROMFS is converted to an image +""" + +from __future__ import print_function +import argparse +import os + +def main(): + + # Parse commandline arguments + parser = argparse.ArgumentParser(description="ROMFS pruner.") + parser.add_argument('--folder', action="store", help="ROMFS scratch folder.") + args = parser.parse_args() + + print("Pruning ROMFS files.") + + # go through + for (root, dirs, files) in os.walk(args.folder): + for file in files: + # only prune text files + if ".zip" in file or ".bin" in file: + continue + + file_path = os.path.join(root, file) + + # read file line by line + pruned_content = "" + with open(file_path, "r") as f: + for line in f: + + # handle mixer files differently than startup files + if ".mix" in file_path: + if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): + pruned_content += line + else: + if not line.isspace() and not line.strip().startswith("#"): + pruned_content += line + + # overwrite old scratch file + with open(file_path, "w") as f: + f.write(pruned_content) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index cb20d9cd1..1b646d9e0 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -355,6 +355,9 @@ ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) LIBS += $(ROMFS_OBJ) LINK_DEPS += $(ROMFS_OBJ) +# Remove all comments from startup and mixer files +ROMFS_PRUNER = $(PX4_BASE)/Tools/px_romfs_pruner.py + # Turn the ROMFS image into an object file $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) $(call BIN_TO_OBJ,$<,$@,romfs_img) @@ -372,6 +375,7 @@ ifneq ($(ROMFS_EXTRA_FILES),) $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras endif + $(Q) $(PYTHON) -u $(ROMFS_PRUNER) --folder $(ROMFS_SCRATCH) EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) -- cgit v1.2.3 From 94c40b6fa44df7d9756a4f318f6bce1de9bedd89 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Feb 2014 09:06:35 +0100 Subject: ROMFS: only identify *.mix as mixer files --- Tools/px_romfs_pruner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index f6ed0a8e4..ceef9f9be 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -67,7 +67,7 @@ def main(): for line in f: # handle mixer files differently than startup files - if ".mix" in file_path: + if file_path.endswith(".mix"): if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): pruned_content += line else: -- cgit v1.2.3 From acf680389e95c82d2e0df1253f16842d320f3504 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 17 Feb 2014 19:48:45 +0100 Subject: launchdetector: add reset, #663 --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 8 +++++++- src/lib/launchdetection/CatapultLaunchMethod.h | 3 +-- src/lib/launchdetection/LaunchDetector.cpp | 6 ++++++ src/lib/launchdetection/LaunchDetector.h | 1 + src/lib/launchdetection/LaunchMethod.h | 1 + src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 + 6 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index d5c759b17..1039b4a09 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -42,7 +42,7 @@ #include CatapultLaunchMethod::CatapultLaunchMethod() : - last_timestamp(0), + last_timestamp(hrt_absolute_time()), integrator(0.0f), launchDetected(false), threshold_accel(NULL, "LAUN_CAT_A", false), @@ -88,3 +88,9 @@ void CatapultLaunchMethod::updateParams() threshold_accel.update(); threshold_time.update(); } + +void CatapultLaunchMethod::reset() +{ + integrator = 0.0f; + launchDetected = false; +} diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index e943f11e9..b8476b4c8 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -55,11 +55,10 @@ public: void update(float accel_x); bool getLaunchDetected(); void updateParams(); + void reset(); private: hrt_abstime last_timestamp; -// float threshold_accel_raw; -// float threshold_time; float integrator; bool launchDetected; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index df9f2fe95..4109a90ba 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -59,6 +59,12 @@ LaunchDetector::~LaunchDetector() } +void LaunchDetector::reset() +{ + /* Reset all detectors */ + launchMethods[0]->reset(); +} + void LaunchDetector::update(float accel_x) { if (launchdetection_on.get() == 1) { diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 7c2ff826c..05708c526 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -53,6 +53,7 @@ class __EXPORT LaunchDetector public: LaunchDetector(); ~LaunchDetector(); + void reset(); void update(float accel_x); bool getLaunchDetected(); diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index bfb5f8cb4..0cfbab3e0 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -47,6 +47,7 @@ public: virtual void update(float accel_x) = 0; virtual bool getLaunchDetected() = 0; virtual void updateParams() = 0; + virtual void reset() = 0; protected: private: }; 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 45fdaa355..a4142266b 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 @@ -1055,6 +1055,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; + launchDetector.reset(); } if (was_circle_mode && !_l1_control.circle_mode()) { -- cgit v1.2.3 From f2b46389ee8c8e9dd73ad9ac1fc8170c759e8b1c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Feb 2014 21:42:41 +0100 Subject: fw: reset landing and takeoff state if switched to manual --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 55 ++++++++++++++++++---- 1 file changed, 45 insertions(+), 10 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 a4142266b..ab2a2439b 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 @@ -176,6 +176,8 @@ private: bool launch_detected; bool usePreTakeoffThrust; + bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) + /* Landingslope object */ Landingslope landingslope; @@ -346,6 +348,16 @@ private: * Main sensor collection task. */ void task_main() __attribute__((noreturn)); + + /* + * Reset takeoff state + */ + int reset_takeoff_state(); + + /* + * Reset landing state + */ + int reset_landing_state(); }; namespace l1_control @@ -396,7 +408,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), - usePreTakeoffThrust(false) + usePreTakeoffThrust(false), + last_manual(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -1042,20 +1055,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // mission is active _loiter_hold = false; - /* reset land state */ + /* reset landing state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { - land_noreturn_horizontal = false; - land_noreturn_vertical = false; - land_stayonground = false; - land_motor_lim = false; - land_onslope = false; + reset_landing_state(); } /* reset takeoff/launch state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { - launch_detected = false; - usePreTakeoffThrust = false; - launchDetector.reset(); + reset_takeoff_state(); } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1150,6 +1157,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; + + /* reset landing and takeoff state */ + if (!last_manual) { + reset_landing_state(); + reset_takeoff_state(); + } } if (usePreTakeoffThrust) { @@ -1160,6 +1173,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _att_sp.pitch_body = _tecs.get_pitch_demand(); + if (_control_mode.flag_control_position_enabled) { + last_manual = false; + } else { + last_manual = true; + } + return setpoint; } @@ -1310,6 +1329,22 @@ FixedwingPositionControl::task_main() _exit(0); } +int FixedwingPositionControl::reset_takeoff_state() +{ + launch_detected = false; + usePreTakeoffThrust = false; + launchDetector.reset(); +} + +int FixedwingPositionControl::reset_landing_state() +{ + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; + land_motor_lim = false; + land_onslope = false; +} + int FixedwingPositionControl::start() { -- cgit v1.2.3 From 5707118a976e63a810203b3e9ad6e74837ab7f30 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 3 Mar 2014 22:09:01 +0100 Subject: add simple trim parameter for fw attitude --- src/modules/fw_att_control/fw_att_control_main.cpp | 35 +++++++++++++++++----- 1 file changed, 28 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 17b1028f9..b040fb5df 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -166,6 +166,11 @@ private: float airspeed_min; float airspeed_trim; float airspeed_max; + + float trim_roll; + float trim_pitch; + float trim_yaw; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -197,6 +202,10 @@ private: param_t airspeed_min; param_t airspeed_trim; param_t airspeed_max; + + param_t trim_roll; + param_t trim_pitch; + param_t trim_yaw; } _parameter_handles; /**< handles for interesting parameters */ @@ -335,6 +344,10 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN"); + _parameter_handles.trim_roll = param_find("TRIM_ROLL"); + _parameter_handles.trim_pitch = param_find("TRIM_PITCH"); + _parameter_handles.trim_yaw = param_find("TRIM_YAW"); + /* fetch initial parameter values */ parameters_update(); } @@ -395,6 +408,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); + param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); + param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); + param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -670,9 +687,13 @@ FixedwingAttitudeControl::task_main() * With this mapping the stick angle is a 1:1 representation of * the commanded attitude. If more than 45 degrees are desired, * a scaling parameter can be applied to the remote. + * + * The trim gets subtracted here from the manual setpoint to get + * the intended attitude setpoint. Later, after the rate control step the + * trim is added again to the control signal. */ - roll_sp = _manual.roll * 0.75f; - pitch_sp = _manual.pitch * 0.75f; + roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f; + pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; @@ -685,7 +706,7 @@ FixedwingAttitudeControl::task_main() att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = roll_sp; att_sp.pitch_body = pitch_sp; - att_sp.yaw_body = 0.0f; + att_sp.yaw_body = 0.0f - _parameters.trim_yaw; att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ @@ -719,12 +740,12 @@ FixedwingAttitudeControl::task_main() speed_body_u, speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude - /* Run attitude RATE controllers which need the desired attitudes from above */ + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; + _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { warnx("roll_u %.4f", roll_u); } @@ -733,7 +754,7 @@ FixedwingAttitudeControl::task_main() _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; + _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); @@ -743,7 +764,7 @@ FixedwingAttitudeControl::task_main() _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; + _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { warnx("yaw_u %.4f", yaw_u); } -- cgit v1.2.3 From 87fd89ea48dab591efd38c083e356d49b369bce9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 5 Mar 2014 00:23:05 +0100 Subject: fw pitch sp and roll sp offset parameter --- src/modules/fw_att_control/fw_att_control_main.cpp | 21 +++++++++++++++------ src/modules/fw_att_control/fw_att_control_params.c | 10 ++++++++++ 2 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index b040fb5df..ab6c1415a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -170,6 +170,8 @@ private: float trim_roll; float trim_pitch; float trim_yaw; + float rollsp_offset; + float pitchsp_offset; } _parameters; /**< local copies of interesting parameters */ @@ -206,6 +208,8 @@ private: param_t trim_roll; param_t trim_pitch; param_t trim_yaw; + param_t rollsp_offset; + param_t pitchsp_offset; } _parameter_handles; /**< handles for interesting parameters */ @@ -347,6 +351,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.trim_roll = param_find("TRIM_ROLL"); _parameter_handles.trim_pitch = param_find("TRIM_PITCH"); _parameter_handles.trim_yaw = param_find("TRIM_YAW"); + _parameter_handles.rollsp_offset = param_find("FW_RSP_OFF"); + _parameter_handles.pitchsp_offset = param_find("FW_PSP_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -411,6 +417,9 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); + param_get(_parameter_handles.rollsp_offset, &(_parameters.rollsp_offset)); + param_get(_parameter_handles.pitchsp_offset, &(_parameters.pitchsp_offset)); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); @@ -665,13 +674,13 @@ FixedwingAttitudeControl::task_main() float airspeed_scaling = _parameters.airspeed_trim / airspeed; //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); - float roll_sp = 0.0f; - float pitch_sp = 0.0f; + float roll_sp = _parameters.rollsp_offset; + float pitch_sp = _parameters.pitchsp_offset; float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { - roll_sp = _att_sp.roll_body; - pitch_sp = _att_sp.pitch_body; + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -692,8 +701,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f; - pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f; + roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset; + pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 1c615094c..c80a44f2a 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -176,3 +176,13 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); // @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively // @Range 0.0 to 30 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f); + +// @DisplayName Roll Setpoint Offset +// @Description An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe +// @Range -90.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); + +// @DisplayName Pitch Setpoint Offset +// @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe +// @Range -90.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); -- cgit v1.2.3 From f60a8af30eee42e5f0c3cea996f32b14fab0f1db Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 5 Mar 2014 22:17:00 +0100 Subject: fw sp offsets: convert deg to rad --- src/modules/fw_att_control/fw_att_control_main.cpp | 32 ++++++++++++---------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ab6c1415a..5ade835ff 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -170,8 +170,10 @@ private: float trim_roll; float trim_pitch; float trim_yaw; - float rollsp_offset; - float pitchsp_offset; + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ } _parameters; /**< local copies of interesting parameters */ @@ -208,8 +210,8 @@ private: param_t trim_roll; param_t trim_pitch; param_t trim_yaw; - param_t rollsp_offset; - param_t pitchsp_offset; + param_t rollsp_offset_deg; + param_t pitchsp_offset_deg; } _parameter_handles; /**< handles for interesting parameters */ @@ -351,8 +353,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.trim_roll = param_find("TRIM_ROLL"); _parameter_handles.trim_pitch = param_find("TRIM_PITCH"); _parameter_handles.trim_yaw = param_find("TRIM_YAW"); - _parameter_handles.rollsp_offset = param_find("FW_RSP_OFF"); - _parameter_handles.pitchsp_offset = param_find("FW_PSP_OFF"); + _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); + _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -417,8 +419,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); - param_get(_parameter_handles.rollsp_offset, &(_parameters.rollsp_offset)); - param_get(_parameter_handles.pitchsp_offset, &(_parameters.pitchsp_offset)); + param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg)); + param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); + _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); + _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); /* pitch control parameters */ @@ -674,13 +678,13 @@ FixedwingAttitudeControl::task_main() float airspeed_scaling = _parameters.airspeed_trim / airspeed; //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); - float roll_sp = _parameters.rollsp_offset; - float pitch_sp = _parameters.pitchsp_offset; + float roll_sp = _parameters.rollsp_offset_rad; + float pitch_sp = _parameters.pitchsp_offset_rad; float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { - roll_sp = _att_sp.roll_body + _parameters.rollsp_offset; - pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset; + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -701,8 +705,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset; - pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset; + roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad; + pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; -- cgit v1.2.3 From 61eb228e4d8cfc95c1c4a84ed870f2b4918d7c6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Mar 2014 15:17:53 +0100 Subject: Reduce excessive stack sizes on main OS stacks. This has been tested on mavlink_beta, but needs more careful testing. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 1dc96b3c3..20edc68aa 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50 # # Stack and heap information # -CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_IDLETHREAD_STACKSIZE=4096 CONFIG_USERMAIN_STACKSIZE=4096 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -720,11 +720,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=4000 +CONFIG_SCHED_WORKSTACKSIZE=2048 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=4000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set -- cgit v1.2.3 From 935362b7a369e75c18a7aed267d7b0b304e4c430 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 8 Mar 2014 19:00:49 +0100 Subject: launchdetection: better integration of Blocks class --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 17 +++++++++-------- src/lib/launchdetection/CatapultLaunchMethod.h | 11 ++++++++--- src/lib/launchdetection/LaunchDetector.cpp | 18 +++++++----------- src/lib/launchdetection/LaunchDetector.h | 9 ++++++--- src/lib/launchdetection/LaunchMethod.h | 7 ++++++- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 ++- 6 files changed, 38 insertions(+), 27 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 1039b4a09..12f80c4a3 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -41,12 +41,16 @@ #include "CatapultLaunchMethod.h" #include -CatapultLaunchMethod::CatapultLaunchMethod() : +namespace launchdetection +{ + +CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : + SuperBlock(parent, "CAT"), last_timestamp(hrt_absolute_time()), integrator(0.0f), launchDetected(false), - threshold_accel(NULL, "LAUN_CAT_A", false), - threshold_time(NULL, "LAUN_CAT_T", false) + threshold_accel(this, "A"), + threshold_time(this, "T") { } @@ -83,14 +87,11 @@ bool CatapultLaunchMethod::getLaunchDetected() return launchDetected; } -void CatapultLaunchMethod::updateParams() -{ - threshold_accel.update(); - threshold_time.update(); -} void CatapultLaunchMethod::reset() { integrator = 0.0f; launchDetected = false; } + +} diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index b8476b4c8..55c46ff3f 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -44,17 +44,20 @@ #include "LaunchMethod.h" #include +#include #include -class CatapultLaunchMethod : public LaunchMethod +namespace launchdetection +{ + +class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock { public: - CatapultLaunchMethod(); + CatapultLaunchMethod(SuperBlock *parent); ~CatapultLaunchMethod(); void update(float accel_x); bool getLaunchDetected(); - void updateParams(); void reset(); private: @@ -68,3 +71,5 @@ private: }; #endif /* CATAPULTLAUNCHMETHOD_H_ */ + +} diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 4109a90ba..bf539701b 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -42,12 +42,16 @@ #include "CatapultLaunchMethod.h" #include +namespace launchdetection +{ + LaunchDetector::LaunchDetector() : - launchdetection_on(NULL, "LAUN_ALL_ON", false), - throttlePreTakeoff(NULL, "LAUN_THR_PRE", false) + SuperBlock(NULL, "LAUN"), + launchdetection_on(this, "ALL_ON"), + throttlePreTakeoff(this, "THR_PRE") { /* init all detectors */ - launchMethods[0] = new CatapultLaunchMethod(); + launchMethods[0] = new CatapultLaunchMethod(this); /* update all parameters of all detectors */ @@ -87,12 +91,4 @@ bool LaunchDetector::getLaunchDetected() return false; } -void LaunchDetector::updateParams() { - - launchdetection_on.update(); - throttlePreTakeoff.update(); - - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - launchMethods[i]->updateParams(); - } } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 05708c526..8066ebab3 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -45,10 +45,13 @@ #include #include "LaunchMethod.h" - +#include #include -class __EXPORT LaunchDetector +namespace launchdetection +{ + +class __EXPORT LaunchDetector : public control::SuperBlock { public: LaunchDetector(); @@ -57,7 +60,6 @@ public: void update(float accel_x); bool getLaunchDetected(); - void updateParams(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } @@ -72,5 +74,6 @@ private: }; +} #endif // LAUNCHDETECTOR_H diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index 0cfbab3e0..01fa7050e 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -41,15 +41,20 @@ #ifndef LAUNCHMETHOD_H_ #define LAUNCHMETHOD_H_ +namespace launchdetection +{ + class LaunchMethod { public: virtual void update(float accel_x) = 0; virtual bool getLaunchDetected() = 0; - virtual void updateParams() = 0; virtual void reset() = 0; + protected: private: }; +} + #endif /* LAUNCHMETHOD_H_ */ 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 774758ef4..8ba8f2ccb 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 @@ -186,7 +186,7 @@ private: float target_bearing; /* Launch detection */ - LaunchDetector launchDetector; + launchdetection::LaunchDetector launchDetector; /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s @@ -989,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* no takeoff detection --> fly */ launch_detected = true; + warnx("launchdetection off"); } } -- cgit v1.2.3 From be140eab5998fbec8c485e58e6f61a5ef950053b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 8 Mar 2014 19:15:45 +0100 Subject: geofence: make better use of Block class for updating parameters --- src/modules/navigator/geofence.cpp | 11 ++++------- src/modules/navigator/geofence.h | 6 +++--- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 9bbaf167a..f452a85f7 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -55,11 +55,13 @@ #endif static const int ERROR = -1; -Geofence::Geofence() : _fence_pub(-1), +Geofence::Geofence() : + SuperBlock(NULL, "GF"), + _fence_pub(-1), _altitude_min(0), _altitude_max(0), _verticesCount(0), - param_geofence_on(NULL, "GF_ON", false) + param_geofence_on(this, "ON") { /* Load initial params */ updateParams(); @@ -292,8 +294,3 @@ int Geofence::clearDm() { dm_clear(DM_KEY_FENCE_POINTS); } - -void Geofence::updateParams() -{ - param_geofence_on.update(); -} diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 5b56ebc7a..9628b7271 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -41,11 +41,13 @@ #define GEOFENCE_H_ #include +#include #include #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" -class Geofence { +class Geofence : public control::SuperBlock +{ private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -85,8 +87,6 @@ public: int loadFromFile(const char *filename); bool isEmpty() {return _verticesCount == 0;} - - void updateParams(); }; -- cgit v1.2.3 From 647142764fa3ffd5d99f4d43950975cc6a19bded Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 9 Mar 2014 22:08:28 +0400 Subject: position_estimator_inav: hotfix, change lower dt limit from 5 ms to 2 ms --- src/modules/position_estimator_inav/inertial_filter.c | 5 +---- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 13328edb4..7cd076948 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -15,10 +15,7 @@ void inertial_filter_predict(float dt, float x[3]) void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = w * dt; - if (ewdt > 1.0f) - ewdt = 1.0f; // prevent over-correcting - ewdt *= e; + float ewdt = e * w * dt; x[i] += ewdt; if (i == 0) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d6d03367b..a14354138 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -623,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; - dt = fmaxf(fminf(0.02, dt), 0.005); + dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; /* use GPS if it's valid and reference position initialized */ -- cgit v1.2.3 From ddb94ceba8f8813a8d434d00daf42a232e58cbfc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 10 Mar 2014 23:39:31 +0400 Subject: attitude_estimator_ekf: hotfix, do mag declination rotation matrix initialization --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 620185fb7..00bd23f83 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -477,6 +477,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); + x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; x_aposteriori_k[2] = z_k[2]; -- cgit v1.2.3 From e0cf0e3f41d0395a902032fc1cf78bc9ca76362c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 10 Mar 2014 22:46:53 +0100 Subject: fw posctrl: change 2 functions that return nothing from int to void --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 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 8ba8f2ccb..7f13df785 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 @@ -350,12 +350,12 @@ private: /* * Reset takeoff state */ - int reset_takeoff_state(); + void reset_takeoff_state(); /* * Reset landing state */ - int reset_landing_state(); + void reset_landing_state(); }; namespace l1_control @@ -1312,14 +1312,14 @@ FixedwingPositionControl::task_main() _exit(0); } -int FixedwingPositionControl::reset_takeoff_state() +void FixedwingPositionControl::reset_takeoff_state() { launch_detected = false; usePreTakeoffThrust = false; launchDetector.reset(); } -int FixedwingPositionControl::reset_landing_state() +void FixedwingPositionControl::reset_landing_state() { land_noreturn_horizontal = false; land_noreturn_vertical = false; -- cgit v1.2.3