From e6b3cf3ac95dc35d5b52e1a3b73695efc85499db Mon Sep 17 00:00:00 2001 From: Emmanuel Roussel Date: Mon, 8 Feb 2016 10:38:19 +0100 Subject: [PATCH] Fix code style (Ran AStyle) --- src/modules/land_detector/FixedwingLandDetector.cpp | 6 ++++-- src/modules/land_detector/LandDetector.cpp | 4 ++-- src/modules/land_detector/LandDetector.h | 6 ++++-- .../land_detector/MulticopterLandDetector.cpp | 12 +++++++++--- src/modules/land_detector/land_detector_params.c | 3 ++- 5 files changed, 21 insertions(+), 10 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index de41a1aa41..af3cfaa61f 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -94,9 +94,11 @@ LandDetectionResult FixedwingLandDetector::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; } diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index a8b61fb1ae..04d5f12f60 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -103,8 +103,8 @@ void LandDetector::cycle() } LandDetectionResult current_state = update(); - bool landDetected = (current_state==LANDDETECTION_RES_LANDED); - bool freefallDetected = (current_state==LANDDETECTION_RES_FREEFALL); + bool landDetected = (current_state == LANDDETECTION_RES_LANDED); + bool freefallDetected = (current_state == LANDDETECTION_RES_FREEFALL); // publish if land detection state has changed if ((_landDetected.landed != landDetected) || (_landDetected.freefall != freefallDetected)) { diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 5e2bd1920c..05b229b7a5 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -108,13 +108,15 @@ 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 LAND_DETECTOR_ARM_PHASE_TIME = 2000000; /**< time interval in which wider acceptance thresholds are used after arming */ + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = + 2000000; /**< time interval in which wider acceptance thresholds are used after arming */ protected: orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ uint64_t _arming_time; /**< timestamp of arming time */ - LandDetectionResult _state; /**< Result of land detection. Can be LANDDETECTION_RES_FLYING, LANDDETECTION_RES_LANDED or LANDDETECTION_RES_FREEFALL */ + LandDetectionResult + _state; /**< Result of land detection. Can be LANDDETECTION_RES_FLYING, LANDDETECTION_RES_LANDED or LANDDETECTION_RES_FREEFALL */ private: bool _taskShouldExit; /**< true if it is requested that this task should exit */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index d10b1189fa..6729b672e3 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -107,8 +107,10 @@ LandDetectionResult MulticopterLandDetector::update() if (get_freefall_state()) { _state = LANDDETECTION_RES_FREEFALL; + } else if (get_landed_state()) { _state = LANDDETECTION_RES_LANDED; + } else { _state = LANDDETECTION_RES_FLYING; } @@ -118,19 +120,23 @@ LandDetectionResult MulticopterLandDetector::update() bool MulticopterLandDetector::get_freefall_state() { - if(_params.acc_threshold_m_s2 < 0.1f || _params.acc_threshold_m_s2 > 10.0f){ //if parameter is set to zero or invalid, disable free-fall detection. + if (_params.acc_threshold_m_s2 < 0.1f + || _params.acc_threshold_m_s2 > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection. 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); //norm of specific force. Should be close to 9.8 m/s^2 when landed. bool freefall = (acc_norm < _params.acc_threshold_m_s2); //true if we are currently falling + if (!freefall || _freefallTimer == 0) { //reset timer if uav not falling _freefallTimer = now; return false; @@ -191,8 +197,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) { diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 2b5b411c73..472ece0b5d 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -105,7 +105,8 @@ PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 2.0f); * * @group Land Detector */ -PARAM_DEFINE_INT32(LNDMC_FFALL_TRIG, 300); //minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h +PARAM_DEFINE_INT32(LNDMC_FFALL_TRIG, + 300); //minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h /** * Fixedwing max horizontal velocity