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.

Sign up for GitHub

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

fw_att_control move orb subscriptions to uORB::Subscription #12184

Merged
merged 1 commit into from
Jun 7, 2019
Merged

fw_att_control move orb subscriptions to uORB::Subscription #12184

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
99 changes: 17 additions & 82 deletions src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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")),
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand All @@ -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) {
Expand Down Expand Up @@ -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;
Expand All @@ -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) {
Expand All @@ -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;
}
}
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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 (updated) {
battery_status_s battery_status = {};
if (_battery_status_sub.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;
}
Expand Down
24 changes: 12 additions & 12 deletions src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,15 +93,18 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
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 */
Expand All @@ -122,8 +125,6 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
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 */
Expand Down Expand Up @@ -294,7 +295,6 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
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();

Expand Down