Browse Source

land_detector: use user-defined literals for time constants

sbg
Beat Küng 7 years ago committed by Lorenz Meier
parent
commit
123f11fcdd
  1. 7
      src/modules/land_detector/FixedwingLandDetector.h
  2. 6
      src/modules/land_detector/LandDetector.cpp
  3. 9
      src/modules/land_detector/MulticopterLandDetector.cpp
  4. 15
      src/modules/land_detector/MulticopterLandDetector.h

7
src/modules/land_detector/FixedwingLandDetector.h

@ -42,12 +42,15 @@ @@ -42,12 +42,15 @@
#pragma once
#include <drivers/drv_hrt.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_local_position.h>
#include "LandDetector.h"
using namespace time_literals;
namespace land_detector
{
@ -67,8 +70,8 @@ protected: @@ -67,8 +70,8 @@ protected:
private:
/** Time in us that landing conditions have to hold before triggering a land. */
static constexpr uint64_t LANDED_TRIGGER_TIME_US = 2000000;
static constexpr uint64_t FLYING_TRIGGER_TIME_US = 0;
static constexpr hrt_abstime LANDED_TRIGGER_TIME_US = 2_s;
static constexpr hrt_abstime FLYING_TRIGGER_TIME_US = 0_us;
struct {
param_t maxVelocity;

6
src/modules/land_detector/LandDetector.cpp

@ -47,6 +47,8 @@ @@ -47,6 +47,8 @@
#include <drivers/drv_hrt.h>
#include "uORB/topics/parameter_update.h"
using namespace time_literals;
namespace land_detector
{
@ -112,7 +114,7 @@ void LandDetector::_cycle() @@ -112,7 +114,7 @@ void LandDetector::_cycle()
const hrt_abstime now = hrt_absolute_time();
// publish at 1 Hz, very first time, or when the result has changed
if ((hrt_elapsed_time(&_landDetected.timestamp) >= 1000000) ||
if ((hrt_elapsed_time(&_landDetected.timestamp) >= 1_s) ||
(_landDetectedPub == nullptr) ||
(_landDetected.landed != landDetected) ||
(_landDetected.freefall != freefallDetected) ||
@ -156,7 +158,7 @@ void LandDetector::_cycle() @@ -156,7 +158,7 @@ void LandDetector::_cycle()
// Schedule next cycle.
work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this,
USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE_HZ));
USEC2TICK(1_s / LAND_DETECTOR_UPDATE_RATE_HZ));
} else {
exit_and_cleanup();

9
src/modules/land_detector/MulticopterLandDetector.cpp

@ -62,7 +62,6 @@ @@ -62,7 +62,6 @@
*/
#include <cmath>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include "MulticopterLandDetector.h"
@ -204,7 +203,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() @@ -204,7 +203,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
bool MulticopterLandDetector::_get_maybe_landed_state()
{
// Time base for this function
const uint64_t now = hrt_absolute_time();
const hrt_abstime now = hrt_absolute_time();
// only trigger flight conditions if we are armed
if (!_arming.armed) {
@ -240,7 +239,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state() @@ -240,7 +239,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
// if this persists for 8 seconds AND the drone is not
// falling consider it to be landed. This should even sustain
// quite acrobatic flight.
return (_min_trust_start > 0) && (hrt_elapsed_time(&_min_trust_start) > 8000000);
return (_min_trust_start > 0) && (hrt_elapsed_time(&_min_trust_start) > 8_s);
}
if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating) {
@ -293,7 +292,7 @@ float MulticopterLandDetector::_get_max_altitude() @@ -293,7 +292,7 @@ float MulticopterLandDetector::_get_max_altitude()
bool MulticopterLandDetector::_has_altitude_lock()
{
return _vehicleLocalPosition.timestamp != 0 &&
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500000 &&
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500_ms &&
_vehicleLocalPosition.z_valid;
}
@ -305,7 +304,7 @@ bool MulticopterLandDetector::_has_position_lock() @@ -305,7 +304,7 @@ bool MulticopterLandDetector::_has_position_lock()
bool MulticopterLandDetector::_is_climb_rate_enabled()
{
bool has_updated = (_vehicleLocalPositionSetpoint.timestamp != 0)
&& (hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500000);
&& (hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500_ms);
return (_control_mode.flag_control_climb_rate_enabled && has_updated && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vz));
}

15
src/modules/land_detector/MulticopterLandDetector.h

@ -44,6 +44,7 @@ @@ -44,6 +44,7 @@
#include "LandDetector.h"
#include <drivers/drv_hrt.h>
#include <systemlib/param/param.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
@ -57,6 +58,8 @@ @@ -57,6 +58,8 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position.h>
using namespace time_literals;
namespace land_detector
{
@ -79,16 +82,16 @@ protected: @@ -79,16 +82,16 @@ protected:
private:
/** Time in us that landing conditions have to hold before triggering a land. */
static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 300000;
static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms;
/** Time in us that almost landing conditions have to hold before triggering almost landed . */
static constexpr uint64_t MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 250000;
static constexpr hrt_abstime MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 250_ms;
/** Time in us that ground contact condition have to hold before triggering contact ground */
static constexpr uint64_t GROUND_CONTACT_TRIGGER_TIME_US = 350000;
static constexpr hrt_abstime GROUND_CONTACT_TRIGGER_TIME_US = 350_ms;
/** Time interval in us in which wider acceptance thresholds are used after landed. */
static constexpr uint64_t LAND_DETECTOR_LAND_PHASE_TIME_US = 2000000;
static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s;
/**
* @brief Handles for interesting parameters
@ -137,8 +140,8 @@ private: @@ -137,8 +140,8 @@ private:
vehicle_control_mode_s _control_mode {};
battery_status_s _battery {};
uint64_t _min_trust_start{0}; ///< timestamp when minimum trust was applied first
uint64_t _landed_time{0};
hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first
hrt_abstime _landed_time{0};
/* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */
float _get_takeoff_throttle();

Loading…
Cancel
Save