Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Cherry pick the src/modules/land_detector/ directory work from PR #9756 #11874

Merged
merged 1 commit into from
Oct 14, 2019
Merged

Cherry pick the src/modules/land_detector/ directory work from PR #9756 #11874

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 8 additions & 25 deletions src/modules/land_detector/FixedwingLandDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,6 @@
* @author Julian Oes <[email protected]>
*/

#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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@mcsauder this is not correct, it re-loads the params every cycle now, as there is no check for _parameter_update_sub here. You can move the _parameter_update_sub.updated() check out of LandDetector::_update_params around the _update_params(); in LandDetector::Run() (and get rid of the force argument).
(Alternatively you can rename this method here to void updateParams() override;, and then make LandDetector::_update_params non-virtual)
There also needs to be a call to updateParams() in LandDetector::_update_params.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @bkueng , I might be mistaken, but with the default argument force = false, I don't think that passing the force argument from MulticopterLandDetector::_update_param(const bool force = false) will result in re-loading the params every cycle. It would only result in forcing a reload if somewhere else this method is being called with a true argument, which doesn't happen anywhere I am able to find. Let me know if I'm not seeing things correctly.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, I think I see that your concern is with the additional 4 param_get() calls. I'll work on getting a PR opened to address that.

Expand Down
10 changes: 3 additions & 7 deletions src/modules/land_detector/MulticopterLandDetector.h
View file
Open in desktop
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
Open in desktop
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