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.

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
Changes from 1 commit
Commits
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
Next Next commit
Refactor the LandDetector class to
 - Reduce duplicate code in LandDetector _check_params() method.
 - Standardize naming cases.
 - Implement DEFINE_PARAMETERS() macro.
mcsauder committed Jun 25, 2019
commit aabe8da5ecf1bfe33ae9dbc14052f904c9c56bdc
43 changes: 22 additions & 21 deletions src/modules/land_detector/LandDetector.cpp
Original file line number Diff line number Diff line change
@@ -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()
@@ -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();

@@ -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) ||
@@ -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);

@@ -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();
}
}

46 changes: 26 additions & 20 deletions src/modules/land_detector/LandDetector.h
Original file line number Diff line number Diff line change
@@ -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>
@@ -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 {
@@ -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[])
@@ -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.
@@ -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};
@@ -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);
Copy link
Member

Choose a reason for hiding this comment

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

Why is force removed?

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 Beat, thanks for your review! I removed force because check_params() is private, so we know nothing outside of the class is utilizing it, and the only place it now gets called is setting force to false, so it has become unneeded.

Copy link
Member

Choose a reason for hiding this comment

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

It needs to be called with force=true here: https://github.com/PX4/Firmware/pull/12209/files#diff-1fd2cab203a1ba1c362f2dd76a74542fL84. Otherwise _total_flight_time does not necessarily get initialized. This only worked because there were other param changes.

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 Beat. So this is broken in current master? I have to admit that I'm a little confused, in the tests I show above only the LAND_FLIGHT_T_HI and LAND_FLIGHT_T_LO are changed, no others. It is also verifiable with no parameter chances in the flight logs below. I'll gladly revert this change though if it's important.

Copy link
Member

Choose a reason for hiding this comment

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

So this is broken in current master?

Yes. We always have other param changes at the end of a flight, which is why it worked.

Copy link
Contributor Author

@mcsauder mcsauder Jun 25, 2019

Choose a reason for hiding this comment

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

Hi @bkueng. I pushed up a commit that calls _check_params(true) within the constructor so that it gets initialized once as used to be the case prior to 106ee28.

Let me know if you think this is appropriate, and otherwise, if it needs to be forced at each Run() call, we can deprecate the bool arg and the if statement so that _check_params() can be effectively an _update_params() call each iteration.

EDITED ABOVE - call moved to the constructor.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

@bkueng , moving to the constructor absolutely breaks the detector. I am removing it and squashing that commit out of existence.

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, placing the call back in the start() method works fine and I think will satisfy the concerns. Latest commit functions fine, flight tested here: https://review.px4.io/plot_app?log=cdc782c2-9999-4f49-a98a-2de786ce5c6f

Let me know if you have any other thoughts! Thanks again for your review!

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"))};
Copy link
Member

Choose a reason for hiding this comment

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

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

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Fixed! Thanks for spotting that!


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