Skip to content

Commit

Permalink
Refactor the LandDetector class to
Browse files Browse the repository at this point in the history
 - Reduce duplicate code in LandDetector _check_params() method.
 - Standardize naming cases.
 - Implement DEFINE_PARAMETERS() macro.
  • Loading branch information
mcsauder authored and bkueng committed Jun 26, 2019
1 parent 4666207 commit faa3c3d
Show file tree
Hide file tree
Showing 2 changed files with 48 additions and 41 deletions.
43 changes: 22 additions & 21 deletions src/modules/land_detector/LandDetector.cpp
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 @@ -81,8 +78,8 @@ void LandDetector::Run()
{
perf_begin(_cycle_perf);

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

Expand All @@ -98,7 +95,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 +117,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 +147,15 @@ void LandDetector::Run()
exit_and_cleanup();
}
}
void LandDetector::_check_params(const bool force)

void LandDetector::_check_params()
{
parameter_update_s paramUpdate;
parameter_update_s param_update;

if (_parameterSub.update(&paramUpdate) || force) {
if (_param_update_sub.update(&param_update)) {
_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;
_total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32;
_total_flight_time |= _param_total_flight_time_low.get();
}
}

Expand Down
46 changes: 26 additions & 20 deletions src/modules/land_detector/LandDetector.h
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 _check_params();

void Run() override;

void _update_state();

param_t _p_total_flight_time_high{PARAM_INVALID};
param_t _p_total_flight_time_low{PARAM_INVALID};
bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state

uint32_t _measure_interval{LAND_DETECTOR_UPDATE_INTERVAL};

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

0 comments on commit faa3c3d

Please sign in to comment.