Land detector: Fix code style

This commit is contained in:
Lorenz Meier 2015-10-19 13:47:57 +02:00
parent 948ff80b80
commit fc29fed260
5 changed files with 31 additions and 25 deletions

View File

@ -49,18 +49,18 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
_paramHandle(), _paramHandle(),
_params(), _params(),
_vehicleLocalPositionSub(-1), _vehicleLocalPositionSub(-1),
_vehicleLocalPosition({}), _vehicleLocalPosition( {}),
_airspeedSub(-1), _airspeedSub(-1),
_vehicleStatusSub(-1), _vehicleStatusSub(-1),
_armingSub(-1), _armingSub(-1),
_airspeed{}, _airspeed{},
_vehicleStatus{}, _vehicleStatus{},
_arming{}, _arming{},
_parameterSub(-1), _parameterSub(-1),
_velocity_xy_filtered(0.0f), _velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f), _velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f), _airspeed_filtered(0.0f),
_landDetectTrigger(0) _landDetectTrigger(0)
{ {
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
@ -101,10 +101,12 @@ bool FixedwingLandDetector::update()
if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx * 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)) { if (PX4_ISFINITE(val)) {
_velocity_xy_filtered = val; _velocity_xy_filtered = val;
} }
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz);
if (PX4_ISFINITE(val)) { if (PX4_ISFINITE(val)) {

View File

@ -47,12 +47,11 @@
LandDetector::LandDetector() : LandDetector::LandDetector() :
_landDetectedPub(0), _landDetectedPub(0),
_landDetected({0, false}), _landDetected( {0, false}),
_arming_time(0), _arming_time(0),
_taskShouldExit(false), _taskShouldExit(false),
_taskIsRunning(false), _taskIsRunning(false),
_work{} _work{} {
{
// ctor // ctor
} }
@ -111,7 +110,8 @@ void LandDetector::cycle()
} }
if (!_taskShouldExit) { 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));
} }
} }

View File

@ -99,7 +99,8 @@ protected:
static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold
before triggering a land */ 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: protected:
orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ orb_advert_t _landDetectedPub; /**< publisher for position in local frame */

View File

@ -99,13 +99,14 @@ bool MulticopterLandDetector::update()
if (!_arming.armed) { if (!_arming.armed) {
_arming_time = 0; _arming_time = 0;
return true; return true;
} else if (_arming_time == 0) { } else if (_arming_time == 0) {
_arming_time = hrt_absolute_time(); _arming_time = hrt_absolute_time();
} }
// return status based on armed state if no position lock is available // return status based on armed state if no position lock is available
if (_vehicleGlobalPosition.timestamp == 0 || if (_vehicleGlobalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) {
// no position lock - not landed if armed // no position lock - not landed if armed
return !_arming.armed; return !_arming.armed;
@ -129,14 +130,14 @@ bool MulticopterLandDetector::update()
// check if we are moving horizontally // check if we are moving horizontally
bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity + _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 // next look if all rotation angles are not moving
float maxRotationScaled = _params.maxRotation * armThresholdFactor; float maxRotationScaled = _params.maxRotation * armThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) || bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
// check if thrust output is minimal (about half of default) // check if thrust output is minimal (about half of default)
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;

View File

@ -152,6 +152,7 @@ static int land_detector_start(const char *mode)
return 1; return 1;
} }
} }
printf("\n"); printf("\n");
} }
@ -176,6 +177,7 @@ int land_detector_main(int argc, char *argv[])
warnx("land_detector start failed"); warnx("land_detector start failed");
return 1; return 1;
} }
return 0; return 0;
} }