From 83246c84cf96037dda75c232c2fa89efa302c588 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 12 Oct 2020 20:03:31 +0200 Subject: [PATCH] Switch manual_control_setpoint.z scaling from [0,1] to [-1,1] To be consistent with all other axes of stick input and avoid future rescaling confusion. Note: for the MAVLink message 69 MANUAL_CONTROL it's using the full range according to the message specs now [-1000,1000]. --- msg/ManualControlSetpoint.msg | 4 +-- .../functions/FunctionManualRC.hpp | 2 +- .../airship_att_control_main.cpp | 2 +- src/modules/commander/Commander.cpp | 4 +-- .../tasks/Utility/Sticks.cpp | 3 +-- .../FixedwingAttitudeControl.cpp | 9 ++++--- .../FixedwingPositionControl.cpp | 27 +++++++------------ .../FixedwingPositionControl.hpp | 4 +-- .../BlockLocalPositionEstimator.hpp | 1 - src/modules/manual_control/ManualControl.cpp | 17 +++++------- src/modules/mavlink/mavlink_receiver.cpp | 9 ++++--- .../mc_att_control/mc_att_control_main.cpp | 6 ++--- .../MulticopterRateControl.cpp | 2 +- 13 files changed, 38 insertions(+), 52 deletions(-) diff --git a/msg/ManualControlSetpoint.msg b/msg/ManualControlSetpoint.msg index bbbdd0bc66..a3162f4f07 100644 --- a/msg/ManualControlSetpoint.msg +++ b/msg/ManualControlSetpoint.msg @@ -32,11 +32,11 @@ float32 y # stick position in y direction -1..1 # in general a positive value means right or positive roll and # a negative value means left or negative roll -float32 z # throttle stick position 0..1 +float32 z # throttle stick position -1..1 # in general corresponds to up/down motion or thrust of vehicle, # in general the value corresponds to the demanded throttle by the user, # if the input is used for setting the setpoint of a vertical position - # controller any value > 0.5 means up and any value < 0.5 means down + # controller any value > 0 means up and any value < 0 means down float32 r # yaw stick/twist position, -1..1 # in general corresponds to the righthand rotation around the vertical diff --git a/src/lib/mixer_module/functions/FunctionManualRC.hpp b/src/lib/mixer_module/functions/FunctionManualRC.hpp index 9ec2db2b6d..32a8477e88 100644 --- a/src/lib/mixer_module/functions/FunctionManualRC.hpp +++ b/src/lib/mixer_module/functions/FunctionManualRC.hpp @@ -59,7 +59,7 @@ public: if (_topic.update(&manual_control_setpoint)) { _data[0] = manual_control_setpoint.y; // roll _data[1] = manual_control_setpoint.x; // pitch - _data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle + _data[2] = manual_control_setpoint.z; // throttle _data[3] = manual_control_setpoint.r; // yaw _data[4] = manual_control_setpoint.flaps; _data[5] = manual_control_setpoint.aux1; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index d66f82840e..2256d12b4f 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -93,7 +93,7 @@ AirshipAttitudeControl::publish_actuator_controls() _actuator_controls.control[0] = 0.0f; _actuator_controls.control[1] = _manual_control_setpoint.x; _actuator_controls.control[2] = _manual_control_setpoint.r; - _actuator_controls.control[3] = _manual_control_setpoint.z; + _actuator_controls.control[3] = (_manual_control_setpoint.z + 1.f) * .5f; } // note: _actuator_controls.timestamp_sample is set in AirshipAttitudeControl::Run() diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index cf023dd3a2..fccb7e88a3 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2714,8 +2714,8 @@ void Commander::manualControlCheck() if (manual_control_updated && manual_control_setpoint.valid) { - _is_throttle_above_center = (manual_control_setpoint.z > 0.6f); - _is_throttle_low = (manual_control_setpoint.z < 0.1f); + _is_throttle_above_center = (manual_control_setpoint.z > 0.2f); + _is_throttle_low = (manual_control_setpoint.z < -0.8f); if (_arm_state_machine.isArmed()) { // Abort autonomous mode and switch to position mode if sticks are moved significantly diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp index f43eff4fba..f2c3b8496a 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -53,8 +53,7 @@ bool Sticks::checkAndUpdateStickInputs() // Linear scale _positions(0) = manual_control_setpoint.x; // NED x, pitch [-1,1] _positions(1) = manual_control_setpoint.y; // NED y, roll [-1,1] - _positions(2) = -(math::constrain(manual_control_setpoint.z, 0.0f, - 1.0f) - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1] + _positions(2) = -manual_control_setpoint.z; // NED z, thrust [-1,1] _positions(3) = manual_control_setpoint.r; // yaw [-1,1] // Exponential scale diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 36194a5f24..c1e1f069d5 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -141,6 +141,8 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) if (!_vcontrol_mode.flag_control_climb_rate_enabled) { + const float throttle = (_manual_control_setpoint.z + 1.f) * .5f; + if (_vcontrol_mode.flag_control_attitude_enabled) { // STABILIZED mode generate the attitude setpoint from manual user inputs @@ -152,7 +154,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); _att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw - _att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + _att_sp.thrust_body[0] = throttle; Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -171,7 +173,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) _rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get()); _rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get()); _rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get()); - _rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + _rates_sp.thrust_body[0] = throttle; _rate_sp_pub.publish(_rates_sp); @@ -183,8 +185,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); _actuator_controls.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, - 1.0f); + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = throttle; _landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.r; } } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 1e7ec5fb68..55ef99fa2d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -317,20 +317,20 @@ FixedwingPositionControl::manual_control_setpoint_poll() _manual_control_setpoint_sub.update(&_manual_control_setpoint); _manual_control_setpoint_for_height_rate = _manual_control_setpoint.x; - _manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + _manual_control_setpoint_for_airspeed = _manual_control_setpoint.z; if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) { /* Alternate stick allocation (similar concept as for multirotor systems: * demanding up/down with the throttle stick, and move faster/break with the pitch one. */ - _manual_control_setpoint_for_height_rate = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f); - _manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f; + _manual_control_setpoint_for_height_rate = -_manual_control_setpoint.z; + _manual_control_setpoint_for_airspeed = _manual_control_setpoint.x; } // send neutral setpoints if no update for 1 s if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) { _manual_control_setpoint_for_height_rate = 0.f; - _manual_control_setpoint_for_airspeed = 0.5f; + _manual_control_setpoint_for_airspeed = 0.f; } } @@ -379,18 +379,9 @@ FixedwingPositionControl::get_manual_airspeed_setpoint() if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) { // neutral throttle corresponds to trim airspeed - if (_manual_control_setpoint_for_airspeed < 0.5f) { - // lower half of throttle is min to trim airspeed - altctrl_airspeed = _param_fw_airspd_min.get() + - (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) * - _manual_control_setpoint_for_airspeed * 2; - - } else { - // upper half of throttle is trim to max airspeed - altctrl_airspeed = _param_fw_airspd_trim.get() + - (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) * - (_manual_control_setpoint_for_airspeed * 2 - 1); - } + return math::interpolateNXY(_manual_control_setpoint_for_airspeed, + {-1.f, 0.f, 1.f}, + {_param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()}); } else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) { altctrl_airspeed = _commanded_manual_airspeed_setpoint; @@ -1910,7 +1901,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, float throttle_max = _param_fw_thr_max.get(); // enable the operator to kill the throttle on ground - if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) { + if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) { throttle_max = 0.0f; } @@ -1958,7 +1949,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, float throttle_max = _param_fw_thr_max.get(); // enable the operator to kill the throttle on ground - if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) { + if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) { throttle_max = 0.0f; } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index e74652031c..0afe48ebc7 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -127,7 +127,7 @@ static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s; static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s; // [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes -static constexpr float THROTTLE_THRESH = 0.05f; +static constexpr float THROTTLE_THRESH = -.9f; // [m/s/s] slew rate limit for airspeed setpoint changes static constexpr float ASPD_SP_SLEW_RATE = 1.f; @@ -293,7 +293,7 @@ private: // [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands float _manual_control_setpoint_for_height_rate{0.0f}; - // [.] normalized setpoint for manual airspeed control [0,1]; 0,0.5,1 maps to min,cruise,max airspeed commands + // [.] normalized setpoint for manual airspeed control [-1,1]; -1,0,1 maps to min,cruise,max airspeed commands float _manual_control_setpoint_for_airspeed{0.0f}; // [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index b8cf96b00a..0b312b9183 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index b0601e2749..cf4e8fc1e2 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -162,15 +162,10 @@ void ManualControl::Run() const float dt_s = (now - _last_time) / 1e6f; const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); - const bool rpy_moving = (fabsf(_x_diff.update(_selector.setpoint().x, dt_s)) > minimum_stick_change) - || (fabsf(_y_diff.update(_selector.setpoint().y, dt_s)) > minimum_stick_change) - || (fabsf(_r_diff.update(_selector.setpoint().r, dt_s)) > minimum_stick_change); - - // Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1] - const bool throttle_moving = - (fabsf(_z_diff.update(_selector.setpoint().z, dt_s)) * 2.f) > minimum_stick_change; - - _selector.setpoint().sticks_moving = rpy_moving || throttle_moving; + _selector.setpoint().sticks_moving = (fabsf(_x_diff.update(_selector.setpoint().x, dt_s)) > minimum_stick_change) + || (fabsf(_y_diff.update(_selector.setpoint().y, dt_s)) > minimum_stick_change) + || (fabsf(_z_diff.update(_selector.setpoint().z, dt_s)) > minimum_stick_change) + || (fabsf(_r_diff.update(_selector.setpoint().r, dt_s)) > minimum_stick_change); if (switches_updated) { // Only use switches if current source is RC as well. @@ -336,7 +331,7 @@ void ManualControl::processStickArming(const manual_control_setpoint_s &input) { // Arm gesture const bool right_stick_centered = (fabsf(input.x) < 0.1f) && (fabsf(input.y) < 0.1f); - const bool left_stick_lower_right = (input.z < 0.1f) && (input.r > 0.9f); + const bool left_stick_lower_right = (input.z < -0.8f) && (input.r > 0.9f); const bool previous_stick_arm_hysteresis = _stick_arm_hysteresis.get_state(); _stick_arm_hysteresis.set_state_and_update(left_stick_lower_right && right_stick_centered, input.timestamp); @@ -346,7 +341,7 @@ void ManualControl::processStickArming(const manual_control_setpoint_s &input) } // Disarm gesture - const bool left_stick_lower_left = (input.z < 0.1f) && (input.r < -0.9f); + const bool left_stick_lower_left = (input.z < -0.8f) && (input.r < -0.9f); const bool previous_stick_disarm_hysteresis = _stick_disarm_hysteresis.get_state(); _stick_disarm_hysteresis.set_state_and_update(left_stick_lower_left && right_stick_centered, input.timestamp); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index dc89b44ae8..38a30d2c53 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2138,10 +2138,11 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } manual_control_setpoint_s manual_control_setpoint{}; - manual_control_setpoint.x = mavlink_manual_control.x / 1000.0f; - manual_control_setpoint.y = mavlink_manual_control.y / 1000.0f; - manual_control_setpoint.r = mavlink_manual_control.r / 1000.0f; - manual_control_setpoint.z = mavlink_manual_control.z / 1000.0f; + manual_control_setpoint.x = mavlink_manual_control.x / 1000.f; + manual_control_setpoint.y = mavlink_manual_control.y / 1000.f; + // For backwards compatibility at the moment interpret throttle in range [0,1000] + manual_control_setpoint.z = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; + manual_control_setpoint.r = mavlink_manual_control.r / 1000.f; manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); _manual_control_input_pub.publish(manual_control_setpoint); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index b4d09217b0..169ec718af 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -116,8 +116,8 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, if (reset_yaw_sp) { _man_yaw_sp = yaw; - } else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f - || _param_mc_airmode.get() == 2) { + } else if ((_manual_control_setpoint.z > -.9f) + || (_param_mc_airmode.get() == 2)) { const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate; @@ -200,7 +200,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); q_sp.copyTo(attitude_setpoint.q_d); - attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.z, 0.f, 1.f)); + attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.z + 1.f) * .5f); attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 716ac105ed..64dc214cea 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -178,7 +178,7 @@ MulticopterRateControl::Run() math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; _rates_setpoint = man_rate_sp.emult(_acro_rate_max); - _thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f); + _thrust_setpoint(2) = -(manual_control_setpoint.z + 1.f) * .5f; _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; // publish rate setpoint