From fc29fed26000a180c9cdd6a9666aed908ca9a43f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:47:57 +0200 Subject: [PATCH] Land detector: Fix code style --- .../land_detector/FixedwingLandDetector.cpp | 28 ++++++++++--------- src/modules/land_detector/LandDetector.cpp | 14 +++++----- src/modules/land_detector/LandDetector.h | 3 +- .../land_detector/MulticopterLandDetector.cpp | 9 +++--- .../land_detector/land_detector_main.cpp | 2 ++ 5 files changed, 31 insertions(+), 25 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 7cfe65bb7f..f8066241c5 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -49,18 +49,18 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle(), _params(), _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _vehicleStatusSub(-1), - _armingSub(-1), - _airspeed{}, - _vehicleStatus{}, - _arming{}, - _parameterSub(-1), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0) + _vehicleLocalPosition( {}), + _airspeedSub(-1), + _vehicleStatusSub(-1), + _armingSub(-1), + _airspeed{}, + _vehicleStatus{}, + _arming{}, + _parameterSub(-1), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); @@ -101,10 +101,12 @@ bool FixedwingLandDetector::update() if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; } + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); if (PX4_ISFINITE(val)) { diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index ae6a1b5037..3040e23407 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -47,12 +47,11 @@ LandDetector::LandDetector() : _landDetectedPub(0), - _landDetected({0, false}), - _arming_time(0), - _taskShouldExit(false), - _taskIsRunning(false), - _work{} -{ + _landDetected( {0, false}), + _arming_time(0), + _taskShouldExit(false), + _taskIsRunning(false), +_work{} { // ctor } @@ -111,7 +110,8 @@ void LandDetector::cycle() } if (!_taskShouldExit) { - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); + work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, + USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); } } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index d28863b499..b86e431150 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -99,7 +99,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 LAND_DETECTOR_ARM_PHASE_TIME = 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = + 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ protected: orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 1d0a6b92dc..fa636e496c 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -99,13 +99,14 @@ bool MulticopterLandDetector::update() if (!_arming.armed) { _arming_time = 0; return true; + } else if (_arming_time == 0) { _arming_time = hrt_absolute_time(); } // return status based on armed state if no position lock is available if (_vehicleGlobalPosition.timestamp == 0 || - hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { + hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { // no position lock - not landed if armed return !_arming.armed; @@ -129,14 +130,14 @@ bool MulticopterLandDetector::update() // check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity - && _vehicleStatus.condition_global_position_valid; + && _vehicleStatus.condition_global_position_valid; // next look if all rotation angles are not moving float maxRotationScaled = _params.maxRotation * armThresholdFactor; bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) || - (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || - (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); + (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || + (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index c4d0e5194d..0aa6e1fad2 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -152,6 +152,7 @@ static int land_detector_start(const char *mode) return 1; } } + printf("\n"); } @@ -176,6 +177,7 @@ int land_detector_main(int argc, char *argv[]) warnx("land_detector start failed"); return 1; } + return 0; }