forked from Archive/PX4-Autopilot
Land detector: Fix code style
This commit is contained in:
parent
948ff80b80
commit
fc29fed260
|
@ -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)) {
|
||||||
|
|
|
@ -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));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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 */
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue