diff options
Diffstat (limited to 'src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp')
-rw-r--r-- | src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp | 78 |
1 files changed, 39 insertions, 39 deletions
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index aff59778e..0de832df9 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -92,28 +92,28 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _publish_att_sp = false; - if (_v_control_mode->get().flag_control_manual_enabled) { + if (_v_control_mode->data().flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ - if (_v_control_mode->get().flag_control_velocity_enabled - || _v_control_mode->get().flag_control_climb_rate_enabled) { + if (_v_control_mode->data().flag_control_velocity_enabled + || _v_control_mode->data().flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); + memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); } - if (!_v_control_mode->get().flag_control_climb_rate_enabled) { + if (!_v_control_mode->data().flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp_mod.thrust = _manual_control_sp->get().z; + _v_att_sp_mod.data().thrust = _manual_control_sp->data().z; _publish_att_sp = true; } - if (!_armed->get().armed) { + if (!_armed->data().armed) { /* reset yaw setpoint when disarmed */ _reset_yaw_sp = true; } /* move yaw setpoint in all modes */ - if (_v_att_sp_mod.thrust < 0.1f) { + if (_v_att_sp_mod.data().thrust < 0.1f) { // TODO //if (_status.condition_landed) { /* reset yaw setpoint if on ground */ @@ -121,71 +121,71 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) //} } else { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max; - _v_att_sp_mod.yaw_body = _wrap_pi( - _v_att_sp_mod.yaw_body + yaw_sp_move_rate * dt); + yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max; + _v_att_sp_mod.data().yaw_body = _wrap_pi( + _v_att_sp_mod.data().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_mod.yaw_body - _v_att->get().yaw); + float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw); if (yaw_offs < -yaw_offs_max) { - _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max); + _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { - _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max); + _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max); } - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } /* reset yaw setpint to current position if needed */ if (_reset_yaw_sp) { _reset_yaw_sp = false; - _v_att_sp_mod.yaw_body = _v_att->get().yaw; - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().yaw_body = _v_att->data().yaw; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } - if (!_v_control_mode->get().flag_control_velocity_enabled) { + if (!_v_control_mode->data().flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp_mod.roll_body = _manual_control_sp->get().y * _params.man_roll_max; - _v_att_sp_mod.pitch_body = -_manual_control_sp->get().x + _v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max; + _v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max; - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); + memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; } - _thrust_sp = _v_att_sp_mod.thrust; + _thrust_sp = _v_att_sp_mod.data().thrust; /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; - if (_v_att_sp_mod.R_valid) { + if (_v_att_sp_mod.data().R_valid) { /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp_mod.R_body[0]); + R_sp.set(&_v_att_sp_mod.data().R_body[0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp_mod.roll_body, _v_att_sp_mod.pitch_body, - _v_att_sp_mod.yaw_body); + R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body, + _v_att_sp_mod.data().yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp_mod.R_body[0], &R_sp.data[0][0], - sizeof(_v_att_sp_mod.R_body)); - _v_att_sp_mod.R_valid = true; + memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0], + sizeof(_v_att_sp_mod.data().R_body)); + _v_att_sp_mod.data().R_valid = true; } /* rotation matrix for current state */ math::Matrix<3, 3> R; - R.set(_v_att->get().R); + R.set(_v_att->data().R); /* all input data is ready, run controller itself */ @@ -266,15 +266,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_armed->get().armed || !_v_status->get().is_rotary_wing) { + if (!_armed->data().armed || !_v_status->data().is_rotary_wing) { _rates_int.zero(); } /* current body angular rates */ math::Vector < 3 > rates; - rates(0) = _v_att->get().rollspeed; - rates(1) = _v_att->get().pitchspeed; - rates(2) = _v_att->get().yawspeed; + rates(0) = _v_att->data().rollspeed; + rates(1) = _v_att->data().pitchspeed; + rates(2) = _v_att->data().yawspeed; /* angular rates error */ math::Vector < 3 > rates_err = _rates_sp - rates; @@ -302,8 +302,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) void MulticopterAttitudeControlBase::set_actuator_controls() { - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; } |