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

mc att: use multiplatform subscriptions type

Browse Source
This commit is contained in:
Thomas Gubler
2014-12-12 10:15:33 +01:00
parent 59d26f5c30
commit c0d386bce0
5 changed files with 44 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/subscriber/subscriber_example.cpp
+1 -1
View File
@@ -78,5 +78,5 @@ SubscriberExample::SubscriberExample() :
void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
msg.timestamp_last_valid, msg.timestamp_last_valid,
_sub_rc_chan->get_msg().timestamp_last_valid); _sub_rc_chan->get().timestamp_last_valid);
} }
src/modules/mc_att_control/mc_att_control.cpp
+6 -6
View File
@@ -214,7 +214,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
dt = 0.02f; dt = 0.02f;
} }
if (_v_control_mode.flag_control_attitude_enabled) { if (_v_control_mode->get().flag_control_attitude_enabled) {
control_attitude(dt); control_attitude(dt);
/* publish the attitude setpoint if needed */ /* publish the attitude setpoint if needed */
@@ -245,11 +245,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
} else { } else {
/* attitude controller disabled, poll rates setpoint topic */ /* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) { if (_v_control_mode->get().flag_control_manual_enabled) {
/* manual rates control - ACRO mode */ /* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _rates_sp = math::Vector<3>(_manual_control_sp->get().y, -_manual_control_sp->get().x,
_manual_control_sp.r).emult(_params.acro_rate_max); _manual_control_sp->get().r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp.z; _thrust_sp = _manual_control_sp->get().z;
/* reset yaw setpoint after ACRO */ /* reset yaw setpoint after ACRO */
_reset_yaw_sp = true; _reset_yaw_sp = true;
@@ -278,7 +278,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
} }
} }
if (_v_control_mode.flag_control_rates_enabled) { if (_v_control_mode->get().flag_control_rates_enabled) {
control_attitude_rates(dt); control_attitude_rates(dt);
/* publish actuator controls */ /* publish actuator controls */
src/modules/mc_att_control/mc_att_control_base.cpp
+26 -26
View File
@@ -58,13 +58,13 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
_publish_att_sp(false) _publish_att_sp(false)
{ {
memset(&_v_att, 0, sizeof(_v_att)); // memset(&_v_att, 0, sizeof(_v_att));
memset(&_v_att_sp, 0, sizeof(_v_att_sp)); // memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); // memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); // memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
memset(&_v_control_mode, 0, sizeof(_v_control_mode)); // memset(&_v_control_mode, 0, sizeof(_v_control_mode));
memset(&_actuators, 0, sizeof(_actuators)); // memset(&_actuators, 0, sizeof(_actuators));
memset(&_armed, 0, sizeof(_armed)); // memset(&_armed, 0, sizeof(_armed));
_params.att_p.zero(); _params.att_p.zero();
_params.rate_p.zero(); _params.rate_p.zero();
@@ -95,22 +95,22 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
float yaw_sp_move_rate = 0.0f; float yaw_sp_move_rate = 0.0f;
_publish_att_sp = false; _publish_att_sp = false;
if (_v_control_mode.flag_control_manual_enabled) { if (_v_control_mode->get().flag_control_manual_enabled) {
/* manual input, set or modify attitude setpoint */ /* manual input, set or modify attitude setpoint */
if (_v_control_mode.flag_control_velocity_enabled if (_v_control_mode->get().flag_control_velocity_enabled
|| _v_control_mode.flag_control_climb_rate_enabled) { || _v_control_mode->get().flag_control_climb_rate_enabled) {
/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
//XXX vehicle_attitude_setpoint_poll(); //XXX vehicle_attitude_setpoint_poll();
} }
if (!_v_control_mode.flag_control_climb_rate_enabled) { if (!_v_control_mode->get().flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude stabilized mode */ /* pass throttle directly if not in altitude stabilized mode */
_v_att_sp.thrust = _manual_control_sp.z; _v_att_sp.thrust = _manual_control_sp->get().z;
_publish_att_sp = true; _publish_att_sp = true;
} }
if (!_armed.armed) { if (!_armed->get().armed) {
/* reset yaw setpoint when disarmed */ /* reset yaw setpoint when disarmed */
_reset_yaw_sp = true; _reset_yaw_sp = true;
} }
@@ -124,17 +124,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
//} //}
} else { } else {
/* move yaw setpoint */ /* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max;
_v_att_sp.yaw_body = _wrap_pi( _v_att_sp.yaw_body = _wrap_pi(
_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att->get().yaw);
if (yaw_offs < -yaw_offs_max) { if (yaw_offs < -yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); _v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) { } else if (yaw_offs > yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); _v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max);
} }
_v_att_sp.R_valid = false; _v_att_sp.R_valid = false;
@@ -144,15 +144,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
/* reset yaw setpint to current position if needed */ /* reset yaw setpint to current position if needed */
if (_reset_yaw_sp) { if (_reset_yaw_sp) {
_reset_yaw_sp = false; _reset_yaw_sp = false;
_v_att_sp.yaw_body = _v_att.yaw; _v_att_sp.yaw_body = _v_att->get().yaw;
_v_att_sp.R_valid = false; _v_att_sp.R_valid = false;
_publish_att_sp = true; _publish_att_sp = true;
} }
if (!_v_control_mode.flag_control_velocity_enabled) { if (!_v_control_mode->get().flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */ /* update attitude setpoint if not in position control mode */
_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; _v_att_sp.roll_body = _manual_control_sp->get().y * _params.man_roll_max;
_v_att_sp.pitch_body = -_manual_control_sp.x _v_att_sp.pitch_body = -_manual_control_sp->get().x
* _params.man_pitch_max; * _params.man_pitch_max;
_v_att_sp.R_valid = false; _v_att_sp.R_valid = false;
_publish_att_sp = true; _publish_att_sp = true;
@@ -188,7 +188,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
/* rotation matrix for current state */ /* rotation matrix for current state */
math::Matrix<3, 3> R; math::Matrix<3, 3> R;
R.set(_v_att.R); R.set(_v_att->get().R);
/* all input data is ready, run controller itself */ /* all input data is ready, run controller itself */
@@ -269,15 +269,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
void MulticopterAttitudeControlBase::control_attitude_rates(float dt) void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
{ {
/* reset integral if disarmed */ /* reset integral if disarmed */
if (!_armed.armed) { if (!_armed->get().armed) {
_rates_int.zero(); _rates_int.zero();
} }
/* current body angular rates */ /* current body angular rates */
math::Vector < 3 > rates; math::Vector < 3 > rates;
rates(0) = _v_att.rollspeed; rates(0) = _v_att->get().rollspeed;
rates(1) = _v_att.pitchspeed; rates(1) = _v_att->get().pitchspeed;
rates(2) = _v_att.yawspeed; rates(2) = _v_att->get().yawspeed;
/* angular rates error */ /* angular rates error */
math::Vector < 3 > rates_err = _rates_sp - rates; math::Vector < 3 > rates_err = _rates_sp - rates;
src/modules/mc_att_control/mc_att_control_base.h
+8 -7
View File
@@ -86,13 +86,14 @@ public:
void set_actuator_controls(); void set_actuator_controls();
protected: protected:
struct vehicle_attitude_s _v_att; /**< vehicle attitude */ px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */
struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator controls */ PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp; /**< vehicle attitude setpoint */
struct actuator_armed_s _armed; /**< actuator arming status */ PX4_TOPIC_T(vehicle_rates_setpoint) _v_rates_sp; /**< vehicle rates setpoint */
PX4_TOPIC_T(actuator_controls) _actuators; /**< actuator controls */
math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_prev; /**< angular rates on previous step */
math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_sp; /**< angular rates setpoint */
src/platforms/px4_subscriber.h
+3 -3
View File
@@ -79,7 +79,7 @@ public:
/** /**
* Get the last message value * Get the last message value
*/ */
virtual const M& get_msg() = 0; virtual const M& get() = 0;
}; };
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
@@ -120,7 +120,7 @@ public:
/** /**
* Get the last message value * Get the last message value
*/ */
const M& get_msg() { return _msg_current; } const M& get() { return _msg_current; }
protected: protected:
/** /**
@@ -210,7 +210,7 @@ public:
/** /**
* Get the last message value * Get the last message value
*/ */
const M& get_msg() { return uORB::Subscription<M>::getData(); } const M& get() { return uORB::Subscription<M>::getData(); }
protected: protected:
std::function<void(const M &)> _callback; /**< Callback handle, std::function<void(const M &)> _callback; /**< Callback handle,