fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 09:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

land_detector move orb subscriptions to uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-05-30 14:10:31 -04:00
parent 2320088541
commit 53aa4130a8
10 changed files with 34 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/FixedwingLandDetector.cpp
+3 -10
View File
@@ -62,18 +62,11 @@ FixedwingLandDetector::FixedwingLandDetector()
_landed_hysteresis.set_hysteresis_time_from(true, FLYING_TRIGGER_TIME_US);
}
void FixedwingLandDetector::_initialize_topics()
{
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
}
void FixedwingLandDetector::_update_topics()
{
_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
_orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors);
_orb_update(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
_airspeedSub.update(&_airspeed);
_sensor_bias_sub.update(&_sensors);
_local_pos_sub.update(&_local_pos);
}
void FixedwingLandDetector::_update_params()
src/modules/land_detector/FixedwingLandDetector.h
+4 -4
View File
@@ -43,6 +43,7 @@
#pragma once
#include <drivers/drv_hrt.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -60,7 +61,6 @@ public:
FixedwingLandDetector();
protected:
void _initialize_topics() override;
void _update_params() override;
void _update_topics() override;
@@ -87,9 +87,9 @@ private:
float maxXYAccel;
} _params{};
int _airspeedSub{-1};
int _sensor_bias_sub{-1};
int _local_pos_sub{-1};
uORB::Subscription _airspeedSub{ORB_ID(airspeed)};
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position});
airspeed_s _airspeed{};
sensor_bias_s _sensors{};
src/modules/land_detector/LandDetector.cpp
+3 -48
View File
@@ -46,7 +46,7 @@
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/drv_hrt.h>
#include "uORB/topics/parameter_update.h"
#include <uORB/topics/parameter_update.h>
using namespace time_literals;
@@ -61,18 +61,6 @@ LandDetector::LandDetector() :
LandDetector::~LandDetector()
{
perf_free(_cycle_perf);
if (_armingSub >= 0) {
orb_unsubscribe(_armingSub);
}
if (_parameterSub >= 0) {
orb_unsubscribe(_parameterSub);
}
if (_landDetectedPub) {
orb_unadvertise(_landDetectedPub);
}
}
int LandDetector::start()
@@ -104,18 +92,13 @@ void LandDetector::_cycle()
_p_total_flight_time_high = param_find("LND_FLIGHT_T_HI");
_p_total_flight_time_low = param_find("LND_FLIGHT_T_LO");
// Initialize uORB topics.
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
_parameterSub = orb_subscribe(ORB_ID(parameter_update));
_initialize_topics();
_check_params(true);
_object.store(this);
}
_check_params(false);
_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
_armingSub.update(&_arming);
_update_topics();
_update_state();
@@ -184,16 +167,9 @@ void LandDetector::_cycle()
}
void LandDetector::_check_params(const bool force)
{
bool updated;
parameter_update_s paramUpdate;
orb_check(_parameterSub, &updated);
if (updated) {
orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
}
if (updated || force) {
if (_parameterSub.update(&paramUpdate) || force) {
_update_params();
uint32_t flight_time;
param_get(_p_total_flight_time_high, (int32_t *)&flight_time);
@@ -231,25 +207,4 @@ void LandDetector::_update_state()
}
}
bool LandDetector::_orb_update(const struct orb_metadata *meta, int handle, void *buffer)
{
bool newData = false;
// check if there is new data to grab
if (orb_check(handle, &newData) != OK) {
return false;
}
if (!newData) {
return false;
}
if (orb_copy(meta, handle, buffer) != OK) {
return false;
}
return true;
}
} // namespace land_detector
src/modules/land_detector/LandDetector.h
+4 -15
View File
@@ -47,8 +47,9 @@
#include <lib/hysteresis/hysteresis.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_land_detected.h>
namespace land_detector
@@ -97,10 +98,6 @@ public:
int start();
protected:
/**
* Called once to initialize uORB topics.
*/
virtual void _initialize_topics() = 0;
/**
* Update uORB topics.
@@ -142,13 +139,6 @@ protected:
*/
virtual bool _get_ground_effect_state() { return false; }
/**
* Convenience function for polling uORB subscriptions.
*
* @return true if there was new data and it was successfully copied
*/
static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer);
/** Run main land detector loop at this rate in Hz. */
static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50;
@@ -185,9 +175,8 @@ private:
bool _previous_arming_state{false}; ///< stores the previous _arming.armed state
int _parameterSub{ -1};
int _armingSub{ -1};
uORB::Subscription _parameterSub{ORB_ID(parameter_update)};
uORB::Subscription _armingSub{ORB_ID(actuator_armed)};
};
} // namespace land_detector
src/modules/land_detector/MulticopterLandDetector.cpp
+7 -21
View File
@@ -90,27 +90,15 @@ MulticopterLandDetector::MulticopterLandDetector()
_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US);
}
void MulticopterLandDetector::_initialize_topics()
{
// subscribe to position, attitude and velocity changes
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_vehicleLocalPositionSetpointSub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_battery_sub = orb_subscribe(ORB_ID(battery_status));
}
void MulticopterLandDetector::_update_topics()
{
_orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
_orb_update(ORB_ID(vehicle_local_position_setpoint), _vehicleLocalPositionSetpointSub, &_vehicleLocalPositionSetpoint);
_orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
_orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
_orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors);
_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
_orb_update(ORB_ID(battery_status), _battery_sub, &_battery);
_vehicleLocalPositionSub.update(&_vehicleLocalPosition);
_vehicleLocalPositionSetpointSub.update(&_vehicleLocalPositionSetpoint);
_attitudeSub.update(&_vehicleAttitude);
_actuatorsSub.update(&_actuators);
_sensor_bias_sub.update(&_sensors);
_vehicle_control_mode_sub.update(&_control_mode);
_battery_sub.update(&_battery);
}
void MulticopterLandDetector::_update_params()
@@ -128,10 +116,8 @@ void MulticopterLandDetector::_update_params()
param_get(_paramHandle.altitude_max, &_params.altitude_max);
param_get(_paramHandle.landSpeed, &_params.landSpeed);
param_get(_paramHandle.low_thrust_threshold, &_params.low_thrust_threshold);
}
bool MulticopterLandDetector::_get_freefall_state()
{
if (_params.freefall_acc_threshold < 0.1f
src/modules/land_detector/MulticopterLandDetector.h
+8 -8
View File
@@ -45,6 +45,7 @@
#include "LandDetector.h"
#include <parameters/param.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -68,7 +69,6 @@ public:
MulticopterLandDetector();
protected:
void _initialize_topics() override;
void _update_params() override;
void _update_topics() override;
@@ -124,13 +124,13 @@ private:
float low_thrust_threshold;
} _params{};
int _vehicleLocalPositionSub{ -1};
int _vehicleLocalPositionSetpointSub{ -1};
int _actuatorsSub{ -1};
int _attitudeSub{ -1};
int _sensor_bias_sub{ -1};
int _vehicle_control_mode_sub{ -1};
int _battery_sub{ -1};
uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicleLocalPositionSetpointSub{ORB_ID(vehicle_local_position_setpoint)};
uORB::Subscription _actuatorsSub{ORB_ID(actuator_controls_0)};
uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _battery_sub{ORB_ID(battery_status)};
vehicle_local_position_s _vehicleLocalPosition {};
vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint {};
src/modules/land_detector/RoverLandDetector.cpp
-4
View File
@@ -46,10 +46,6 @@
namespace land_detector
{
void RoverLandDetector::_initialize_topics()
{
}
void RoverLandDetector::_update_topics()
{
}
src/modules/land_detector/RoverLandDetector.h
-2
View File
@@ -54,8 +54,6 @@ public:
RoverLandDetector() = default;
protected:
virtual void _initialize_topics() override;
virtual void _update_params() override;
virtual void _update_topics() override;
src/modules/land_detector/VtolLandDetector.cpp
+2 -10
View File
@@ -51,20 +51,12 @@ VtolLandDetector::VtolLandDetector()
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
}
void VtolLandDetector::_initialize_topics()
{
MulticopterLandDetector::_initialize_topics();
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}
void VtolLandDetector::_update_topics()
{
MulticopterLandDetector::_update_topics();
_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
_orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
_airspeedSub.update(&_airspeed);
_vehicle_status_sub.update(&_vehicle_status);
}
bool VtolLandDetector::_get_maybe_landed_state()
src/modules/land_detector/VtolLandDetector.h
+3 -3
View File
@@ -41,6 +41,7 @@
#pragma once
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_status.h>
@@ -55,7 +56,6 @@ public:
VtolLandDetector();
protected:
void _initialize_topics() override;
void _update_params() override;
void _update_topics() override;
bool _get_landed_state() override;
@@ -70,8 +70,8 @@ private:
float maxAirSpeed;
} _params{};
int _airspeedSub{-1};
int _vehicle_status_sub{-1};
uORB::Subscription _airspeedSub{ORB_ID(airspeed)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
airspeed_s _airspeed{};
vehicle_status_s _vehicle_status{};