fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 12:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cherry pick the directory and voted_sensors_update.h from PR #9756.

Browse Source 
Consolidate _update_params() methods for improved inheritance from the LandDetector base class.
Move common uORB::Subscriptions to the base class for inheritance.
Deprecate redundant override methods.
This commit is contained in:
mcsauder
2019-09-01 22:52:01 -06:00
committed by Matthias Grob
parent c44e4b9578
commit fb12ddb69a
8 changed files with 63 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/FixedwingLandDetector.cpp
+8 -25
View File
@@ -39,8 +39,6 @@
* @author Julian Oes <julian@oes.ch>
*/
#include <matrix/math.hpp>
#include "FixedwingLandDetector.h"
namespace land_detector
@@ -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;
}
@@ -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.
src/modules/land_detector/FixedwingLandDetector.h
+4 -10
View File
@@ -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"
@@ -60,12 +59,13 @@ public:
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. */
@@ -73,13 +73,8 @@ private:
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};
@@ -93,7 +88,6 @@ private:
(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
src/modules/land_detector/LandDetector.cpp
+12 -16
View File
@@ -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
@@ -71,7 +63,7 @@ LandDetector::~LandDetector()
void LandDetector::start()
{
_check_params(true);
_update_params(true);
ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL);
}
@@ -79,7 +71,7 @@ void LandDetector::Run()
{
perf_begin(_cycle_perf);
_check_params(false);
_update_params();
_actuator_armed_sub.update(&_actuator_armed);
_update_topics();
_update_state();
@@ -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();
}
@@ -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;
src/modules/land_detector/LandDetector.h
+28 -16
View File
@@ -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;
@@ -100,14 +109,15 @@ public:
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.
@@ -132,7 +142,7 @@ protected:
/**
* @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)
@@ -142,9 +152,6 @@ protected:
/** 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};
@@ -153,27 +160,32 @@ protected:
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{};
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)};
private:
void _check_params(bool force = false);
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(
src/modules/land_detector/MulticopterLandDetector.cpp
+5 -3
View File
@@ -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);
src/modules/land_detector/MulticopterLandDetector.h
+3 -7
View File
@@ -67,7 +67,7 @@ public:
MulticopterLandDetector();
protected:
void _update_params() override;
void _update_params(const bool force = false) override;
void _update_topics() override;
bool _get_landed_state() override;
@@ -79,7 +79,7 @@ protected:
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();
@@ -100,9 +100,7 @@ private:
/** 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;
@@ -127,10 +125,8 @@ private:
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
src/modules/land_detector/RoverLandDetector.cpp
-13
View File
@@ -44,14 +44,6 @@
namespace land_detector
{
void RoverLandDetector::_update_topics()
{
}
void RoverLandDetector::_update_params()
{
}
bool RoverLandDetector::_get_ground_contact_state()
{
return true;
@@ -66,9 +58,4 @@ bool RoverLandDetector::_get_landed_state()
return false;
}
float RoverLandDetector::_get_max_altitude()
{
return 0.0f;
}
} // namespace land_detector
src/modules/land_detector/RoverLandDetector.h
+3 -12
View File
@@ -41,8 +41,6 @@
#pragma once
#include <uORB/topics/airspeed.h>
#include "LandDetector.h"
namespace land_detector
@@ -54,18 +52,11 @@ public:
RoverLandDetector() = default;
protected:
virtual void _update_params() override;
virtual void _update_topics() override;
virtual bool _get_landed_state() override;
virtual bool _get_ground_contact_state() override;
virtual float _get_max_altitude() override;
bool _get_ground_contact_state() override;
bool _get_landed_state() override;
private:
};
} // namespace land_detector