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

fw_att_control move orb subscriptions to uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-06-04 20:09:49 -04:00
parent 4bef573497
commit be02ad3514
2 changed files with 29 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+17 -82
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -45,8 +45,6 @@ using namespace time_literals;
extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
FixedwingAttitudeControl::FixedwingAttitudeControl() :
_airspeed_sub(ORB_ID(airspeed)),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fwa_dt")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")),
@@ -133,29 +131,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
// subscriptions
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
}
FixedwingAttitudeControl::~FixedwingAttitudeControl()
{
orb_unsubscribe(_att_sub);
orb_unsubscribe(_att_sp_sub);
orb_unsubscribe(_vcontrol_mode_sub);
orb_unsubscribe(_params_sub);
orb_unsubscribe(_manual_sub);
orb_unsubscribe(_global_pos_sub);
orb_unsubscribe(_vehicle_status_sub);
orb_unsubscribe(_vehicle_land_detected_sub);
orb_unsubscribe(_battery_status_sub);
orb_unsubscribe(_rates_sp_sub);
perf_free(_loop_perf);
perf_free(_nonfinite_input_perf);
@@ -279,14 +259,7 @@ FixedwingAttitudeControl::parameters_update()
void
FixedwingAttitudeControl::vehicle_control_mode_poll()
{
bool updated = false;
/* Check if vehicle control mode has changed */
orb_check(_vcontrol_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
}
_vcontrol_mode_sub.update(&_vcontrol_mode);
if (_vehicle_status.is_vtol) {
const bool is_hovering = _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode;
@@ -308,7 +281,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) {
// Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values
if (orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual) == PX4_OK) {
if (_manual_sub.copy(&_manual)) {
// Check if we are in rattitude mode and the pilot is above the threshold on pitch
if (_vcontrol_mode.flag_control_rattitude_enabled) {
@@ -378,29 +351,17 @@ FixedwingAttitudeControl::vehicle_manual_poll()
void
FixedwingAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated = false;
orb_check(_att_sp_sub, &updated);
if (updated) {
if (orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp) == PX4_OK) {
_rates_sp.thrust_body[0] = _att_sp.thrust_body[0];
_rates_sp.thrust_body[1] = _att_sp.thrust_body[1];
_rates_sp.thrust_body[2] = _att_sp.thrust_body[2];
}
if (_att_sp_sub.update(&_att_sp)) {
_rates_sp.thrust_body[0] = _att_sp.thrust_body[0];
_rates_sp.thrust_body[1] = _att_sp.thrust_body[1];
_rates_sp.thrust_body[2] = _att_sp.thrust_body[2];
}
}
void
FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated = false;
orb_check(_rates_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), _rates_sp_sub, &_rates_sp);
if (_rates_sp_sub.update(&_rates_sp)) {
if (_is_tailsitter) {
float tmp = _rates_sp.roll;
_rates_sp.roll = -_rates_sp.yaw;
@@ -409,28 +370,10 @@ FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
}
}
void
FixedwingAttitudeControl::global_pos_poll()
{
/* check if there is a new global position */
bool global_pos_updated;
orb_check(_global_pos_sub, &global_pos_updated);
if (global_pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
}
}
void
FixedwingAttitudeControl::vehicle_status_poll()
{
/* check if there is new status information */
bool vehicle_status_updated;
orb_check(_vehicle_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
if (_vehicle_status_sub.update(&_vehicle_status)) {
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_actuators_id) {
if (_vehicle_status.is_vtol) {
@@ -454,14 +397,10 @@ FixedwingAttitudeControl::vehicle_status_poll()
void
FixedwingAttitudeControl::vehicle_land_detected_poll()
{
/* check if there is new status information */
bool vehicle_land_detected_updated;
orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
if (vehicle_land_detected_updated) {
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected {};
if (orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected) == PX4_OK) {
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
_landed = vehicle_land_detected.landed;
}
}
@@ -520,13 +459,12 @@ void FixedwingAttitudeControl::run()
while (!should_exit()) {
/* only update parameters if they changed */
bool params_updated = false;
orb_check(_params_sub, &params_updated);
bool params_updated = _params_sub.updated();
if (params_updated) {
/* read from param to clear updated flag */
parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
_params_sub.copy(&update);
/* update parameters from storage */
parameters_update();
@@ -613,7 +551,7 @@ void FixedwingAttitudeControl::run()
vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
global_pos_poll();
_global_pos_sub.update(&_global_pos);
vehicle_status_poll();
vehicle_land_detected_poll();
@@ -801,13 +739,10 @@ void FixedwingAttitudeControl::run()
if (_parameters.bat_scale_en &&
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
bool updated = false;
orb_check(_battery_status_sub, &updated);
if (_battery_status_sub.updated()) {
battery_status_s battery_status{};
if (updated) {
battery_status_s battery_status = {};
if (orb_copy(ORB_ID(battery_status), _battery_status_sub, &battery_status) == PX4_OK) {
if (_battery_status_sub.copy(&battery_status)) {
if (battery_status.scale > 0.0f) {
_battery_scale = battery_status.scale;
}
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+12 -12
View File
@@ -93,15 +93,18 @@ public:
private:
int _att_sub{-1}; /**< vehicle attitude */
int _att_sp_sub{-1}; /**< vehicle attitude setpoint */
int _rates_sp_sub{-1}; /**< vehicle rates setpoint */
int _battery_status_sub{-1}; /**< battery status subscription */
int _global_pos_sub{-1}; /**< global position subscription */
int _manual_sub{-1}; /**< notification of manual control updates */
int _params_sub{-1}; /**< notification of parameter updates */
int _vcontrol_mode_sub{-1}; /**< vehicle status subscription */
int _vehicle_land_detected_sub{-1}; /**< vehicle land detected subscription */
int _vehicle_status_sub{-1}; /**< vehicle status subscription */
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::SubscriptionData<airspeed_s> _airspeed_sub{ORB_ID(airspeed)};
orb_advert_t _rate_sp_pub{nullptr}; /**< rate setpoint publication */
orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */
@@ -122,8 +125,6 @@ private:
vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */
vehicle_status_s _vehicle_status {}; /**< vehicle status */
SubscriptionData<airspeed_s> _airspeed_sub;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
@@ -294,7 +295,6 @@ private:
void vehicle_manual_poll();
void vehicle_attitude_setpoint_poll();
void vehicle_rates_setpoint_poll();
void global_pos_poll();
void vehicle_status_poll();
void vehicle_land_detected_poll();