dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_rate_control: simplify manual_control_setpoint update

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -159,32 +159,19 @@ MulticopterRateControl::Run()
}
}
const bool manual_control_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint);
// generate the rate setpoint from sticks?
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 (_v_control_mode.flag_control_manual_enabled && !_v_control_mode.flag_control_attitude_enabled) {
// generate the rate setpoint from sticks
manual_control_setpoint_s manual_control_setpoint;
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// manual rates control - ACRO mode
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.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.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.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);
_thrust_sp = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
// publish rate setpoint
vehicle_rates_setpoint_s v_rates_sp{};

1
src/modules/mc_rate_control/MulticopterRateControl.hpp
View File

@ -108,7 +108,6 @@ private:
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 */
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_control_mode_s _v_control_mode{};
vehicle_status_s _vehicle_status{};