Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 16:49:51 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

if vtol, publish virtual attitude setpoint

Browse Source 
Conflicts:
	src/modules/mc_pos_control/mc_pos_control_main.cpp
This commit is contained in:
tumbili
2015-09-07 09:21:24 +02:00
parent 932883e303
commit d1dc8ed432
3 changed files with 50 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
+11 -9
View File
@@ -59,6 +59,7 @@
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
@@ -142,6 +143,7 @@ private:
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
orb_id_t _attitude_setpoint_id;
struct control_state_s _ctrl_state; /**< control state */
struct accel_report _accel; /**< body frame accelerations */
@@ -360,6 +362,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_rates_sp_id(0),
_actuators_id(0),
_attitude_setpoint_id(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -633,9 +636,11 @@ FixedwingAttitudeControl::vehicle_status_poll()
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
@@ -1021,15 +1026,12 @@ FixedwingAttitudeControl::task_main()
att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */
if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
}
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp);
} else if (_attitude_setpoint_id) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp);
}
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+18 -4
View File
@@ -72,6 +72,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@@ -166,6 +167,8 @@ private:
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
orb_id_t _attitude_setpoint_id;
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
@@ -506,6 +509,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
_tecs_status_pub(nullptr),
_nav_capabilities_pub(nullptr),
/* publication ID */
_attitude_setpoint_id(0),
/* states */
_ctrl_state(),
_att_sp(),
@@ -766,6 +772,14 @@ FixedwingPositionControl::vehicle_status_poll()
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
}
@@ -1990,13 +2004,13 @@ FixedwingPositionControl::task_main()
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);
} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
} else if (_attitude_setpoint_id) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
/* XXX check if radius makes sense here */
src/modules/mc_pos_control/mc_pos_control_main.cpp
+21 -10
View File
@@ -69,6 +69,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/mc_virtual_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
@@ -141,6 +142,8 @@ private:
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
orb_id_t _attitude_setpoint_id;
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct control_state_s _ctrl_state; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
@@ -336,6 +339,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_att_sp_pub(nullptr),
_local_pos_sp_pub(nullptr),
_global_vel_sp_pub(nullptr),
_attitude_setpoint_id(0),
_manual_thr_min(this, "MANTHR_MIN"),
_manual_thr_max(this, "MANTHR_MAX"),
_vel_x_deriv(this, "VELD"),
@@ -533,6 +537,14 @@ MulticopterPositionControl::poll_subscriptions()
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
orb_check(_ctrl_state_sub, &updated);
@@ -1183,10 +1195,9 @@ MulticopterPositionControl::task_main()
/* publish attitude setpoint */
if (_att_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
} else {
@@ -1615,13 +1626,13 @@ MulticopterPositionControl::task_main()
* attitude setpoints for the transition).
*/
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
} else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
}