diff options
-rw-r--r-- | Tools/px_romfs_pruner.py | 83 | ||||
-rw-r--r-- | makefiles/firmware.mk | 4 | ||||
-rw-r--r-- | nuttx-configs/px4fmu-v1/nsh/defconfig | 6 | ||||
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.cpp | 21 | ||||
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.h | 14 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.cpp | 24 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.h | 10 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchMethod.h | 8 | ||||
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 3 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 56 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_params.c | 10 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 55 | ||||
-rw-r--r-- | src/modules/navigator/geofence.cpp | 11 | ||||
-rw-r--r-- | src/modules/navigator/geofence.h | 6 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/inertial_filter.c | 5 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 |
16 files changed, 253 insertions, 65 deletions
diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py new file mode 100644 index 000000000..ceef9f9be --- /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 <joes@student.ethz.ch> + +# 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 file_path.endswith(".mix"): + 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) 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 diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index d5c759b17..12f80c4a3 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -41,12 +41,16 @@ #include "CatapultLaunchMethod.h" #include <systemlib/err.h> -CatapultLaunchMethod::CatapultLaunchMethod() : - last_timestamp(0), +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,8 +87,11 @@ bool CatapultLaunchMethod::getLaunchDetected() return launchDetected; } -void CatapultLaunchMethod::updateParams() + +void CatapultLaunchMethod::reset() { - threshold_accel.update(); - threshold_time.update(); + integrator = 0.0f; + launchDetected = false; +} + } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index e943f11e9..55c46ff3f 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -44,22 +44,24 @@ #include "LaunchMethod.h" #include <drivers/drv_hrt.h> +#include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> -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: hrt_abstime last_timestamp; -// float threshold_accel_raw; -// float threshold_time; float integrator; bool launchDetected; @@ -69,3 +71,5 @@ private: }; #endif /* CATAPULTLAUNCHMETHOD_H_ */ + +} diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index df9f2fe95..bf539701b 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -42,12 +42,16 @@ #include "CatapultLaunchMethod.h" #include <systemlib/err.h> +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 */ @@ -59,6 +63,12 @@ LaunchDetector::~LaunchDetector() } +void LaunchDetector::reset() +{ + /* Reset all detectors */ + launchMethods[0]->reset(); +} + void LaunchDetector::update(float accel_x) { if (launchdetection_on.get() == 1) { @@ -81,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 7c2ff826c..8066ebab3 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -45,18 +45,21 @@ #include <stdint.h> #include "LaunchMethod.h" - +#include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> -class __EXPORT LaunchDetector +namespace launchdetection +{ + +class __EXPORT LaunchDetector : public control::SuperBlock { public: LaunchDetector(); ~LaunchDetector(); + void reset(); void update(float accel_x); bool getLaunchDetected(); - void updateParams(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } @@ -71,5 +74,6 @@ private: }; +} #endif // LAUNCHDETECTOR_H diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index bfb5f8cb4..01fa7050e 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -41,14 +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/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 3881bdf55..10a6cd2c5 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -473,6 +473,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]; 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..5ade835ff 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,15 @@ private: float airspeed_min; float airspeed_trim; float airspeed_max; + + float trim_roll; + float trim_pitch; + float trim_yaw; + 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 */ struct { @@ -197,6 +206,12 @@ 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; + param_t rollsp_offset_deg; + param_t pitchsp_offset_deg; } _parameter_handles; /**< handles for interesting parameters */ @@ -335,6 +350,12 @@ 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"); + _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(); } @@ -395,6 +416,15 @@ 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)); + 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 */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -648,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 = 0.0f; - float pitch_sp = 0.0f; + 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; - pitch_sp = _att_sp.pitch_body; + 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 */ @@ -670,9 +700,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 + _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; @@ -685,7 +719,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 +753,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 +767,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 +777,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); } 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); 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 ed6d8792c..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 @@ -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; @@ -184,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 @@ -344,6 +346,16 @@ private: * Main sensor collection task. */ void task_main() __attribute__((noreturn)); + + /* + * Reset takeoff state + */ + void reset_takeoff_state(); + + /* + * Reset landing state + */ + void reset_landing_state(); }; namespace l1_control @@ -389,6 +401,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), launch_detected(false), + last_manual(false), usePreTakeoffThrust(false), flare_curve_alt_last(0.0f), launchDetector(), @@ -976,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* no takeoff detection --> fly */ launch_detected = true; + warnx("launchdetection off"); } } @@ -1022,19 +1036,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; + reset_takeoff_state(); } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1131,6 +1140,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) { @@ -1141,6 +1156,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; } @@ -1291,6 +1312,22 @@ FixedwingPositionControl::task_main() _exit(0); } +void FixedwingPositionControl::reset_takeoff_state() +{ + launch_detected = false; + usePreTakeoffThrust = false; + launchDetector.reset(); +} + +void 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() { 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 <uORB/topics/fence.h> +#include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> #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(); }; 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 37f507086..368fa6ee2 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 */ |