Skip to content

Commit

Permalink
Cherry pick the directory and voted_sensors_update.h from PR #9756.
Browse files Browse the repository at this point in the history 
Consolidate _update_params() methods for improved inheritance from the LandDetector base class.
Move common uORB::Subscriptions to the base class for inheritance.
Deprecate redundant override methods.
  • Loading branch information
@mcsauder
mcsauder committed Oct 14, 2019
1 parent b72d010 commit adb10a1
Show file tree
Hide file tree
Showing 8 changed files with 63 additions and 102 deletions.
33 changes: 8 additions & 25 deletions src/modules/land_detector/FixedwingLandDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,6 @@
* @author Julian Oes <julian@oes.ch>
*/

#include <matrix/math.hpp>

#include "FixedwingLandDetector.h"

namespace land_detector
Expand All @@ -55,35 +53,20 @@ FixedwingLandDetector::FixedwingLandDetector()

void FixedwingLandDetector::_update_topics()
{
LandDetector::_update_topics();
_airspeed_sub.update(&_airspeed);
_vehicle_acceleration_sub.update(&_vehicle_acceleration);
_vehicle_local_position_sub.update(&_vehicle_local_position);
}

void FixedwingLandDetector::_update_params()
{
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);

// update parameters from storage
_update_params();
}
}

float FixedwingLandDetector::_get_max_altitude()
{
// TODO
// This means no altitude limit as the limit
// is always current position plus 10000 meters
// TODO: This means no altitude limit as the limit
// is always current position plus 10000 meters.
return roundf(-_vehicle_local_position.z + 10000);
}

bool FixedwingLandDetector::_get_landed_state()
{
// only trigger flight conditions if we are armed
// Only trigger flight conditions if we are armed.
if (!_actuator_armed.armed) {
return true;
}
Expand Down Expand Up @@ -116,11 +99,11 @@ bool FixedwingLandDetector::_get_landed_state()

_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;

// crude land detector for fixedwing
landDetected = _xy_accel_filtered < _param_lndfw_xyaccel_max.get()
&& _airspeed_filtered < _param_lndfw_airspd.get()
// Crude land detector for fixedwing.
landDetected = _airspeed_filtered < _param_lndfw_airspd.get()
&& _velocity_xy_filtered < _param_lndfw_vel_xy_max.get()
&& _velocity_z_filtered < _param_lndfw_vel_z_max.get();
&& _velocity_z_filtered < _param_lndfw_vel_z_max.get()
&& _xy_accel_filtered < _param_lndfw_xyaccel_max.get();

} else {
// Control state topic has timed out and we need to assume we're landed.
Expand Down
14 changes: 4 additions & 10 deletions src/modules/land_detector/FixedwingLandDetector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,10 +42,9 @@

#pragma once

#include <matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_local_position.h>

#include "LandDetector.h"

Expand All @@ -60,26 +59,22 @@ class FixedwingLandDetector final : public LandDetector
FixedwingLandDetector();

protected:
void _update_params() override;
void _update_topics() override;

bool _get_landed_state() override;

float _get_max_altitude() override;

void _update_topics() override;

private:

/** Time in us that landing conditions have to hold before triggering a land. */
static constexpr hrt_abstime LANDED_TRIGGER_TIME_US = 2_s;
static constexpr hrt_abstime FLYING_TRIGGER_TIME_US = 0_us;

uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};

airspeed_s _airspeed{};
vehicle_acceleration_s _vehicle_acceleration{};
vehicle_local_position_s _vehicle_local_position{};

float _airspeed_filtered{0.0f};
float _velocity_xy_filtered{0.0f};
Expand All @@ -93,7 +88,6 @@ class FixedwingLandDetector final : public LandDetector
(ParamFloat<px4::params::LNDFW_VEL_XY_MAX>) _param_lndfw_vel_xy_max,
(ParamFloat<px4::params::LNDFW_VEL_Z_MAX>) _param_lndfw_vel_z_max
);

};

} // namespace land_detector
28 changes: 12 additions & 16 deletions src/modules/land_detector/LandDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,14 +40,6 @@

#include "LandDetector.h"

#include <float.h>
#include <math.h>

#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/parameter_update.h>

using namespace time_literals;

namespace land_detector
Expand All @@ -71,15 +63,15 @@ LandDetector::~LandDetector()

void LandDetector::start()
{
_check_params(true);
_update_params(true);
ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL);
}

void LandDetector::Run()
{
perf_begin(_cycle_perf);

_check_params(false);
_update_params();
_actuator_armed_sub.update(&_actuator_armed);
_update_topics();
_update_state();
Expand Down Expand Up @@ -149,16 +141,13 @@ void LandDetector::Run()
}
}

void LandDetector::_check_params(const bool force)
void LandDetector::_update_params(const bool force)
{
// check for parameter updates
if (_parameter_update_sub.updated() || force) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);

// update parameters from storage
_update_params();
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);

_update_total_flight_time();
}
Expand Down Expand Up @@ -192,6 +181,13 @@ void LandDetector::_update_state()
}
}

void LandDetector::_update_topics()
{
_actuator_armed_sub.update(&_actuator_armed);
_vehicle_acceleration_sub.update(&_vehicle_acceleration);
_vehicle_local_position_sub.update(&_vehicle_local_position);
}

void LandDetector::_update_total_flight_time()
{
_total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32;
Expand Down
44 changes: 28 additions & 16 deletions src/modules/land_detector/LandDetector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,16 +42,25 @@

#pragma once

#include <float.h>
#include <lib/hysteresis/hysteresis.h>
#include <parameters/param.h>
#include <math.h>
#include <perf/perf_counter.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>


using namespace time_literals;

Expand Down Expand Up @@ -100,14 +109,15 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
protected:

/**
* Update parameters.
* Updates parameters if changes have occurred or if forced.
* @var force Forces a parameter update.
*/
virtual void _update_params() = 0;
virtual void _update_params(const bool force = false);

/**
* Update uORB topics.
* Updates subscribed uORB topics.
*/
virtual void _update_topics() = 0;
virtual void _update_topics();

/**
* @return true if UAV is in a landed state.
Expand All @@ -132,7 +142,7 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
/**
* @return maximum altitude that can be reached
*/
virtual float _get_max_altitude() = 0;
virtual float _get_max_altitude() { return INFINITY; }

/**
* @return true if vehicle could be in ground effect (close to ground)
Expand All @@ -142,9 +152,6 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
/** Run main land detector loop at this interval. */
static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL = 20_ms;

orb_advert_t _land_detected_pub{nullptr};
vehicle_land_detected_s _land_detected{};

LandDetectionState _state{LandDetectionState::LANDED};

systemlib::Hysteresis _freefall_hysteresis{false};
Expand All @@ -153,27 +160,32 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
systemlib::Hysteresis _ground_contact_hysteresis{true};
systemlib::Hysteresis _ground_effect_hysteresis{false};

actuator_armed_s _actuator_armed {};
actuator_armed_s _actuator_armed{};
vehicle_acceleration_s _vehicle_acceleration{};
vehicle_land_detected_s _land_detected{};
vehicle_local_position_s _vehicle_local_position{};

private:
orb_advert_t _land_detected_pub{nullptr};

uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};

void _check_params(bool force = false);
private:

void Run() override;

void _update_state();

void _update_total_flight_time();

bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state

uint64_t _total_flight_time{0}; ///< in microseconds
bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state

hrt_abstime _takeoff_time{0};
hrt_abstime _total_flight_time{0}; ///< total vehicle flight time in microseconds

perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, "land_detector_cycle")};

uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};

DEFINE_PARAMETERS_CUSTOM_PARENT(
Expand Down
8 changes: 5 additions & 3 deletions src/modules/land_detector/MulticopterLandDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,17 +86,19 @@ MulticopterLandDetector::MulticopterLandDetector()

void MulticopterLandDetector::_update_topics()
{
LandDetector::_update_topics();

_actuator_controls_sub.update(&_actuator_controls);
_battery_sub.update(&_battery_status);
_vehicle_acceleration_sub.update(&_vehicle_acceleration);
_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
_vehicle_local_position_sub.update(&_vehicle_local_position);
_vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint);
}

void MulticopterLandDetector::_update_params()
void MulticopterLandDetector::_update_params(const bool force)
{
LandDetector::_update_params(force);

_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_lndmc_ffall_ttri.get()));

param_get(_paramHandle.minThrottle, &_params.minThrottle);
Expand Down
10 changes: 3 additions & 7 deletions src/modules/land_detector/MulticopterLandDetector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ class MulticopterLandDetector : public LandDetector
MulticopterLandDetector();

protected:
void _update_params() override;
void _update_params(const bool force = false) override;
void _update_topics() override;

bool _get_landed_state() override;
Expand All @@ -79,7 +79,7 @@ class MulticopterLandDetector : public LandDetector
float _get_max_altitude() override;
private:

/* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */
/** Get control mode dependent pilot throttle threshold with which we should quit landed state and take off. */
float _get_takeoff_throttle();

bool _has_low_thrust();
Expand All @@ -100,9 +100,7 @@ class MulticopterLandDetector : public LandDetector
/** Time interval in us in which wider acceptance thresholds are used after landed. */
static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s;

/**
* @brief Handles for interesting parameters
**/
/** Handles for interesting parameters. **/
struct {
param_t minThrottle;
param_t hoverThrottle;
Expand All @@ -127,10 +125,8 @@ class MulticopterLandDetector : public LandDetector

actuator_controls_s _actuator_controls {};
battery_status_s _battery_status {};
vehicle_acceleration_s _vehicle_acceleration{};
vehicle_angular_velocity_s _vehicle_angular_velocity{};
vehicle_control_mode_s _vehicle_control_mode {};
vehicle_local_position_s _vehicle_local_position {};
vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {};

hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first
Expand Down
13 changes: 0 additions & 13 deletions src/modules/land_detector/RoverLandDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,14 +44,6 @@
namespace land_detector
{

void RoverLandDetector::_update_topics()
{
}

void RoverLandDetector::_update_params()
{
}

bool RoverLandDetector::_get_ground_contact_state()
{
return true;
Expand All @@ -66,9 +58,4 @@ bool RoverLandDetector::_get_landed_state()
return false;
}

float RoverLandDetector::_get_max_altitude()
{
return 0.0f;
}

} // namespace land_detector
Loading

0 comments on commit adb10a1

Please sign in to comment.