From fbc109436f00a815eb6130c6d177086e6b1ae80a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Dec 2021 18:26:04 +0100 Subject: [PATCH] MultiCopterRateControl: refactor setpoint naming --- .../MulticopterRateControl.cpp | 80 ++++++++++--------- .../MulticopterRateControl.hpp | 12 +-- 2 files changed, 47 insertions(+), 45 deletions(-) diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 8709e2a1f4..c4bb0a7824 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -180,31 +180,33 @@ MulticopterRateControl::Run() math::superexpo(-manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; - _rates_sp = man_rate_sp.emult(_acro_rate_max); - _thrust_sp = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f); + _rates_setpoint = man_rate_sp.emult(_acro_rate_max); + _thrust_setpoint = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f); // publish rate setpoint - vehicle_rates_setpoint_s v_rates_sp{}; - v_rates_sp.roll = _rates_sp(0); - v_rates_sp.pitch = _rates_sp(1); - v_rates_sp.yaw = _rates_sp(2); - v_rates_sp.thrust_body[0] = 0.0f; - v_rates_sp.thrust_body[1] = 0.0f; - v_rates_sp.thrust_body[2] = -_thrust_sp; - v_rates_sp.timestamp = hrt_absolute_time(); - - _v_rates_sp_pub.publish(v_rates_sp); + vehicle_rates_setpoint_s v_rates_setpoint{}; + v_rates_setpoint.roll = _rates_setpoint(0); + v_rates_setpoint.pitch = _rates_setpoint(1); + v_rates_setpoint.yaw = _rates_setpoint(2); + v_rates_setpoint.thrust_body[0] = 0.0f; + v_rates_setpoint.thrust_body[1] = 0.0f; + v_rates_setpoint.thrust_body[2] = _thrust_setpoint; + v_rates_setpoint.timestamp = hrt_absolute_time(); + + _v_rates_setpoint_pub.publish(v_rates_setpoint); } } else { // use rates setpoint topic - vehicle_rates_setpoint_s v_rates_sp; - - if (_v_rates_sp_sub.update(&v_rates_sp)) { - _rates_sp(0) = PX4_ISFINITE(v_rates_sp.roll) ? v_rates_sp.roll : rates(0); - _rates_sp(1) = PX4_ISFINITE(v_rates_sp.pitch) ? v_rates_sp.pitch : rates(1); - _rates_sp(2) = PX4_ISFINITE(v_rates_sp.yaw) ? v_rates_sp.yaw : rates(2); - _thrust_sp = -v_rates_sp.thrust_body[2]; + vehicle_rates_setpoint_s vehicle_rates_setpoint; + + if (_v_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + if (_v_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { + _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); + _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); + _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); + _thrust_setpoint = -vehicle_rates_setpoint.thrust_body[2]; + } } } @@ -239,7 +241,7 @@ MulticopterRateControl::Run() } // run rate controller - const Vector3f att_control = _rate_control.update(rates, _rates_sp, angular_accel, dt, _maybe_landed || _landed); + const Vector3f att_control = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; @@ -252,13 +254,13 @@ MulticopterRateControl::Run() actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f; actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f; actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f; - actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f; + actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint) ? _thrust_setpoint : 0.0f; actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear; actuators.timestamp_sample = angular_velocity.timestamp_sample; if (!_vehicle_status.is_vtol) { publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample); - publishThrustSetpoint(angular_velocity.timestamp_sample); + publishThrustSetpoint(_thrust_setpoint, angular_velocity.timestamp_sample); } // scale effort by battery status if enabled @@ -298,26 +300,26 @@ MulticopterRateControl::Run() void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime ×tamp_sample) { - vehicle_torque_setpoint_s v_torque_sp = {}; - v_torque_sp.timestamp = hrt_absolute_time(); - v_torque_sp.timestamp_sample = timestamp_sample; - v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f; - v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f; - v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f; - - _vehicle_torque_setpoint_pub.publish(v_torque_sp); + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + vehicle_torque_setpoint.timestamp_sample = timestamp_sample; + vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f; + vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f; + vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f; + + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); } -void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample) +void MulticopterRateControl::publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime ×tamp_sample) { - vehicle_thrust_setpoint_s v_thrust_sp = {}; - v_thrust_sp.timestamp = hrt_absolute_time(); - v_thrust_sp.timestamp_sample = timestamp_sample; - v_thrust_sp.xyz[0] = 0.0f; - v_thrust_sp.xyz[1] = 0.0f; - v_thrust_sp.xyz[2] = PX4_ISFINITE(_thrust_sp) ? -_thrust_sp : 0.0f; // Z is Down - - _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + vehicle_thrust_setpoint.timestamp_sample = timestamp_sample; + vehicle_thrust_setpoint.xyz[0] = 0.0f; + vehicle_thrust_setpoint.xyz[1] = 0.0f; + vehicle_thrust_setpoint.xyz[2] = PX4_ISFINITE(thrust_setpoint) ? -thrust_setpoint : 0.0f; // Z is Down + + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); } void MulticopterRateControl::updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt) diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 4117dfd88d..2e0523c660 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -94,7 +94,7 @@ private: void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt); void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime ×tamp_sample); - void publishThrustSetpoint(const hrt_abstime ×tamp_sample); + void publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime ×tamp_sample); RateControl _rate_control; ///< class for rate control calculations @@ -103,7 +103,7 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _v_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; @@ -115,7 +115,7 @@ private: uORB::Publication _actuators_0_pub; uORB::Publication _actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)}; uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; - uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _v_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; @@ -132,9 +132,9 @@ private: perf_counter_t _loop_perf; /**< loop duration performance counter */ - matrix::Vector3f _rates_sp; /**< angular rates setpoint */ - - float _thrust_sp{0.0f}; /**< thrust setpoint */ + // keep setpoint values between updates + matrix::Vector3f _rates_setpoint{}; + float _thrust_setpoint{0.0f}; hrt_abstime _last_run{0};