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

multicopter land detector ground contact fixes #15083

Merged
merged 6 commits into from
Aug 2, 2020
Merged

multicopter land detector ground contact fixes #15083

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
0c8a759
land_detector: multicopter ground contact robustness, fixes, and cleanup
dagar Jul 27, 2020
a653b70
Update src/modules/land_detector/FixedwingLandDetector.cpp
dagar Jul 29, 2020
684d409
Update src/modules/land_detector/MulticopterLandDetector.cpp
dagar Jul 29, 2020
cf09c0a
Update src/modules/land_detector/MulticopterLandDetector.cpp
dagar Jul 29, 2020
eea697d
change from code review
dagar Jul 29, 2020
ec66e18
Merge remote-tracking branch 'px4/master' into pr-land_detector_mc_gr…
dagar Jul 29, 2020
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
7 changes: 0 additions & 7 deletions src/modules/commander/Commander.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1575,7 +1575,6 @@ Commander::run()
/* Update land detector */
if (_land_detector_sub.updated()) {
_was_landed = _land_detector.landed;
bool was_falling = _land_detector.freefall;
_land_detector_sub.copy(&_land_detector);

// Only take actions if armed
Expand All @@ -1596,12 +1595,6 @@ Commander::run()
_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
}
}

if (was_falling != _land_detector.freefall) {
if (_land_detector.freefall) {
mavlink_log_info(&mavlink_log_pub, "Freefall detected");
}
}
}
}

Expand Down
19 changes: 7 additions & 12 deletions src/modules/land_detector/FixedwingLandDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,16 +51,10 @@ FixedwingLandDetector::FixedwingLandDetector()
_landed_hysteresis.set_hysteresis_time_from(true, FLYING_TRIGGER_TIME_US);
}

void FixedwingLandDetector::_update_topics()
{
LandDetector::_update_topics();
_airspeed_validated_sub.update(&_airspeed_validated);
}

bool FixedwingLandDetector::_get_landed_state()
{
// Only trigger flight conditions if we are armed.
if (!_actuator_armed.armed) {
if (!_armed) {
return true;
}

Expand All @@ -83,19 +77,20 @@ bool FixedwingLandDetector::_get_landed_state()
_velocity_z_filtered = val;
}

airspeed_validated_s airspeed_validated{};
_airspeed_validated_sub.copy(&airspeed_validated);

// set _airspeed_filtered to 0 if airspeed data is invalid
if (!PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed_validated.timestamp) > 1_s) {
if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
_airspeed_filtered = 0.0f;

} else {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s;
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s;
}

// A leaking lowpass prevents biases from building up, but
// gives a mostly correct response for short impulses.
const matrix::Vector3f accel{_vehicle_acceleration.xyz};
const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1));

const float acc_hor = matrix::Vector2f(_acceleration).norm();
_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;

// Crude land detector for fixedwing.
Expand Down
3 changes: 0 additions & 3 deletions src/modules/land_detector/FixedwingLandDetector.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,6 @@ class FixedwingLandDetector final : public LandDetector
protected:

bool _get_landed_state() override;
void _update_topics() override;

private:

Expand All @@ -71,8 +70,6 @@ class FixedwingLandDetector final : public LandDetector

uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};

airspeed_validated_s _airspeed_validated{};

float _airspeed_filtered{0.0f};
float _velocity_xy_filtered{0.0f};
float _velocity_z_filtered{0.0f};
Expand Down
86 changes: 41 additions & 45 deletions src/modules/land_detector/LandDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,21 +57,52 @@ LandDetector::~LandDetector()

void LandDetector::start()
{
_update_params();
ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL);
ScheduleDelayed(50_ms);
_vehicle_local_position_sub.registerCallback();
}

void LandDetector::Run()
{
// push backup schedule
ScheduleDelayed(50_ms);

perf_begin(_cycle_perf);

if (_parameter_update_sub.updated()) {
if (_parameter_update_sub.updated() || (_land_detected.timestamp == 0)) {
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);

updateParams();
_update_params();

_total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32;
_total_flight_time |= static_cast<uint32_t>(_param_total_flight_time_low.get());
}

actuator_armed_s actuator_armed;

if (_actuator_armed_sub.update(&actuator_armed)) {
_armed = actuator_armed.armed;
}

_actuator_armed_sub.update(&_actuator_armed);
vehicle_acceleration_s vehicle_acceleration;

if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) {
_acceleration = matrix::Vector3f{vehicle_acceleration.xyz};
}

_vehicle_local_position_sub.update(&_vehicle_local_position);
_vehicle_status_sub.update(&_vehicle_status);

_update_topics();
_update_state();

const hrt_abstime now_us = hrt_absolute_time();

_freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us);
_ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state(), now_us);
_maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state(), now_us);
_landed_hysteresis.set_state_and_update(_get_landed_state(), now_us);
_ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state(), now_us);

const bool freefallDetected = _freefall_hysteresis.get_state();
const bool ground_contactDetected = _ground_contact_hysteresis.get_state();
Expand All @@ -80,8 +111,6 @@ void LandDetector::Run()
const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : INFINITY;
const bool in_ground_effect = _ground_effect_hysteresis.get_state();

const hrt_abstime now = hrt_absolute_time();

// publish at 1 Hz, very first time, or when the result has changed
if ((hrt_elapsed_time(&_land_detected.timestamp) >= 1_s) ||
(_land_detected.landed != landDetected) ||
Expand All @@ -93,24 +122,23 @@ void LandDetector::Run()

if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */
// We did take off
_takeoff_time = now;
_takeoff_time = now_us;
}

_land_detected.timestamp = hrt_absolute_time();
_land_detected.landed = landDetected;
_land_detected.freefall = freefallDetected;
_land_detected.maybe_landed = maybe_landedDetected;
_land_detected.ground_contact = ground_contactDetected;
_land_detected.alt_max = alt_max;
_land_detected.in_ground_effect = in_ground_effect;

_land_detected.timestamp = hrt_absolute_time();
_vehicle_land_detected_pub.publish(_land_detected);
}

// set the flight time when disarming (not necessarily when landed, because all param changes should
// happen on the same event and it's better to set/save params while not in armed state)
if (_takeoff_time != 0 && !_actuator_armed.armed && _previous_armed_state) {
_total_flight_time += now - _takeoff_time;
if (_takeoff_time != 0 && !_armed && _previous_armed_state) {
_total_flight_time += now_us - _takeoff_time;
_takeoff_time = 0;

uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
Expand All @@ -124,7 +152,7 @@ void LandDetector::Run()
_param_total_flight_time_low.commit_no_notification();
}

_previous_armed_state = _actuator_armed.armed;
_previous_armed_state = _armed;

perf_end(_cycle_perf);

Expand All @@ -134,36 +162,4 @@ void LandDetector::Run()
}
}

void LandDetector::_update_params()
{
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);

updateParams();
_update_total_flight_time();
}

void LandDetector::_update_state()
{
const hrt_abstime now_us = hrt_absolute_time();
_freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us);
_ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state(), now_us);
_maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state(), now_us);
_landed_hysteresis.set_state_and_update(_get_landed_state(), now_us);
_ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state(), now_us);
}

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;
_total_flight_time |= static_cast<uint32_t>(_param_total_flight_time_low.get());
}

} // namespace land_detector
45 changes: 20 additions & 25 deletions src/modules/land_detector/LandDetector.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,11 +56,13 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>

using namespace time_literals;

Expand All @@ -70,11 +72,9 @@ namespace land_detector
class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::ScheduledWorkItem
{
public:

LandDetector();
virtual ~LandDetector();


/** @see ModuleBase */
static int custom_command(int argc, char *argv[])
{
Expand All @@ -99,12 +99,12 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
/**
* Updates parameters.
*/
virtual void _update_params();
virtual void _update_params() {};

/**
* Updates subscribed uORB topics.
*/
virtual void _update_topics();
virtual void _update_topics() {};

/**
* @return true if UAV is in a landed state.
Expand Down Expand Up @@ -136,17 +136,22 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
*/
virtual bool _get_ground_effect_state() { return false; }

/** Run main land detector loop at this interval. */
static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL = 20_ms;

systemlib::Hysteresis _freefall_hysteresis{false};
systemlib::Hysteresis _landed_hysteresis{true};
systemlib::Hysteresis _maybe_landed_hysteresis{true};
systemlib::Hysteresis _ground_contact_hysteresis{true};
systemlib::Hysteresis _ground_effect_hysteresis{false};

actuator_armed_s _actuator_armed{};
vehicle_acceleration_s _vehicle_acceleration{};
vehicle_local_position_s _vehicle_local_position{};
vehicle_status_s _vehicle_status{};

matrix::Vector3f _acceleration{};

bool _armed{false};
bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state
Copy link
Member

Choose a reason for hiding this comment

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

Tabs to indent, spaces to align right?

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

Copy link
Member Author

Choose a reason for hiding this comment

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

Yes, but also trying to maintain consistency by default. In this case the entire line was moved from below.
Something like clang-format could probably help get us there overall.


private:
void Run() override;

vehicle_land_detected_s _land_detected = {
.timestamp = 0,
Expand All @@ -157,29 +162,19 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
.landed = true,
};

vehicle_local_position_s _vehicle_local_position{};

uORB::Publication<vehicle_land_detected_s> _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)};

private:

void Run() override;

void _update_state();

void _update_total_flight_time();

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")};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};

uORB::Publication<vehicle_land_detected_s> _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)};

uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
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)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};

DEFINE_PARAMETERS_CUSTOM_PARENT(
ModuleParams,
Expand Down
Loading