aboutsummaryrefslogtreecommitdiff
path: root/src/lib
diff options
context:
space:
mode:
Diffstat (limited to 'src/lib')
-rw-r--r--src/lib/ecl/ecl.h3
-rw-r--r--src/lib/ecl/l1/ecl_l1_pos_controller.cpp13
-rw-r--r--src/lib/external_lgpl/tecs/tecs.cpp5
-rw-r--r--src/lib/external_lgpl/tecs/tecs.h27
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.cpp8
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.h3
-rw-r--r--src/lib/launchdetection/LaunchDetector.cpp6
-rw-r--r--src/lib/launchdetection/LaunchDetector.h1
-rw-r--r--src/lib/launchdetection/LaunchMethod.h1
-rw-r--r--src/lib/launchdetection/launchdetection_params.c48
10 files changed, 75 insertions, 40 deletions
diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
index e0f207696..aa3c5000a 100644
--- a/src/lib/ecl/ecl.h
+++ b/src/lib/ecl/ecl.h
@@ -38,7 +38,6 @@
*/
#include <drivers/drv_hrt.h>
-#include <geo/geo.h>
#define ecl_absolute_time hrt_absolute_time
-#define ecl_elapsed_time hrt_elapsed_time \ No newline at end of file
+#define ecl_elapsed_time hrt_elapsed_time
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 3b68a0a4e..d1c864d78 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -38,6 +38,8 @@
*
*/
+#include <float.h>
+
#include "ecl_l1_pos_controller.h"
float ECL_L1_Pos_Controller::nav_roll()
@@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
/* calculate the vector from waypoint A to current position */
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
- /* store the normalized vector from waypoint A to current position */
- math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+ math::Vector<2> vector_A_to_airplane_unit;
+
+ /* prevent NaN when normalizing */
+ if (vector_A_to_airplane.length() > FLT_EPSILON) {
+ /* store the normalized vector from waypoint A to current position */
+ vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+ } else {
+ vector_A_to_airplane_unit = vector_A_to_airplane;
+ }
/* calculate eta angle towards the loiter center */
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 0f28bccad..3730b1920 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -3,13 +3,10 @@
#include "tecs.h"
#include <ecl/ecl.h>
#include <systemlib/err.h>
+#include <geo/geo.h>
using namespace math;
-#ifndef CONSTANTS_ONE_G
-#define CONSTANTS_ONE_G GRAVITY
-#endif
-
/**
* @file tecs.cpp
*
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index d1ebacda1..5cafb1c79 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -28,16 +28,7 @@ class __EXPORT TECS
{
public:
TECS() :
-
- _airspeed_enabled(false),
- _throttle_slewrate(0.0f),
- _climbOutDem(false),
- _hgt_dem_prev(0.0f),
- _hgt_dem_adj_last(0.0f),
- _hgt_dem_in_old(0.0f),
- _TAS_dem_last(0.0f),
- _TAS_dem_adj(0.0f),
- _TAS_dem(0.0f),
+ _pitch_dem(0.0f),
_integ1_state(0.0f),
_integ2_state(0.0f),
_integ3_state(0.0f),
@@ -45,8 +36,16 @@ public:
_integ5_state(0.0f),
_integ6_state(0.0f),
_integ7_state(0.0f),
- _pitch_dem(0.0f),
_last_pitch_dem(0.0f),
+ _vel_dot(0.0f),
+ _TAS_dem(0.0f),
+ _TAS_dem_last(0.0f),
+ _hgt_dem_in_old(0.0f),
+ _hgt_dem_adj_last(0.0f),
+ _hgt_dem_prev(0.0f),
+ _TAS_dem_adj(0.0f),
+ _STEdotErrLast(0.0f),
+ _climbOutDem(false),
_SPE_dem(0.0f),
_SKE_dem(0.0f),
_SPEdot_dem(0.0f),
@@ -55,9 +54,9 @@ public:
_SKE_est(0.0f),
_SPEdot(0.0f),
_SKEdot(0.0f),
- _vel_dot(0.0f),
- _STEdotErrLast(0.0f) {
-
+ _airspeed_enabled(false),
+ _throttle_slewrate(0.0f)
+ {
}
bool airspeed_sensor_enabled() {
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 <systemlib/err.h>
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/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index 63a8981aa..45d7957f1 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -45,28 +45,46 @@
#include <systemlib/param/param.h>
/*
- * Launch detection parameters, accessible via MAVLink
+ * Catapult 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
+/**
+ * Enable launch detection.
+ *
+ * @min 0
+ * @max 1
+ * @group Launch detection
+ */
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
+/**
+ * Catapult accelerometer theshold.
+ *
+ * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
+ *
+ * @min 0
+ * @group Launch detection
+ */
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
+/**
+ * Catapult time theshold.
+ *
+ * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
+ *
+ * @min 0
+ * @group Launch detection
+ */
PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
-// @DisplayName Throttle setting while detecting the launch
-// @Description The throttle is set to this value while the system is waiting for the takeoff
-// @Range 0 to 1
+/**
+ * Throttle setting while detecting launch.
+ *
+ * The throttle is set to this value while the system is waiting for the take-off.
+ *
+ * @min 0
+ * @max 1
+ * @group Launch detection
+ */
PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-19 00:53:32 +0000