aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--makefiles/config_px4fmu-v1_backside.mk1
-rw-r--r--makefiles/config_px4fmu-v1_default.mk1
-rw-r--r--makefiles/config_px4fmu-v2_default.mk1
-rw-r--r--makefiles/config_px4fmu-v2_logging.mk1
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.cpp85
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.h71
-rw-r--r--src/lib/launchdetection/LaunchDetector.cpp94
-rw-r--r--src/lib/launchdetection/LaunchDetector.h72
-rw-r--r--src/lib/launchdetection/LaunchMethod.h54
-rw-r--r--src/lib/launchdetection/launchdetection_params.c67
-rw-r--r--src/lib/launchdetection/module.mk40
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp64
12 files changed, 533 insertions, 18 deletions
diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk
index 6da78b287..1d32ee7fb 100644
--- a/makefiles/config_px4fmu-v1_backside.mk
+++ b/makefiles/config_px4fmu-v1_backside.mk
@@ -119,6 +119,7 @@ MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
+MODULES += lib/launchdetection
#
# Demo apps
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index f724962b2..2f76d81a4 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -118,6 +118,7 @@ MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
+MODULES += lib/launchdetection
#
# Demo apps
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 3d3dd7203..f0a9c0c06 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -116,6 +116,7 @@ MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
+MODULES += lib/launchdetection
#
# Demo apps
diff --git a/makefiles/config_px4fmu-v2_logging.mk b/makefiles/config_px4fmu-v2_logging.mk
index 4e3cae5ee..561ce0982 100644
--- a/makefiles/config_px4fmu-v2_logging.mk
+++ b/makefiles/config_px4fmu-v2_logging.mk
@@ -116,6 +116,7 @@ MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
MODULES += modules/dataman
+MODULES += lib/launchdetection
#
# Demo apps
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
new file mode 100644
index 000000000..0a95b46f6
--- /dev/null
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * 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 documentation4 and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name ECL 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file CatapultLaunchMethod.cpp
+ * Catpult Launch detection
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "CatapultLaunchMethod.h"
+
+CatapultLaunchMethod::CatapultLaunchMethod() :
+ last_timestamp(0),
+ integrator(0.0f),
+ launchDetected(false),
+ threshold_accel(NULL, "LAUN_CAT_A", false),
+ threshold_time(NULL, "LAUN_CAT_T", false)
+{
+
+}
+
+CatapultLaunchMethod::~CatapultLaunchMethod() {
+
+}
+
+void CatapultLaunchMethod::update(float accel_x)
+{
+ float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
+ last_timestamp = hrt_absolute_time();
+
+ if (accel_x > threshold_accel.get()) {
+ integrator += accel_x * dt;
+ if (integrator > threshold_accel.get() * threshold_time.get()) {
+ launchDetected = true;
+ }
+
+ } else {
+ /* reset integrator */
+ integrator = 0.0f;
+ launchDetected = false;
+ }
+
+}
+
+bool CatapultLaunchMethod::getLaunchDetected()
+{
+ return launchDetected;
+}
+
+void CatapultLaunchMethod::updateParams()
+{
+ threshold_accel.update();
+ threshold_time.update();
+}
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
new file mode 100644
index 000000000..e943f11e9
--- /dev/null
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * 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 documentation4 and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name ECL 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file CatapultLaunchMethod.h
+ * Catpult Launch detection
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef CATAPULTLAUNCHMETHOD_H_
+#define CATAPULTLAUNCHMETHOD_H_
+
+#include "LaunchMethod.h"
+
+#include <drivers/drv_hrt.h>
+#include <controllib/block/BlockParam.hpp>
+
+class CatapultLaunchMethod : public LaunchMethod
+{
+public:
+ CatapultLaunchMethod();
+ ~CatapultLaunchMethod();
+
+ void update(float accel_x);
+ bool getLaunchDetected();
+ void updateParams();
+
+private:
+ hrt_abstime last_timestamp;
+// float threshold_accel_raw;
+// float threshold_time;
+ float integrator;
+ bool launchDetected;
+
+ control::BlockParamFloat threshold_accel;
+ control::BlockParamFloat threshold_time;
+
+};
+
+#endif /* CATAPULTLAUNCHMETHOD_H_ */
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
new file mode 100644
index 000000000..2545e0a7e
--- /dev/null
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * 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 ECL 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file launchDetection.cpp
+ * Auto Detection for different launch methods (e.g. catapult)
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "LaunchDetector.h"
+#include "CatapultLaunchMethod.h"
+#include <systemlib/err.h>
+
+LaunchDetector::LaunchDetector() :
+ launchdetection_on(NULL, "LAUN_ALL_ON", false)
+{
+ /* init all detectors */
+ launchMethods[0] = new CatapultLaunchMethod();
+
+
+ /* update all parameters of all detectors */
+ updateParams();
+}
+
+LaunchDetector::~LaunchDetector()
+{
+
+}
+
+void LaunchDetector::update(float accel_x)
+{
+ if (launchdetection_on.get() == 1) {
+ for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+ launchMethods[i]->update(accel_x);
+ }
+ }
+}
+
+bool LaunchDetector::getLaunchDetected()
+{
+ if (launchdetection_on.get() == 1) {
+ for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+ if(launchMethods[i]->getLaunchDetected()) {
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+void LaunchDetector::updateParams() {
+
+ warnx(" LaunchDetector::updateParams()");
+ //launchdetection_on.update();
+
+ for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+ //launchMethods[i]->updateParams();
+ warnx("updating component %d", i);
+ }
+
+ warnx(" LaunchDetector::updateParams() ended");
+}
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
new file mode 100644
index 000000000..981891143
--- /dev/null
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * 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 documentation4 and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name ECL 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file LaunchDetector.h
+ * Auto Detection for different launch methods (e.g. catapult)
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef LAUNCHDETECTOR_H
+#define LAUNCHDETECTOR_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "LaunchMethod.h"
+
+#include <controllib/block/BlockParam.hpp>
+
+class __EXPORT LaunchDetector
+{
+public:
+ LaunchDetector();
+ ~LaunchDetector();
+
+ void update(float accel_x);
+ bool getLaunchDetected();
+ void updateParams();
+ bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
+
+// virtual bool getLaunchDetected();
+protected:
+private:
+ LaunchMethod* launchMethods[1];
+ control::BlockParamInt launchdetection_on;
+
+
+};
+
+
+#endif // LAUNCHDETECTOR_H
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
new file mode 100644
index 000000000..bfb5f8cb4
--- /dev/null
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * 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 documentation4 and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name ECL 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file LaunchMethod.h
+ * Base class for different launch methods
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef LAUNCHMETHOD_H_
+#define LAUNCHMETHOD_H_
+
+class LaunchMethod
+{
+public:
+ virtual void update(float accel_x) = 0;
+ virtual bool getLaunchDetected() = 0;
+ virtual void updateParams() = 0;
+protected:
+private:
+};
+
+#endif /* LAUNCHMETHOD_H_ */
diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
new file mode 100644
index 000000000..536749c88
--- /dev/null
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Author: Lorenz Meier <lm@inf.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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fw_pos_control_l1_params.c
+ *
+ * Parameters defined by the L1 position control task
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Launch detection parameters, accessible via MAVLink
+ *
+ */
+
+/* Catapult Launch detection */
+
+// @DisplayName Switch to enable launchdetection
+// @Description if set to 1 launchdetection is enabled
+// @Range 0 or 1
+PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
+
+// @DisplayName Catapult Accelerometer Threshold
+// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
+// @Range > 0
+PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
+
+// @DisplayName Catapult Time Threshold
+// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
+// @Range > 0, in seconds
+PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk
new file mode 100644
index 000000000..13648b74c
--- /dev/null
+++ b/src/lib/launchdetection/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+# Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+#
+# 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.
+#
+############################################################################
+
+#
+# Launchdetection Library
+#
+
+SRCS = LaunchDetector.cpp \
+ CatapultLaunchMethod.cpp \
+ launchdetection_params.c
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 ee0aca69e..26f6768cc 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
@@ -85,11 +85,12 @@
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
-
+#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include "landingslope.h"
+
/**
* L1 control app start / stop handling function
*
@@ -177,6 +178,9 @@ private:
/* heading hold */
float target_bearing;
+ /* Launch detection */
+ LaunchDetector launchDetector;
+
/* throttle and airspeed states */
float _airspeed_error; ///< airspeed error to setpoint in m/s
bool _airspeed_valid; ///< flag if a valid airspeed estimate exists
@@ -384,7 +388,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_motor_lim(false),
land_onslope(false),
flare_curve_alt_last(0.0f),
- _mavlink_fd(-1)
+ _mavlink_fd(-1),
+ launchDetector()
{
/* safely initialize structs */
vehicle_attitude_s _att = {0};
@@ -554,6 +559,9 @@ FixedwingPositionControl::parameters_update()
_nav_capabilities.landing_flare_length = landingslope.flare_length();
navigation_capabilities_publish();
+ /* Update Launch Detector Parameters */
+ launchDetector.updateParams();
+
return OK;
}
@@ -964,30 +972,50 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+ /* Perform launch detection */
+ bool do_fly_takeoff = false;
+ warnx("Launch detection running");
+ if (launchDetector.launchDetectionEnabled()) {
+ launchDetector.update(_accel.x);
+ if (launchDetector.getLaunchDetected()) {
+ do_fly_takeoff = true;
+ warnx("Launch detected. Taking off!");
+ }
+ } else {
+ /* no takeoff detection --> fly */
+ do_fly_takeoff = true;
+ }
+
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
- /* apply minimum pitch and limit roll if target altitude is not within 10 meters */
- if (altitude_error > 15.0f) {
+ if (do_fly_takeoff) {
- /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
- _airspeed.indicated_airspeed_m_s, eas2tas,
- true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)),
- _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
- math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+ /* apply minimum pitch and limit roll if target altitude is not within 10 meters */
+ if (altitude_error > 15.0f) {
- /* limit roll motion to ensure enough lift */
- _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
+ /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
+ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+ _airspeed.indicated_airspeed_m_s, eas2tas,
+ true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)),
+ _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+ math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
- } else {
+ /* limit roll motion to ensure enough lift */
+ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
- _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
- _airspeed.indicated_airspeed_m_s, eas2tas,
- false, math::radians(_parameters.pitch_limit_min),
- _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
- math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+ } else {
+
+ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+ _airspeed.indicated_airspeed_m_s, eas2tas,
+ false, math::radians(_parameters.pitch_limit_min),
+ _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+ math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+ }
+
+ } else {
+ throttle_max = 0.0f;
}
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-05 10:49:59 +0000