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

Implement ModuleParams inheritance and DEFINE_PARAMETERS in the LandDetector class #12209

Merged
merged 4 commits into from
Jun 26, 2019
Merged

Implement ModuleParams inheritance and DEFINE_PARAMETERS in the LandDetector class #12209

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
aabe8da
Refactor the LandDetector class to
mcsauder Jun 5, 2019
55d52c4
Delete _measure_interval and redundant parentheses in LandDetector.h.
mcsauder Jun 24, 2019
bf67344
Revert changes to the check_params(const bool force) declaration/defi…
mcsauder Jun 25, 2019
a0d3f51
Add check_params(true) call to the LandDetector start() method. Break…
mcsauder Jun 25, 2019
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
45 changes: 26 additions & 19 deletions src/modules/land_detector/LandDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,17 +54,14 @@ namespace land_detector
{

LandDetector::LandDetector() :
ScheduledWorkItem(px4::wq_configurations::hp_default),
_cycle_perf(perf_alloc(PC_ELAPSED, "land_detector_cycle"))
ModuleParams(nullptr),
ScheduledWorkItem(px4::wq_configurations::hp_default)
{
_landDetected.timestamp = hrt_absolute_time();
_landDetected.freefall = false;
_landDetected.landed = true;
_landDetected.ground_contact = false;
_landDetected.maybe_landed = false;

_p_total_flight_time_high = param_find("LND_FLIGHT_T_HI");
_p_total_flight_time_low = param_find("LND_FLIGHT_T_LO");
}

LandDetector::~LandDetector()
Expand All @@ -74,6 +71,7 @@ LandDetector::~LandDetector()

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

Expand All @@ -82,7 +80,7 @@ void LandDetector::Run()
perf_begin(_cycle_perf);

_check_params(false);
_armingSub.update(&_arming);
_actuator_armed_sub.update(&_arming);
_update_topics();
_update_state();

Expand All @@ -98,7 +96,7 @@ void LandDetector::Run()

// publish at 1 Hz, very first time, or when the result has changed
if ((hrt_elapsed_time(&_landDetected.timestamp) >= 1_s) ||
(_landDetectedPub == nullptr) ||
(_land_detected_pub == nullptr) ||
(_landDetected.landed != landDetected) ||
(_landDetected.freefall != freefallDetected) ||
(_landDetected.maybe_landed != maybe_landedDetected) ||
Expand All @@ -120,22 +118,28 @@ void LandDetector::Run()
_landDetected.in_ground_effect = in_ground_effect;

int instance;
orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected,
orb_publish_auto(ORB_ID(vehicle_land_detected), &_land_detected_pub, &_landDetected,
&instance, ORB_PRIO_DEFAULT);
}

// 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 && !_arming.armed && _previous_arming_state) {
if (_takeoff_time != 0 && !_arming.armed && _previous_armed_state) {
_total_flight_time += now - _takeoff_time;
_takeoff_time = 0;

uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
param_set_no_notification(_p_total_flight_time_high, &flight_time);

_param_total_flight_time_high.set(flight_time);
_param_total_flight_time_high.commit_no_notification();

flight_time = _total_flight_time & 0xffffffff;
param_set_no_notification(_p_total_flight_time_low, &flight_time);

_param_total_flight_time_low.set(flight_time);
_param_total_flight_time_low.commit_no_notification();
}

_previous_arming_state = _arming.armed;
_previous_armed_state = _arming.armed;

perf_end(_cycle_perf);

Expand All @@ -144,17 +148,14 @@ void LandDetector::Run()
exit_and_cleanup();
}
}

void LandDetector::_check_params(const bool force)
{
parameter_update_s paramUpdate;
parameter_update_s param_update;

if (_parameterSub.update(&paramUpdate) || force) {
if (_param_update_sub.update(&param_update) || force) {
_update_params();
uint32_t flight_time;
param_get(_p_total_flight_time_high, (int32_t *)&flight_time);
_total_flight_time = ((uint64_t)flight_time) << 32;
param_get(_p_total_flight_time_low, (int32_t *)&flight_time);
_total_flight_time |= flight_time;
_update_total_flight_time();
}
}

Expand Down Expand Up @@ -186,4 +187,10 @@ void LandDetector::_update_state()
}
}

void LandDetector::_update_total_flight_time()
{
_total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32;
_total_flight_time |= _param_total_flight_time_low.get();
}

} // namespace land_detector
44 changes: 25 additions & 19 deletions src/modules/land_detector/LandDetector.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
#pragma once

#include <px4_module.h>
#include <px4_module_params.h>
#include <lib/hysteresis/hysteresis.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
Expand All @@ -57,7 +58,7 @@ using namespace time_literals;
namespace land_detector
{

class LandDetector : public ModuleBase<LandDetector>, px4::ScheduledWorkItem
class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::ScheduledWorkItem
{
public:
enum class LandDetectionState {
Expand All @@ -71,7 +72,6 @@ class LandDetector : public ModuleBase<LandDetector>, px4::ScheduledWorkItem
LandDetector();
virtual ~LandDetector();

static int task_spawn(int argc, char *argv[]);

/** @see ModuleBase */
static int custom_command(int argc, char *argv[])
Expand All @@ -88,27 +88,26 @@ class LandDetector : public ModuleBase<LandDetector>, px4::ScheduledWorkItem
/**
* @return current state.
*/
LandDetectionState get_state() const
{
return _state;
}
LandDetectionState get_state() const { return _state; }

/**
* Get the work queue going.
*/
void start();

static int task_spawn(int argc, char *argv[]);

protected:

/**
* Update uORB topics.
* Update parameters.
*/
virtual void _update_topics() = 0;
virtual void _update_params() = 0;

/**
* Update parameters.
* Update uORB topics.
*/
virtual void _update_params() = 0;
virtual void _update_topics() = 0;

/**
* @return true if UAV is in a landed state.
Expand Down Expand Up @@ -143,7 +142,7 @@ class LandDetector : public ModuleBase<LandDetector>, px4::ScheduledWorkItem
/** Run main land detector loop at this interval. */
static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL = 20_ms;

orb_advert_t _landDetectedPub{nullptr};
orb_advert_t _land_detected_pub{nullptr};
vehicle_land_detected_s _landDetected{};

LandDetectionState _state{LandDetectionState::LANDED};
Expand All @@ -154,26 +153,33 @@ class LandDetector : public ModuleBase<LandDetector>, px4::ScheduledWorkItem
systemlib::Hysteresis _ground_contact_hysteresis{true};
systemlib::Hysteresis _ground_effect_hysteresis{false};

struct actuator_armed_s _arming {};
actuator_armed_s _arming {};

private:
void Run() override;

void _check_params(bool force = false);

void Run() override;

void _update_state();

param_t _p_total_flight_time_high{PARAM_INVALID};
param_t _p_total_flight_time_low{PARAM_INVALID};
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

hrt_abstime _takeoff_time{0};

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

bool _previous_arming_state{false}; ///< stores the previous _arming.armed state
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _param_update_sub{ORB_ID(parameter_update)};

uORB::Subscription _parameterSub{ORB_ID(parameter_update)};
uORB::Subscription _armingSub{ORB_ID(actuator_armed)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::LND_FLIGHT_T_HI>) _param_total_flight_time_high,
(ParamInt<px4::params::LND_FLIGHT_T_LO>) _param_total_flight_time_low
);
};

} // namespace land_detector