fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

use uORB ID to determine the correct rate_sp- and actuator topic to publish on

Browse Source
This commit is contained in:
tumbili 2014-12-03 09:44:34 +01:00
parent fa8ca2fc10
commit 0e33e52d4a
2 changed files with 44 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -130,12 +130,13 @@ private:
int _vehicle_status_sub; /**< vehicle status subscription */
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
orb_advert_t _rate_sp_virtual_pub; /* virtual att rates setpoint topic (vtol) */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */
orb_advert_t _actuators_virtual_fw_pub; /**publisher for VTOL vehicle*/
orb_advert_t _actuators_2_pub; /**< actuator control group 1 setpoint (Airframe) */
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
@ -334,10 +335,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
/* publications */
_rate_sp_pub(-1),
_rate_sp_virtual_pub(-1),
_attitude_sp_pub(-1),
_actuators_0_pub(-1),
_actuators_virtual_fw_pub(-1),
_actuators_2_pub(-1),
/* performance counters */
@ -402,6 +401,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
/* fetch initial parameter values */
parameters_update();
// set correct uORB ID, depending on if vehicle is VTOL or not
if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
}
else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
FixedwingAttitudeControl::~FixedwingAttitudeControl()
@ -629,19 +637,7 @@ FixedwingAttitudeControl::task_main()
orb_set_interval(_att_sub, 17);
parameters_update();
/*Publish to correct actuator control topic, depending on what airframe we are using
* If airframe is of type VTOL then we want to publish the actuator controls on the virtual fixed wing
* topic, from which the vtol_att_control module is receiving data and processing it further)*/
if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/
_actuators_virtual_fw_pub = orb_advertise(ORB_ID(actuator_controls_virtual_fw), &_actuators);
_rate_sp_virtual_pub = orb_advertise(ORB_ID(fw_virtual_rates_setpoint), &_rates_sp);
} else { /*airframe is not of type VTOL, use standard topic for controls publication*/
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
}
/* get an initial update for all sensor and status data */
vehicle_airspeed_poll();
vehicle_setpoint_poll();
@ -1008,11 +1004,10 @@ FixedwingAttitudeControl::task_main()
if (_rate_sp_pub > 0) {
/* publish the attitude rates setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
} else if (_rate_sp_virtual_pub > 0) {
/* publish the virtual attitude setpoint */
orb_publish(ORB_ID(fw_virtual_rates_setpoint), _rate_sp_virtual_pub, &_rates_sp);
orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
} else {
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
}
} else {
@ -1033,11 +1028,10 @@ FixedwingAttitudeControl::task_main()
_actuators_airframe.timestamp = hrt_absolute_time();
/* publish the actuator controls */
if (_actuators_0_pub > 0) { //normal fixed wing airframe
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else if (_actuators_0_pub < 0 && !_vehicle_status.is_rotary_wing) { //VTOL airframe
orb_publish(ORB_ID(actuator_controls_virtual_fw), _actuators_virtual_fw_pub, &_actuators);
if (_actuators_0_pub > 0) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
}
if (_actuators_2_pub > 0) {

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

@ -124,9 +124,10 @@ private:
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _v_rates_sp_virtual_pub; /* virtual att rates sp publication */
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
orb_advert_t _actuators_virtual_mc_pub; /**attitude actuator controls publication if vehicle is VTOL*/
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
@ -283,9 +284,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
/* publications */
_att_sp_pub(-1),
_v_rates_sp_pub(-1),
_v_rates_sp_virtual_pub(-1),
_actuators_0_pub(-1),
_actuators_virtual_mc_pub(-1),
_actuators_0_circuit_breaker_enabled(false),
@ -349,6 +348,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
/* fetch initial parameter values */
parameters_update();
// set correct uORB ID, depending on if vehicle is VTOL or not
if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
}
else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
MulticopterAttitudeControl::~MulticopterAttitudeControl()
@ -776,19 +784,6 @@ MulticopterAttitudeControl::task_main()
/* initialize parameters cache */
parameters_update();
/*Subscribe to correct actuator control topic, depending on what airframe we are using
* If airframe is of type VTOL then we want to publish the actuator controls on the virtual multicopter
* topic, from which the VTOL_att_control module is receiving data and processing it further)*/
if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/
_actuators_virtual_mc_pub = orb_advertise(ORB_ID(actuator_controls_virtual_mc), &_actuators);
_v_rates_sp_virtual_pub = orb_advertise(ORB_ID(mc_virtual_rates_setpoint), &_v_rates_sp);
} else { /*airframe is not of type VTOL, use standard topic for controls publication*/
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
}
/* wakeup source: vehicle attitude */
struct pollfd fds[1];
@ -849,10 +844,10 @@ MulticopterAttitudeControl::task_main()
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_v_rates_sp_virtual_pub > 0) {
_v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
} else {
@ -873,11 +868,11 @@ MulticopterAttitudeControl::task_main()
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_v_rates_sp_virtual_pub > 0) {
_v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp);
}
} else {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
@ -900,11 +895,11 @@ MulticopterAttitudeControl::task_main()
_actuators.timestamp = hrt_absolute_time();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub > 0) { //normal mutlicopter airframe
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
if (_actuators_0_pub > 0) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_0_pub < 0 && _vehicle_status.is_rotary_wing) {
orb_publish(ORB_ID(actuator_controls_virtual_mc), _actuators_virtual_mc_pub, &_actuators);
} else {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}