fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 02:00: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/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;
}
if (_v_control_mode.flag_control_attitude_enabled) {
if (_v_control_mode->get().flag_control_attitude_enabled) {
control_attitude(dt);
/* publish the attitude setpoint if needed */
@@ -245,11 +245,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
} else {
/* 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 */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
_manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp.z;
_rates_sp = math::Vector<3>(_manual_control_sp->get().y, -_manual_control_sp->get().x,
_manual_control_sp->get().r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp->get().z;
/* reset yaw setpoint after ACRO */
_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);
/* 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)
{
memset(&_v_att, 0, sizeof(_v_att));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
memset(&_v_control_mode, 0, sizeof(_v_control_mode));
memset(&_actuators, 0, sizeof(_actuators));
memset(&_armed, 0, sizeof(_armed));
// memset(&_v_att, 0, sizeof(_v_att));
// memset(&_v_att_sp, 0, sizeof(_v_att_sp));
// memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
// memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
// memset(&_v_control_mode, 0, sizeof(_v_control_mode));
// memset(&_actuators, 0, sizeof(_actuators));
// memset(&_armed, 0, sizeof(_armed));
_params.att_p.zero();
_params.rate_p.zero();
@@ -95,22 +95,22 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
float yaw_sp_move_rate = 0.0f;
_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 */
if (_v_control_mode.flag_control_velocity_enabled
|| _v_control_mode.flag_control_climb_rate_enabled) {
if (_v_control_mode->get().flag_control_velocity_enabled
|| _v_control_mode->get().flag_control_climb_rate_enabled) {
/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
//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 */
_v_att_sp.thrust = _manual_control_sp.z;
_v_att_sp.thrust = _manual_control_sp->get().z;
_publish_att_sp = true;
}
if (!_armed.armed) {
if (!_armed->get().armed) {
/* reset yaw setpoint when disarmed */
_reset_yaw_sp = true;
}
@@ -124,17 +124,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
//}
} else {
/* 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 + yaw_sp_move_rate * dt);
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) {
_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) {
_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;
@@ -144,15 +144,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
/* reset yaw setpint to current position if needed */
if (_reset_yaw_sp) {
_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;
_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 */
_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
_v_att_sp.pitch_body = -_manual_control_sp.x
_v_att_sp.roll_body = _manual_control_sp->get().y * _params.man_roll_max;
_v_att_sp.pitch_body = -_manual_control_sp->get().x
* _params.man_pitch_max;
_v_att_sp.R_valid = false;
_publish_att_sp = true;
@@ -188,7 +188,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.set(_v_att.R);
R.set(_v_att->get().R);
/* 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)
{
/* reset integral if disarmed */
if (!_armed.armed) {
if (!_armed->get().armed) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector < 3 > rates;
rates(0) = _v_att.rollspeed;
rates(1) = _v_att.pitchspeed;
rates(2) = _v_att.yawspeed;
rates(0) = _v_att->get().rollspeed;
rates(1) = _v_att->get().pitchspeed;
rates(2) = _v_att->get().yawspeed;
/* angular rates error */
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();
protected:
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator controls */
struct actuator_armed_s _armed; /**< actuator arming status */
px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att; /**< vehicle attitude */
px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */
px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */
px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */
PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp; /**< vehicle attitude setpoint */
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_sp; /**< angular rates setpoint */