fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 17:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

vtol_att_control move to new uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-06-05 20:33:20 -04:00
parent 71d58c9278
commit c0e735c88a
2 changed files with 34 additions and 233 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_att_control_main.cpp
+17 -206
View File
@@ -140,36 +140,6 @@ VtolAttitudeControl::~VtolAttitudeControl()
VTOL_att_control::g_control = nullptr;
}
/**
* Check for changes in vehicle control mode.
*/
void VtolAttitudeControl::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);
}
}
/**
* Check for changes in manual inputs.
*/
void VtolAttitudeControl::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);
}
}
/**
* Check for inputs from mc attitude controller.
*/
@@ -196,158 +166,18 @@ void VtolAttitudeControl::actuator_controls_fw_poll()
}
}
/**
* Check for airspeed updates.
*/
void
VtolAttitudeControl::vehicle_airspeed_poll()
{
bool updated;
orb_check(_airspeed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
}
}
/**
* Check for attitude update.
*/
void
VtolAttitudeControl::vehicle_attitude_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
}
}
/**
* Check for sensor updates.
*/
void
VtolAttitudeControl::vehicle_local_pos_poll()
{
bool updated;
/* Check if parameters have changed */
orb_check(_local_pos_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
}
}
/**
* Check for setpoint updates.
*/
void
VtolAttitudeControl::vehicle_local_pos_sp_poll()
{
bool updated;
/* Check if parameters have changed */
orb_check(_local_pos_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp);
}
}
/**
* Check for position setpoint updates.
*/
void
VtolAttitudeControl::pos_sp_triplet_poll()
{
bool updated;
/* Check if parameters have changed */
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
}
/**
* Check for mc virtual attitude setpoint updates.
*/
void
VtolAttitudeControl::mc_virtual_att_sp_poll()
{
bool updated;
orb_check(_mc_virtual_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub, &_mc_virtual_att_sp);
}
}
/**
* Check for fw virtual attitude setpoint updates.
*/
void
VtolAttitudeControl::fw_virtual_att_sp_poll()
{
bool updated;
orb_check(_fw_virtual_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub, &_fw_virtual_att_sp);
}
}
/**
* Check for command updates.
*/
void
VtolAttitudeControl::vehicle_cmd_poll()
{
bool updated;
orb_check(_vehicle_cmd_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub, &_vehicle_cmd);
if (_vehicle_cmd_sub.updated()) {
_vehicle_cmd_sub.copy(&_vehicle_cmd);
handle_command();
}
}
/**
* Check for TECS status updates.
*/
void
VtolAttitudeControl::tecs_status_poll()
{
bool updated;
orb_check(_tecs_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(tecs_status), _tecs_status_sub, &_tecs_status);
}
}
/**
* Check for land detector updates.
*/
void
VtolAttitudeControl::land_detected_poll()
{
bool updated;
orb_check(_land_detected_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected);
}
}
/**
* Check received command
*/
@@ -526,23 +356,7 @@ VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
void VtolAttitudeControl::task_main()
{
fflush(stdout);
/* do subscriptions */
_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_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));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
_tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
_actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
@@ -557,13 +371,10 @@ void VtolAttitudeControl::task_main()
while (!_task_should_exit) {
/* only update parameters if they changed */
bool params_updated = false;
orb_check(_params_sub, &params_updated);
if (params_updated) {
if (_params_sub.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();
@@ -598,16 +409,16 @@ void VtolAttitudeControl::task_main()
continue;
}
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_attitude_poll();
vehicle_local_pos_poll();
vehicle_local_pos_sp_poll();
pos_sp_triplet_poll();
vehicle_airspeed_poll();
_v_control_mode_sub.update(&_v_control_mode);
_manual_control_sp_sub.update(&_manual_control_sp);
_v_att_sub.update(&_v_att);
_local_pos_sub.update(&_local_pos);
_local_pos_sp_sub.update(&_local_pos_sp);
_pos_sp_triplet_sub.update(&_pos_sp_triplet);
_airspeed_sub.update(&_airspeed);
_tecs_status_sub.update(&_tecs_status);
_land_detected_sub.update(&_land_detected);
vehicle_cmd_poll();
tecs_status_poll();
land_detected_poll();
actuator_controls_fw_poll();
actuator_controls_mc_poll();
@@ -633,7 +444,7 @@ void VtolAttitudeControl::task_main()
// check in which mode we are in and call mode specific functions
if (_vtol_type->get_mode() == mode::ROTARY_WING) {
mc_virtual_att_sp_poll();
_mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
// vehicle is in rotary wing mode
_vtol_vehicle_status.vtol_in_rw_mode = true;
@@ -645,7 +456,7 @@ void VtolAttitudeControl::task_main()
} else if (_vtol_type->get_mode() == mode::FIXED_WING) {
fw_virtual_att_sp_poll();
_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
// vehicle is in fw mode
_vtol_vehicle_status.vtol_in_rw_mode = false;
@@ -656,8 +467,8 @@ void VtolAttitudeControl::task_main()
} else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC || _vtol_type->get_mode() == mode::TRANSITION_TO_FW) {
mc_virtual_att_sp_poll();
fw_virtual_att_sp_poll();
_mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
// vehicle is doing a transition
_vtol_vehicle_status.vtol_in_trans_mode = true;
src/modules/vtol_att_control/vtol_att_control_main.h
+17 -27
View File
@@ -54,7 +54,6 @@
#include <px4_tasks.h>
#include <px4_posix.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/ecl/geo/geo.h>
@@ -62,6 +61,7 @@
#include <matrix/math.hpp>
#include <parameters/param.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/manual_control_setpoint.h>
@@ -123,21 +123,22 @@ private:
int _control_task{-1}; //task handle for VTOL attitude controller
/* handlers for subscriptions */
int _actuator_inputs_fw{-1}; //topic on which the fw_att_controller publishes actuator inputs
int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs
int _airspeed_sub{-1}; // airspeed subscription
int _fw_virtual_att_sp_sub{-1};
int _land_detected_sub{-1};
int _local_pos_sp_sub{-1}; // setpoint subscription
int _local_pos_sub{-1}; // sensor subscription
int _manual_control_sp_sub{-1}; //manual control setpoint subscription
int _mc_virtual_att_sp_sub{-1};
int _params_sub{-1}; //parameter updates subscription
int _pos_sp_triplet_sub{-1}; // local position setpoint subscription
int _tecs_status_sub{-1};
int _v_att_sub{-1}; //vehicle attitude subscription
int _v_control_mode_sub{-1}; //vehicle control mode subscription
int _vehicle_cmd_sub{-1};
int _actuator_inputs_fw{-1}; //topic on which the fw_att_controller publishes actuator inputs
int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; // airspeed subscription
uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)};
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor subscription
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; //manual control setpoint subscription
uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; //parameter updates subscription
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription
uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; //vehicle attitude subscription
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; //vehicle control mode subscription
uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};
//handlers for publishers
orb_advert_t _actuators_0_pub{nullptr}; //input for the mixer (roll,pitch,yaw,thrust)
@@ -212,20 +213,9 @@ private:
void task_main(); //main task
static int task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create.
void land_detected_poll();
void tecs_status_poll();
void vehicle_attitude_poll(); //Check for attitude updates.
void vehicle_cmd_poll();
void vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
void vehicle_manual_poll(); //Check for changes in manual inputs.
void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
void fw_virtual_att_sp_poll();
void mc_virtual_att_sp_poll();
void pos_sp_triplet_poll(); // Check for changes in position setpoint values
void vehicle_airspeed_poll(); // Check for changes in airspeed
void vehicle_local_pos_poll(); // Check for changes in sensor values
void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values
int parameters_update(); //Update local parameter cache