yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control move most orb subscriptions to uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-05-30 13:48:31 -04:00
parent c0e735c88a
commit 4c42cac380
2 changed files with 38 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
src/modules/mc_att_control/mc_att_control.hpp
View File

@@ -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>
@@ -101,19 +102,10 @@ private:
/**
* 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();
@@ -150,20 +142,21 @@ private:
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};

211
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@@ -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) {
@@ -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
@@ -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)
{
@@ -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) {
@@ -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;
@@ -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 */
@@ -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
@@ -864,7 +714,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;
@@ -913,24 +763,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[])