From 78a99797fb505ec909b96375d0c9692965e728d6 Mon Sep 17 00:00:00 2001 From: Emmanuel Roussel Date: Wed, 27 Jan 2016 19:40:47 +0100 Subject: [PATCH] Implemented first try on multicopter free-fall detection --- src/modules/land_detector/LandDetector.h | 2 ++ .../land_detector/MulticopterLandDetector.cpp | 35 +++++++++++++++---- .../land_detector/MulticopterLandDetector.h | 9 +++-- .../land_detector/land_detector_params.c | 12 +++++++ 4 files changed, 49 insertions(+), 9 deletions(-) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index ce7b862684..0c3f28ff73 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -108,6 +108,8 @@ protected: static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr uint64_t FREEFALL_DETECTOR_TRIGGER_TIME = 300000; /**< usec that freefall conditions have to hold + before triggering a freefall */ static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 2000000; /**< time interval in which wider acceptance thresholds are used after arming */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 03ece61eb2..d7594487d7 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -61,12 +61,15 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _actuators{}, _arming{}, _vehicleAttitude{}, - _landTimer(0) + _sensors{}, + _landTimer(0), + _freefallTimer(0) { _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); _paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX"); _paramHandle.maxThrottle = param_find("LNDMC_THR_MAX"); + _paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR"); } void MulticopterLandDetector::initialize() @@ -78,6 +81,7 @@ void MulticopterLandDetector::initialize() _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); _manualSub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _sensorsSub = orb_subscribe(ORB_ID(sensor_combined)); // download parameters updateParameterCache(true); @@ -90,6 +94,7 @@ void MulticopterLandDetector::updateSubscriptions() orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); + orb_update(ORB_ID(sensor_combined), _sensorsSub, &_sensors); } LandDetectionResult MulticopterLandDetector::update() @@ -101,9 +106,9 @@ LandDetectionResult MulticopterLandDetector::update() if (get_freefall_state()) { _state = LANDDETECTION_RES_FREEFALL; - }else if(get_landed_state()){ + } else if (get_landed_state()) { _state = LANDDETECTION_RES_LANDED; - }else{ + } else { _state = LANDDETECTION_RES_FLYING; } @@ -112,8 +117,23 @@ LandDetectionResult MulticopterLandDetector::update() bool MulticopterLandDetector::get_freefall_state() { - //TODO - return false; + const uint64_t now = hrt_absolute_time(); + + float acc_norm = 0.0; + for (int i = 0; i < 3; i++) { + acc_norm += _sensors.accelerometer_m_s2[i] * _sensors.accelerometer_m_s2[i]; + } + acc_norm = sqrtf(acc_norm); + + bool freefall = (acc_norm < _params.acc_threshold_m_s2); + if(!freefall){ + _freefallTimer = now; + return false; + }else{ + PX4_WARN("[freefalldetector] FREE FALL !! \tacc_norm:\t%8.4f", (double)acc_norm); + } + + return (now - _freefallTimer) > FREEFALL_DETECTOR_TRIGGER_TIME; } bool MulticopterLandDetector::get_landed_state() @@ -168,8 +188,8 @@ bool MulticopterLandDetector::get_landed_state() float maxRotationScaled = _params.maxRotation_rad_s * armThresholdFactor; bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) || - (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || - (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); + (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || + (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { @@ -198,6 +218,7 @@ void MulticopterLandDetector::updateParameterCache(const bool force) param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s); _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); param_get(_paramHandle.maxThrottle, &_params.maxThrottle); + param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2); } } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 7eda290d8b..69970eb5a4 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -50,6 +50,7 @@ #include #include #include +#include #include namespace landdetection @@ -101,6 +102,7 @@ private: param_t maxVelocity; param_t maxRotation; param_t maxThrottle; + param_t acc_threshold_m_s2; } _paramHandle; struct { @@ -108,6 +110,7 @@ private: float maxVelocity; float maxRotation_rad_s; float maxThrottle; + float acc_threshold_m_s2; } _params; private: @@ -117,15 +120,17 @@ private: int _parameterSub; int _attitudeSub; int _manualSub; + int _sensorsSub; struct vehicle_local_position_s _vehicleLocalPosition; struct actuator_controls_s _actuators; struct actuator_armed_s _arming; struct vehicle_attitude_s _vehicleAttitude; struct manual_control_setpoint_s _manual; + struct sensor_combined_s _sensors; - /* timestamp in microseconds since a possible land was detected */ - uint64_t _landTimer; + uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ + uint64_t _freefallTimer; /**< timestamp in microseconds since a possible freefall was detected*/ }; } diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 1f796f1556..0be9116098 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -83,6 +83,18 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); */ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f); +/** + * Multicopter specific force threshold + * + * Multicopter threshold on the specific force measured by accelerometers in m/s^2 for free-fall detection + * + * @min 0.1 + * @max 10 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 1.0f); + /** * Fixedwing max horizontal velocity *