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

mc_att_control move most orb subscriptions to uORB::Subscription #12120

Merged
merged 1 commit into from
Jun 6, 2019
Merged

mc_att_control move most orb subscriptions to uORB::Subscription #12120

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
37 changes: 15 additions & 22 deletions src/modules/mc_att_control/mc_att_control.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <px4_module_params.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
Expand Down Expand Up @@ -101,19 +102,10 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
/**
* Check for parameter update and handle it.
*/
void battery_status_poll();
void parameter_update_poll();
void sensor_bias_poll();
void vehicle_land_detected_poll();
void sensor_correction_poll();
bool vehicle_attitude_poll();
void vehicle_attitude_setpoint_poll();
void vehicle_control_mode_poll();
bool vehicle_manual_poll();
void vehicle_motor_limits_poll();
bool vehicle_rates_setpoint_poll();
void vehicle_status_poll();
void landing_gear_state_poll();

void publish_actuator_controls();
void publish_rates_setpoint();
Expand Down Expand Up @@ -150,20 +142,21 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>

AttitudeControl _attitude_control; /**< class for attitude control calculations */

int _v_att_sub{-1}; /**< vehicle attitude subscription */
int _v_att_sp_sub{-1}; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub{-1}; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub{-1}; /**< vehicle control mode subscription */
int _params_sub{-1}; /**< parameter updates subscription */
int _manual_control_sp_sub{-1}; /**< manual control setpoint subscription */
int _vehicle_status_sub{-1}; /**< vehicle status subscription */
int _motor_limits_sub{-1}; /**< motor limits subscription */
int _battery_status_sub{-1}; /**< battery status subscription */
uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude subscription */
uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};

int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */
int _sensor_correction_sub{-1}; /**< sensor thermal correction subscription */
int _sensor_bias_sub{-1}; /**< sensor in-run bias correction subscription */
int _vehicle_land_detected_sub{-1}; /**< vehicle land detected subscription */
int _landing_gear_sub{-1};

unsigned _gyro_count{1};
int _selected_gyro{0};
Expand Down
211 changes: 23 additions & 188 deletions src/modules/mc_att_control/mc_att_control_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,83 +171,20 @@ MulticopterAttitudeControl::parameters_updated()
void
MulticopterAttitudeControl::parameter_update_poll()
{
bool updated;

/* Check if parameters have changed */
orb_check(_params_sub, &updated);
parameter_update_s param_update;

if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
if (_params_sub.update(&param_update)) {
updateParams();
parameters_updated();
}
}

void
MulticopterAttitudeControl::vehicle_control_mode_poll()
{
bool updated;

/* Check if vehicle control mode has changed */
orb_check(_v_control_mode_sub, &updated);

if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
}
}

bool
MulticopterAttitudeControl::vehicle_manual_poll()
{
bool updated;

/* get pilots inputs */
orb_check(_manual_control_sp_sub, &updated);

if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
return true;
}
return false;
}

void
MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sp_sub, &updated);

if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
}
}

bool
MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_rates_sp_sub, &updated);

if (updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
return true;
}
return false;
}

void
MulticopterAttitudeControl::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 == nullptr) {
if (_vehicle_status.is_vtol) {
Expand All @@ -271,41 +208,20 @@ void
MulticopterAttitudeControl::vehicle_motor_limits_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_motor_limits_sub, &updated);

if (updated) {
multirotor_motor_limits_s motor_limits = {};
orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &motor_limits);
multirotor_motor_limits_s motor_limits{};

if (_motor_limits_sub.update(&motor_limits)) {
_saturation_status.value = motor_limits.saturation_status;
}
}

void
MulticopterAttitudeControl::battery_status_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_battery_status_sub, &updated);

if (updated) {
orb_copy(ORB_ID(battery_status), _battery_status_sub, &_battery_status);
}
}

bool
MulticopterAttitudeControl::vehicle_attitude_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_v_att_sub, &updated);

if (updated) {
uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter;

orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter;

if (_v_att_sub.update(&_v_att)) {
// Check for a heading reset
if (prev_quat_reset_counter != _v_att.quat_reset_counter) {
// we only extract the heading change from the delta quaternion
Expand All @@ -316,60 +232,6 @@ MulticopterAttitudeControl::vehicle_attitude_poll()
return false;
}

void
MulticopterAttitudeControl::sensor_correction_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_sensor_correction_sub, &updated);

if (updated) {
orb_copy(ORB_ID(sensor_correction), _sensor_correction_sub, &_sensor_correction);
}

/* update the latest gyro selection */
if (_sensor_correction.selected_gyro_instance < _gyro_count) {
_selected_gyro = _sensor_correction.selected_gyro_instance;
}
}

void
MulticopterAttitudeControl::sensor_bias_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_sensor_bias_sub, &updated);

if (updated) {
orb_copy(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensor_bias);
}

}

void
MulticopterAttitudeControl::vehicle_land_detected_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_vehicle_land_detected_sub, &updated);

if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
}

}

void
MulticopterAttitudeControl::landing_gear_state_poll()
{
bool updated;
orb_check(_landing_gear_sub, &updated);

if (updated) {
orb_copy(ORB_ID(landing_gear), _landing_gear_sub, &_landing_gear);
}
}

float
MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
{
Expand Down Expand Up @@ -519,7 +381,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
void
MulticopterAttitudeControl::control_attitude()
{
vehicle_attitude_setpoint_poll();
_v_att_sp_sub.update(&_v_att_sp);

// reinitialize the setpoint while not armed to make sure no value from the last flight is still kept
if (!_v_control_mode.flag_armed) {
Expand Down Expand Up @@ -714,31 +576,12 @@ MulticopterAttitudeControl::publish_actuator_controls()
void
MulticopterAttitudeControl::run()
{

/*
* do subscriptions
*/
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));

_gyro_count = math::constrain(orb_group_count(ORB_ID(sensor_gyro)), 1, MAX_GYRO_COUNT);

for (unsigned s = 0; s < _gyro_count; s++) {
_sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}

_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_landing_gear_sub = orb_subscribe(ORB_ID(landing_gear));

/* wakeup source: gyro data from sensor selected by the sensor app */
px4_pollfd_struct_t poll_fds = {};
poll_fds.events = POLLIN;
Expand All @@ -754,7 +597,13 @@ MulticopterAttitudeControl::run()
while (!should_exit()) {

// check if the selected gyro has updated first
sensor_correction_poll();
_sensor_correction_sub.update(&_sensor_correction);

/* update the latest gyro selection */
if (_sensor_correction.selected_gyro_instance < _gyro_count) {
_selected_gyro = _sensor_correction.selected_gyro_instance;
}

poll_fds.fd = _sensor_gyro_sub[_selected_gyro];

/* wait for up to 100ms for data */
Expand Down Expand Up @@ -795,15 +644,16 @@ MulticopterAttitudeControl::run()
}

/* check for updates in other topics */
vehicle_control_mode_poll();
_v_control_mode_sub.update(&_v_control_mode);
_battery_status_sub.update(&_battery_status);
_sensor_bias_sub.update(&_sensor_bias);
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
_landing_gear_sub.update(&_landing_gear);
vehicle_status_poll();
vehicle_motor_limits_poll();
battery_status_poll();
sensor_bias_poll();
vehicle_land_detected_poll();
landing_gear_state_poll();
const bool manual_control_updated = vehicle_manual_poll();
const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
const bool attitude_updated = vehicle_attitude_poll();

attitude_dt += dt;

/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
Expand Down Expand Up @@ -856,7 +706,7 @@ MulticopterAttitudeControl::run()

} else {
/* attitude controller disabled, poll rates setpoint topic */
if (vehicle_rates_setpoint_poll()) {
if (_v_rates_sp_sub.update(&_v_rates_sp)) {
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
Expand Down Expand Up @@ -905,24 +755,9 @@ MulticopterAttitudeControl::run()
perf_end(_loop_perf);
}

orb_unsubscribe(_v_att_sub);
orb_unsubscribe(_v_att_sp_sub);
orb_unsubscribe(_v_rates_sp_sub);
orb_unsubscribe(_v_control_mode_sub);
orb_unsubscribe(_params_sub);
orb_unsubscribe(_manual_control_sp_sub);
orb_unsubscribe(_vehicle_status_sub);
orb_unsubscribe(_motor_limits_sub);
orb_unsubscribe(_battery_status_sub);

for (unsigned s = 0; s < _gyro_count; s++) {
orb_unsubscribe(_sensor_gyro_sub[s]);
}

orb_unsubscribe(_sensor_correction_sub);
orb_unsubscribe(_sensor_bias_sub);
orb_unsubscribe(_vehicle_land_detected_sub);
orb_unsubscribe(_landing_gear_sub);
}

int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])
Expand Down