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

mc att: use multiplatform publisher

Browse Source
This commit is contained in:
Thomas Gubler
2014-12-11 19:02:14 +01:00
parent b755312a1e
commit ab35b6470c
4 changed files with 31 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control.cpp
+23 -25
View File
@@ -74,10 +74,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_armed_sub(-1), _armed_sub(-1),
/* publications */ /* publications */
_att_sp_pub(-1), _att_sp_pub(nullptr),
_v_rates_sp_pub(-1), _v_rates_sp_pub(nullptr),
_actuators_0_pub(-1), _actuators_0_pub(nullptr),
n(), _n(),
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
@@ -111,19 +111,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
* do subscriptions * do subscriptions
*/ */
// _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); // _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
// _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); // _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, 0); PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
// _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); // _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
PX4_SUBSCRIBE(n, vehicle_rates_setpoint, 0); PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
// _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); // _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
PX4_SUBSCRIBE(n, vehicle_control_mode, 0); PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
// _params_sub = orb_subscribe(ORB_ID(parameter_update)); // _params_sub = orb_subscribe(ORB_ID(parameter_update));
PX4_SUBSCRIBE(n, parameter_update, 0); PX4_SUBSCRIBE(_n, parameter_update, 0);
// _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); // _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
PX4_SUBSCRIBE(n, manual_control_setpoint, 0); PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
// _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); // _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
PX4_SUBSCRIBE(n, actuator_armed, 0); PX4_SUBSCRIBE(_n, actuator_armed, 0);
} }
@@ -235,13 +235,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
if (_publish_att_sp) { if (_publish_att_sp) {
_v_att_sp.timestamp = hrt_absolute_time(); _v_att_sp.timestamp = hrt_absolute_time();
if (_att_sp_pub > 0) { if (_att_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, _att_sp_pub->publish(_v_att_sp);
&_v_att_sp);
} else { } else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint);
&_v_att_sp);
} }
} }
@@ -252,11 +250,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_v_rates_sp.thrust = _thrust_sp; _v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time(); _v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) { if (_v_rates_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); _v_rates_sp_pub->publish(_v_rates_sp);
} else { } else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
} }
} else { } else {
@@ -277,11 +275,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_v_rates_sp.thrust = _thrust_sp; _v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time(); _v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) { if (_v_rates_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); _v_rates_sp_pub->publish(_v_rates_sp);
} else { } else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
} }
} else { } else {
@@ -305,11 +303,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
_actuators.timestamp = hrt_absolute_time(); _actuators.timestamp = hrt_absolute_time();
if (!_actuators_0_circuit_breaker_enabled) { if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub > 0) { if (_actuators_0_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); _actuators_0_pub->publish(_actuators);
} else { } else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
} }
} }
} }
src/modules/mc_att_control/mc_att_control.h
+5 -5
View File
@@ -87,7 +87,7 @@ public:
void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
void spin() { n.spin(); } void spin() { _n.spin(); }
private: private:
bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_should_exit; /**< if true, sensor task should exit */
@@ -103,11 +103,11 @@ private:
int _manual_control_sp_sub; /**< manual control setpoint subscription */ int _manual_control_sp_sub; /**< manual control setpoint subscription */
int _armed_sub; /**< arming status subscription */ int _armed_sub; /**< arming status subscription */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */
px4::NodeHandle n; px4::NodeHandle _n;
struct { struct {
px4_param_t roll_p; px4_param_t roll_p;
src/modules/uORB/uORB.h
+1
View File
@@ -267,5 +267,6 @@ ORB_DECLARE(actuator_controls_0);
ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3); ORB_DECLARE(actuator_controls_3);
typedef struct actuator_controls_s actuator_controls_0_s;
#endif /* _UORB_UORB_H */ #endif /* _UORB_UORB_H */
src/platforms/px4_defines.h
+2
View File
@@ -138,6 +138,8 @@ typedef param_t px4_param_t;
#endif #endif
/* Defines for all platforms */
/* Shortcut for subscribing to topics /* Shortcut for subscribing to topics
* Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all
*/ */