Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 15:08:16 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

mc_rate_control: simplify manual_control_setpoint update

Browse Source
This commit is contained in:
Daniel Agar
2021-03-03 19:40:43 -05:00
parent bb12fce66c
commit cc3c6f63e5
2 changed files with 8 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_rate_control/MulticopterRateControl.cpp
+8 -21
View File
@@ -159,32 +159,19 @@ MulticopterRateControl::Run()
} }
} }
const bool manual_control_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint); if (_v_control_mode.flag_control_manual_enabled && !_v_control_mode.flag_control_attitude_enabled) {
// generate the rate setpoint from sticks
// generate the rate setpoint from sticks? manual_control_setpoint_s manual_control_setpoint;
bool manual_rate_sp = false;
if (_v_control_mode.flag_control_manual_enabled &&
!_v_control_mode.flag_control_altitude_enabled &&
!_v_control_mode.flag_control_velocity_enabled &&
!_v_control_mode.flag_control_position_enabled) {
if (!_v_control_mode.flag_control_attitude_enabled) {
manual_rate_sp = true;
}
}
if (manual_rate_sp) {
if (manual_control_updated) {
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// manual rates control - ACRO mode // manual rates control - ACRO mode
const Vector3f man_rate_sp{ const Vector3f man_rate_sp{
math::superexpo(_manual_control_setpoint.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), math::superexpo(manual_control_setpoint.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(-_manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), 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())}; 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); _rates_sp = man_rate_sp.emult(_acro_rate_max);
_thrust_sp = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); _thrust_sp = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
// publish rate setpoint // publish rate setpoint
vehicle_rates_setpoint_s v_rates_sp{}; vehicle_rates_setpoint_s v_rates_sp{};
src/modules/mc_rate_control/MulticopterRateControl.hpp
-1
View File
@@ -108,7 +108,6 @@ private:
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_control_mode_s _v_control_mode{}; vehicle_control_mode_s _v_control_mode{};
vehicle_status_s _vehicle_status{}; vehicle_status_s _vehicle_status{};