From c0d386bce0a2fd7d119dd8495d1ca68d985ae411 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 12 Dec 2014 10:15:33 +0100 Subject: mc att: use multiplatform subscriptions type --- src/modules/mc_att_control/mc_att_control.cpp | 12 ++--- src/modules/mc_att_control/mc_att_control_base.cpp | 52 +++++++++++----------- src/modules/mc_att_control/mc_att_control_base.h | 15 ++++--- 3 files changed, 40 insertions(+), 39 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index bbda1f8aa..59eaaec6d 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -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 */ diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 0a05be924..b9a57ce69 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -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; diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 0783bb71e..05f89abe6 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -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 */ -- cgit v1.2.3