From 8073a70d2bfb0af7228061b5ba2ced469a694d53 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 25 Jun 2020 10:56:12 +0200 Subject: [PATCH 1/4] mavlink_receiver: refactor forgotten manual_control_setpoint naming --- .../airship_att_control.hpp | 14 ++++++------ .../airship_att_control_main.cpp | 8 +++---- src/modules/mavlink/mavlink_receiver.cpp | 22 +++++++++---------- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index dbb876ec6298..b6ff54a46483 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -82,9 +82,9 @@ class AirshipAttitudeControl : public ModuleBase, public void publishTorqueSetpoint(const hrt_abstime ×tamp_sample); void publishThrustSetpoint(const hrt_abstime ×tamp_sample); - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< parameter updates subscription */ - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; @@ -92,10 +92,10 @@ class AirshipAttitudeControl : public ModuleBase, public uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; - manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */ - vehicle_status_s _vehicle_status {}; /**< vehicle status */ - actuator_controls_s _actuator_controls {}; /**< actuator controls */ + manual_control_setpoint_s _manual_control_setpoint{}; + vehicle_status_s _vehicle_status{}; + actuator_controls_s _actuator_controls{}; - perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _loop_perf; }; 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 e76cdf582cbc..d66f82840ea9 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -91,9 +91,9 @@ AirshipAttitudeControl::publish_actuator_controls() } else { _actuator_controls.control[0] = 0.0f; - _actuator_controls.control[1] = _manual_control_sp.x; - _actuator_controls.control[2] = _manual_control_sp.r; - _actuator_controls.control[3] = _manual_control_sp.z; + _actuator_controls.control[1] = _manual_control_setpoint.x; + _actuator_controls.control[2] = _manual_control_setpoint.r; + _actuator_controls.control[3] = _manual_control_setpoint.z; } // note: _actuator_controls.timestamp_sample is set in AirshipAttitudeControl::Run() @@ -150,7 +150,7 @@ AirshipAttitudeControl::Run() publishThrustSetpoint(angular_velocity.timestamp_sample); /* check for updates in manual control topic */ - _manual_control_sp_sub.update(&_manual_control_sp); + _manual_control_setpoint_sub.update(&_manual_control_setpoint); /* check for updates in vehicle status topic */ _vehicle_status_sub.update(&_vehicle_status); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index af3fae916359..dc89b44ae82e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2129,22 +2129,22 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) void MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) { - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); + mavlink_manual_control_t mavlink_manual_control; + mavlink_msg_manual_control_decode(msg, &mavlink_manual_control); // Check target - if (man.target != 0 && man.target != _mavlink->get_system_id()) { + if (mavlink_manual_control.target != 0 && mavlink_manual_control.target != _mavlink->get_system_id()) { return; } - manual_control_setpoint_s manual{}; - manual.x = man.x / 1000.0f; - manual.y = man.y / 1000.0f; - manual.r = man.r / 1000.0f; - manual.z = man.z / 1000.0f; - manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); - manual.timestamp = manual.timestamp_sample = hrt_absolute_time(); - _manual_control_input_pub.publish(manual); + 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.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); } void From 34729cf3498d5f4124007236f66b21f0a5cad384 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 12 Oct 2020 20:03:31 +0200 Subject: [PATCH 2/4] 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 bbbdd0bc6660..a3162f4f07d6 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 9ec2db2b6d9c..32a8477e8883 100644 --- a/src/lib/mixer_module/functions/FunctionManualRC.hpp +++ b/src/lib/mixer_module/functions/FunctionManualRC.hpp @@ -59,7 +59,7 @@ class FunctionManualRC : public FunctionProviderBase 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 d66f82840ea9..2256d12b4f8e 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 cf023dd3a25d..fccb7e88a39f 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 f43eff4fba60..f2c3b8496a05 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 36194a5f24df..c1e1f069d519 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 1e7ec5fb6867..55ef99fa2dcc 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 e74652031cf7..0afe48ebc7dd 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 @@ class FixedwingPositionControl final : public ModuleBase #include #include -#include #include #include #include diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index b0601e27497a..cf4e8fc1e227 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 dc89b44ae82e..38a30d2c53fc 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 b4d09217b010..169ec718af34 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 716ac105ed93..64dc214ceaff 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 From 03f8842fae0376e6a75e14b74c1d720675af0d95 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 27 Jun 2022 18:16:05 +0200 Subject: [PATCH 3/4] manual_control_setpoint: change stick axes naming In review it was requested to have a different name for manual_control_setpoint.z because of the adjusted range. I started to investigate what naming is most intuitive and found that most people recognize the stick axes as roll, pitch, yaw, throttle. It comes at no surprise because other autopilots and APIs seem to share this convention. While changing the code I realized that even within the code base the axes are usually assigned to a variable with that name or have comments next to the assignment clarifying the axes using these names. --- msg/ManualControlSetpoint.msg | 38 +++++++------------ .../actuators/modalai_esc/modalai_esc.cpp | 12 +++--- .../functions/FunctionManualRC.hpp | 8 ++-- .../airship_att_control_main.cpp | 6 +-- src/modules/commander/Commander.cpp | 4 +- src/modules/commander/rc_calibration.cpp | 6 +-- .../tasks/Utility/Sticks.cpp | 8 ++-- .../FixedwingAttitudeControl.cpp | 26 ++++++------- .../FixedwingPositionControl.cpp | 32 ++++++++-------- src/modules/manual_control/ManualControl.cpp | 22 +++++------ src/modules/manual_control/ManualControl.hpp | 8 ++-- src/modules/mavlink/mavlink_receiver.cpp | 8 ++-- .../mavlink/streams/MANUAL_CONTROL.hpp | 8 ++-- src/modules/mc_att_control/mc_att_control.hpp | 4 +- .../mc_att_control/mc_att_control_main.cpp | 27 ++++++------- .../mc_autotune_attitude_control.cpp | 4 +- .../MulticopterRateControl.cpp | 8 ++-- src/modules/rc_update/rc_update.cpp | 8 ++-- .../RoverPositionControl.cpp | 12 +++--- .../uuv_att_control/uuv_att_control.cpp | 6 +-- 20 files changed, 120 insertions(+), 135 deletions(-) diff --git a/msg/ManualControlSetpoint.msg b/msg/ManualControlSetpoint.msg index a3162f4f07d6..486aba8edd27 100644 --- a/msg/ManualControlSetpoint.msg +++ b/msg/ManualControlSetpoint.msg @@ -16,31 +16,15 @@ uint8 data_source # Any of the channels may not be available and be set to NaN # to indicate that it does not contain valid data. -# The variable names follow the definition of the -# MANUAL_CONTROL mavlink message. -# The default range is from -1 to 1 (mavlink message -1000 to 1000) -# The range for the z variable is defined from 0 to 1. (The z field of -# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) - -float32 x # stick position in x direction -1..1 - # in general corresponds to forward/back motion or pitch of vehicle, - # in general a positive value means forward or negative pitch and - # a negative value means backward or positive pitch - -float32 y # stick position in y direction -1..1 - # in general corresponds to right/left motion or roll of vehicle, - # in general a positive value means right or positive roll and - # a negative value means left or negative roll - -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 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 - # (downwards) axis of the vehicle + +# Stick positions [-1,1] +# on a common RC mode 1/2/3/4 remote/joystick the stick deflection: -1 is down/left, 1 is up/right +# Note: QGC sends throttle/z in range [0,1000] - [0,1]. The MAVLink input conversion [0,1] to [-1,1] is at the moment kept backwards compatible. +# Positive values are generally used for: +float32 roll # move right, positive roll rotation, right side down +float32 pitch # move forward, negative pitch rotation, nose down +float32 yaw # positive yaw rotation, clockwise when seen top down +float32 throttle # move up, positive thrust, -1 is minimum available 0% or -100% +1 is 100% thrust float32 flaps # flap position @@ -54,3 +38,7 @@ float32 aux6 bool sticks_moving # TOPICS manual_control_setpoint manual_control_input +# DEPRECATED: float32 x +# DEPRECATED: float32 y +# DEPRECATED: float32 z +# DEPRECATED: float32 r diff --git a/src/drivers/actuators/modalai_esc/modalai_esc.cpp b/src/drivers/actuators/modalai_esc/modalai_esc.cpp index 0d3148358589..e5c2832392db 100644 --- a/src/drivers/actuators/modalai_esc/modalai_esc.cpp +++ b/src/drivers/actuators/modalai_esc/modalai_esc.cpp @@ -892,9 +892,9 @@ void ModalaiEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) const float flip_pwr_mult = 1.0f - ((float)_parameters.turtle_motor_expo / 100.0f); // Sitck deflection - const float stick_def_p_abs = fabsf(_manual_control_setpoint.x); - const float stick_def_r_abs = fabsf(_manual_control_setpoint.y); - const float stick_def_y_abs = fabsf(_manual_control_setpoint.r); + const float stick_def_r_abs = fabsf(_manual_control_setpoint.roll); + const float stick_def_p_abs = fabsf(_manual_control_setpoint.pitch); + const float stick_def_y_abs = fabsf(_manual_control_setpoint.yaw); const float stick_def_p_expo = flip_pwr_mult * stick_def_p_abs + powf(stick_def_p_abs, 3.0) * (1 - flip_pwr_mult); @@ -903,9 +903,9 @@ void ModalaiEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) const float stick_def_y_expo = flip_pwr_mult * stick_def_y_abs + powf(stick_def_y_abs, 3.0) * (1 - flip_pwr_mult); - float sign_p = _manual_control_setpoint.x < 0 ? 1 : -1; - float sign_r = _manual_control_setpoint.y < 0 ? 1 : -1; - float sign_y = _manual_control_setpoint.r < 0 ? 1 : -1; + float sign_r = _manual_control_setpoint.roll < 0 ? 1 : -1; + float sign_p = _manual_control_setpoint.pitch < 0 ? 1 : -1; + float sign_y = _manual_control_setpoint.yaw < 0 ? 1 : -1; float stick_def_len = sqrtf(powf(stick_def_p_abs, 2.0) + powf(stick_def_r_abs, 2.0)); float stick_def_expo_len = sqrtf(powf(stick_def_p_expo, 2.0) + powf(stick_def_r_expo, 2.0)); diff --git a/src/lib/mixer_module/functions/FunctionManualRC.hpp b/src/lib/mixer_module/functions/FunctionManualRC.hpp index 32a8477e8883..38f7d096a94e 100644 --- a/src/lib/mixer_module/functions/FunctionManualRC.hpp +++ b/src/lib/mixer_module/functions/FunctionManualRC.hpp @@ -57,10 +57,10 @@ class FunctionManualRC : public FunctionProviderBase manual_control_setpoint_s manual_control_setpoint; 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; // throttle - _data[3] = manual_control_setpoint.r; // yaw + _data[0] = manual_control_setpoint.roll; + _data[1] = manual_control_setpoint.pitch; + _data[2] = manual_control_setpoint.throttle; + _data[3] = manual_control_setpoint.yaw; _data[4] = manual_control_setpoint.flaps; _data[5] = manual_control_setpoint.aux1; _data[6] = manual_control_setpoint.aux2; 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 2256d12b4f8e..0a4b8bdab34e 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -91,9 +91,9 @@ AirshipAttitudeControl::publish_actuator_controls() } else { _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 + 1.f) * .5f; + _actuator_controls.control[1] = _manual_control_setpoint.pitch; + _actuator_controls.control[2] = _manual_control_setpoint.yaw; + _actuator_controls.control[3] = (_manual_control_setpoint.throttle + 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 fccb7e88a39f..1e012bafe06c 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.2f); - _is_throttle_low = (manual_control_setpoint.z < -0.8f); + _is_throttle_above_center = (manual_control_setpoint.throttle > 0.2f); + _is_throttle_low = (manual_control_setpoint.throttle < -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/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 973e651aa292..ff8ba001e251 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -85,15 +85,15 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub) /* set parameters: the new trim values are the combination of active trim values and the values coming from the remote control of the user */ - float p = manual_control_setpoint.y * roll_scale + roll_trim_active; + float p = manual_control_setpoint.roll * roll_scale + roll_trim_active; int p1r = param_set(param_find("TRIM_ROLL"), &p); /* we explicitly swap sign here because the trim is added to the actuator controls which are moving in an inverse sense to manual pitch inputs */ - p = -manual_control_setpoint.x * pitch_scale + pitch_trim_active; + p = -manual_control_setpoint.pitch * pitch_scale + pitch_trim_active; int p2r = param_set(param_find("TRIM_PITCH"), &p); - p = manual_control_setpoint.r * yaw_scale + yaw_trim_active; + p = manual_control_setpoint.yaw * yaw_scale + yaw_trim_active; int p3r = param_set(param_find("TRIM_YAW"), &p); if (p1r != 0 || p2r != 0 || p3r != 0) { diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp index f2c3b8496a05..af2d28b41b7e 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -51,10 +51,10 @@ bool Sticks::checkAndUpdateStickInputs() if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { // 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) = -manual_control_setpoint.z; // NED z, thrust [-1,1] - _positions(3) = manual_control_setpoint.r; // yaw [-1,1] + _positions(0) = manual_control_setpoint.pitch; + _positions(1) = manual_control_setpoint.roll; + _positions(2) = -manual_control_setpoint.throttle; + _positions(3) = manual_control_setpoint.yaw; // Exponential scale _positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get()); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index c1e1f069d519..214bc534f4e0 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -141,14 +141,14 @@ 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; + const float throttle = (_manual_control_setpoint.throttle + 1.f) * .5f; if (_vcontrol_mode.flag_control_attitude_enabled) { // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); + _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); - _att_sp.pitch_body = -_manual_control_setpoint.x * radians(_param_fw_man_p_max.get()) + _att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) + radians(_param_fw_psp_off.get()); _att_sp.pitch_body = constrain(_att_sp.pitch_body, -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); @@ -170,9 +170,9 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) // RATE mode we need to generate the rate setpoint from manual user inputs _rates_sp.timestamp = hrt_absolute_time(); - _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.roll = _manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get()); + _rates_sp.pitch = -_manual_control_setpoint.pitch * radians(_param_fw_acro_y_max.get()); + _rates_sp.yaw = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get()); _rates_sp.thrust_body[0] = throttle; _rate_sp_pub.publish(_rates_sp); @@ -180,13 +180,13 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) } else { /* manual/direct control */ _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = - _manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); + _manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get(); _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = - -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); + -_manual_control_setpoint.pitch * _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(); + _manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = throttle; - _landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.r; + _landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.yaw; } } } @@ -551,7 +551,7 @@ void FixedwingAttitudeControl::Run() /* add yaw rate setpoint from sticks in all attitude-controlled modes */ if (_vcontrol_mode.flag_control_manual_enabled) { - body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.r * radians(_param_fw_man_yr_max.get()), + body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.yaw * radians(_param_fw_man_yr_max.get()), -radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get())); } @@ -593,11 +593,11 @@ void FixedwingAttitudeControl::Run() if (_vcontrol_mode.flag_control_manual_enabled) { // always direct control of steering wheel with yaw stick in manual modes - wheel_u = _manual_control_setpoint.r; + wheel_u = _manual_control_setpoint.yaw; } else { // XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from - // position controller during auto modes _manual_control_setpoint.r gets passed + // position controller during auto modes _manual_control_setpoint.yaw gets passed // whenever nudging is enabled, otherwise zero wheel_u = wheel_control ? _wheel_ctrl.control_bodyrate(dt, control_input) + _att_sp.yaw_sp_move_rate : 0.f; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 55ef99fa2dcc..714358aa4f8e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -316,15 +316,15 @@ 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 = _manual_control_setpoint.z; + _manual_control_setpoint_for_height_rate = _manual_control_setpoint.pitch; + _manual_control_setpoint_for_airspeed = _manual_control_setpoint.throttle; 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 = -_manual_control_setpoint.z; - _manual_control_setpoint_for_airspeed = _manual_control_setpoint.x; + _manual_control_setpoint_for_height_rate = -_manual_control_setpoint.throttle; + _manual_control_setpoint_for_airspeed = _manual_control_setpoint.pitch; } // send neutral setpoints if no update for 1 s @@ -820,7 +820,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) /* reset setpoints from other modes (auto) otherwise we won't * level out without new manual input */ - _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_r_lim.get()); + _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } @@ -1426,7 +1426,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo // XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface. if (_param_rwto_nudge.get()) { - _att_sp.yaw_sp_move_rate = _manual_control_setpoint.r; + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw; } // tune up the lateral position control guidance when on the ground @@ -1794,7 +1794,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo // XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface. if (_param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled) { - _att_sp.yaw_sp_move_rate = _manual_control_setpoint.r; + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw; } // blend the height rate controlled throttle setpoints with initial throttle setting over the flare @@ -1919,7 +1919,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, false, height_rate_sp); - _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_r_lim.get()); + _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); @@ -1954,8 +1954,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval, } /* heading control */ - if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH && - fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) { + if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH && + fabsf(_manual_control_setpoint.yaw) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ if (fabsf(_yawrate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { @@ -2028,14 +2028,14 @@ FixedwingPositionControl::control_manual_position(const float control_interval, false, height_rate_sp); - if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH || - fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) { + if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH || + fabsf(_manual_control_setpoint.yaw) >= HDG_HOLD_MAN_INPUT_THRESH) { _hdg_hold_enabled = false; _yaw_lock_engaged = false; // do slew rate limiting on roll if enabled - float roll_sp_new = _manual_control_setpoint.y * radians(_param_fw_r_lim.get()); + float roll_sp_new = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get()); if (control_interval > 0.f && roll_rate_slew_rad > 0.f) { @@ -2650,14 +2650,14 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po Vector2f FixedwingPositionControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position) { - if (fabsf(_manual_control_setpoint.r) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE + if (fabsf(_manual_control_setpoint.yaw) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE && _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled && !_flare_states.flaring) { // laterally nudge touchdown location with yaw stick // positive is defined in the direction of a right hand turn starting from the approach vector direction const float signed_deadzone_threshold = MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE * math::signNoZero( - _manual_control_setpoint.r); - _lateral_touchdown_position_offset += (_manual_control_setpoint.r - signed_deadzone_threshold) * + _manual_control_setpoint.yaw); + _lateral_touchdown_position_offset += (_manual_control_setpoint.yaw - signed_deadzone_threshold) * MAX_TOUCHDOWN_POSITION_NUDGE_RATE * control_interval; _lateral_touchdown_position_offset = math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(), _param_fw_lnd_td_off.get()); diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index cf4e8fc1e227..c3b42c258e2c 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -162,10 +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(); - _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); + _selector.setpoint().sticks_moving = (fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > minimum_stick_change) + || (fabsf(_pitch_diff.update(_selector.setpoint().pitch, dt_s)) > minimum_stick_change) + || (fabsf(_yaw_diff.update(_selector.setpoint().yaw, dt_s)) > minimum_stick_change) + || (fabsf(_throttle_diff.update(_selector.setpoint().throttle, dt_s)) > minimum_stick_change); if (switches_updated) { // Only use switches if current source is RC as well. @@ -310,10 +310,10 @@ void ManualControl::Run() _manual_control_setpoint_pub.publish(_selector.setpoint()); } - _x_diff.reset(); - _y_diff.reset(); - _z_diff.reset(); - _r_diff.reset(); + _roll_diff.reset(); + _pitch_diff.reset(); + _yaw_diff.reset(); + _throttle_diff.reset(); _stick_arm_hysteresis.set_state_and_update(false, now); _stick_disarm_hysteresis.set_state_and_update(false, now); _button_hysteresis.set_state_and_update(false, now); @@ -330,8 +330,8 @@ void ManualControl::Run() 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.8f) && (input.r > 0.9f); + const bool right_stick_centered = (fabsf(input.pitch) < 0.1f) && (fabsf(input.roll) < 0.1f); + const bool left_stick_lower_right = (input.throttle < -0.8f) && (input.yaw > 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); @@ -341,7 +341,7 @@ void ManualControl::processStickArming(const manual_control_setpoint_s &input) } // Disarm gesture - const bool left_stick_lower_left = (input.z < -0.8f) && (input.r < -0.9f); + const bool left_stick_lower_left = (input.throttle < -0.8f) && (input.yaw < -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/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 738fd92ea56b..d28166112e62 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -117,10 +117,10 @@ class ManualControl : public ModuleBase, public ModuleParams, pub ManualControlSelector _selector; bool _published_invalid_once{false}; - MovingDiff _x_diff{}; - MovingDiff _y_diff{}; - MovingDiff _z_diff{}; - MovingDiff _r_diff{}; + MovingDiff _roll_diff{}; + MovingDiff _pitch_diff{}; + MovingDiff _yaw_diff{}; + MovingDiff _throttle_diff{}; manual_control_switches_s _previous_switches{}; bool _previous_switches_initialized{false}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 38a30d2c53fc..f42b0a6e1e5d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2138,11 +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.f; - manual_control_setpoint.y = mavlink_manual_control.y / 1000.f; + manual_control_setpoint.pitch = mavlink_manual_control.x / 1000.f; + manual_control_setpoint.roll = 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.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; + manual_control_setpoint.yaw = 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/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp index 1f1c9e67cce4..8be68fdc3fad 100644 --- a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp +++ b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -68,10 +68,10 @@ class MavlinkStreamManualControl : public MavlinkStream mavlink_manual_control_t msg{}; msg.target = mavlink_system.sysid; - msg.x = manual_control_setpoint.x * 1000; - msg.y = manual_control_setpoint.y * 1000; - msg.z = manual_control_setpoint.z * 1000; - msg.r = manual_control_setpoint.r * 1000; + msg.x = manual_control_setpoint.pitch * 1000; + msg.y = manual_control_setpoint.roll * 1000; + msg.z = manual_control_setpoint.throttle * 1000; + msg.r = manual_control_setpoint.yaw * 1000; manual_control_switches_s manual_control_switches{}; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index c176a0266bce..fd85488737d2 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -118,8 +118,8 @@ class MulticopterAttitudeControl : public ModuleBase float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ - AlphaFilter _man_x_input_filter; - AlphaFilter _man_y_input_filter; + AlphaFilter _man_roll_input_filter; + AlphaFilter _man_pitch_input_filter; hrt_abstime _last_run{0}; hrt_abstime _last_attitude_setpoint{0}; 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 169ec718af34..ad8f22d5311b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -116,11 +116,11 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, if (reset_yaw_sp) { _man_yaw_sp = yaw; - } else if ((_manual_control_setpoint.z > -.9f) + } else if ((_manual_control_setpoint.throttle > -.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; + attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate; _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); } @@ -128,21 +128,18 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, * Input mapping for roll & pitch setpoints * ---------------------------------------- * We control the following 2 angles: - * - tilt angle, given by sqrt(x*x + y*y) + * - tilt angle, given by sqrt(roll*roll + pitch*pitch) * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion * * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick * points to, and changes of the stick input are linear. */ - _man_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get()); - _man_y_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get()); - _man_x_input_filter.update(_manual_control_setpoint.x * _man_tilt_max); - _man_y_input_filter.update(_manual_control_setpoint.y * _man_tilt_max); - const float x = _man_x_input_filter.getState(); - const float y = _man_y_input_filter.getState(); - - // we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane - Vector2f v = Vector2f(y, -x); + _man_roll_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get()); + _man_pitch_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get()); + + // we want to fly towards the direction of (roll, pitch) + Vector2f v = Vector2f(_man_roll_input_filter.update(_manual_control_setpoint.roll * _man_tilt_max), + -_man_pitch_input_filter.update(_manual_control_setpoint.pitch * _man_tilt_max)); float v_norm = v.norm(); // the norm of v defines the tilt angle if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle @@ -200,7 +197,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((_manual_control_setpoint.z + 1.f) * .5f); + attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.throttle + 1.f) * .5f); attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); @@ -316,8 +313,8 @@ MulticopterAttitudeControl::Run() attitude_setpoint_generated = true; } else { - _man_x_input_filter.reset(0.f); - _man_y_input_filter.reset(0.f); + _man_roll_input_filter.reset(0.f); + _man_pitch_input_filter.reset(0.f); } Vector3f rates_sp = _attitude_control.update(q); diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp index 40ce8ea685a4..2a982c36c531 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp @@ -445,8 +445,8 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) if (_state != state::wait_for_disarm && _state != state::idle && (((now - _state_start_time) > 20_s) - || (fabsf(manual_control_setpoint.x) > 0.05f) - || (fabsf(manual_control_setpoint.y) > 0.05f))) { + || (fabsf(manual_control_setpoint.roll) > 0.05f) + || (fabsf(manual_control_setpoint.pitch) > 0.05f))) { _state = state::fail; _state_start_time = now; } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 64dc214ceaff..6890d1588872 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -173,12 +173,12 @@ MulticopterRateControl::Run() 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.roll, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-manual_control_setpoint.pitch, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(manual_control_setpoint.yaw, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; _rates_setpoint = man_rate_sp.emult(_acro_rate_max); - _thrust_setpoint(2) = -(manual_control_setpoint.z + 1.f) * .5f; + _thrust_setpoint(2) = -(manual_control_setpoint.throttle + 1.f) * .5f; _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; // publish rate setpoint diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index c2858c176180..a81e4ac9f6e4 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -661,10 +661,10 @@ void RCUpdate::UpdateManualControlInput(const hrt_abstime ×tamp_sample) manual_control_input.data_source = manual_control_setpoint_s::SOURCE_RC; // limit controls - manual_control_input.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f); - manual_control_input.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f); - manual_control_input.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f); - manual_control_input.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f); + manual_control_input.roll = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f); + manual_control_input.pitch = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f); + manual_control_input.yaw = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f); + manual_control_input.throttle = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f); manual_control_input.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f); manual_control_input.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f); manual_control_input.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f); diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index eb3e39c0e985..5e620948c4c6 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -136,12 +136,12 @@ RoverPositionControl::manual_control_setpoint_poll() } else { const float yaw_rate = math::radians(_param_gnd_man_y_max.get()); - _att_sp.yaw_sp_move_rate = _manual_control_setpoint.y * yaw_rate; + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.roll * yaw_rate; _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); } _att_sp.yaw_body = _manual_yaw_sp; - _att_sp.thrust_body[0] = _manual_control_setpoint.z; + _att_sp.thrust_body[0] = _manual_control_setpoint.throttle; Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -152,13 +152,13 @@ RoverPositionControl::manual_control_setpoint_poll() _attitude_sp_pub.publish(_att_sp); } else { - _act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y; - _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x; + _act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.roll; + _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.pitch; // Set heading from the manual roll input channel _act_controls.control[actuator_controls_s::INDEX_YAW] = - _manual_control_setpoint.y; // Nominally yaw: _manual_control_setpoint.r; + _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw; // Set throttle from the manual throttle channel - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.throttle; _reset_yaw_sp = true; } diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index e7ec5b94ec3c..2ea576d0d383 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -280,9 +280,9 @@ void UUVAttitudeControl::Run() // returning immediately and this loop will eat up resources. if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) { /* manual/direct control */ - constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x, - _manual_control_setpoint.r, - _manual_control_setpoint.z, 0.f, 0.f); + constrain_actuator_commands(_manual_control_setpoint.roll, -_manual_control_setpoint.pitch, + _manual_control_setpoint.yaw, + _manual_control_setpoint.throttle, 0.f, 0.f); } } From 3835c51f9be2e96d65b18eeeaf5ed6228c7c2ecb Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 24 Nov 2022 09:30:47 +0100 Subject: [PATCH 4/4] rc_update: adapt throttle trim calibration for [-1,1] This is fully backwards compatible: If the throttle trim is set to the minimum then it's the legacy calibration and gets interpreted such that there is no trim and behavior remains as before. If the trim is set to a different value than the minimum then it gets used like with all other channels which was unsupported before. --- src/modules/rc_update/params.c | 36 ++++++++++++++--------------- src/modules/rc_update/rc_update.cpp | 15 ++++++++++++ 2 files changed, 33 insertions(+), 18 deletions(-) diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c index 737d061be92c..1c78fa38e53e 100644 --- a/src/modules/rc_update/params.c +++ b/src/modules/rc_update/params.c @@ -53,7 +53,7 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); /** * RC channel 1 trim * - * Mid point value (same as min for throttle) + * Mid point value * * @min 800.0 * @max 2200.0 @@ -114,7 +114,7 @@ PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); /** * RC channel 2 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -175,7 +175,7 @@ PARAM_DEFINE_FLOAT(RC3_MIN, 1000); /** * RC channel 3 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -236,7 +236,7 @@ PARAM_DEFINE_FLOAT(RC4_MIN, 1000); /** * RC channel 4 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -297,7 +297,7 @@ PARAM_DEFINE_FLOAT(RC5_MIN, 1000); /** * RC channel 5 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -357,7 +357,7 @@ PARAM_DEFINE_FLOAT(RC6_MIN, 1000); /** * RC channel 6 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -417,7 +417,7 @@ PARAM_DEFINE_FLOAT(RC7_MIN, 1000); /** * RC channel 7 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -477,7 +477,7 @@ PARAM_DEFINE_FLOAT(RC8_MIN, 1000); /** * RC channel 8 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -537,7 +537,7 @@ PARAM_DEFINE_FLOAT(RC9_MIN, 1000); /** * RC channel 9 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -597,7 +597,7 @@ PARAM_DEFINE_FLOAT(RC10_MIN, 1000); /** * RC channel 10 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -657,7 +657,7 @@ PARAM_DEFINE_FLOAT(RC11_MIN, 1000); /** * RC channel 11 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -717,7 +717,7 @@ PARAM_DEFINE_FLOAT(RC12_MIN, 1000); /** * RC channel 12 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -777,7 +777,7 @@ PARAM_DEFINE_FLOAT(RC13_MIN, 1000); /** * RC channel 13 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -837,7 +837,7 @@ PARAM_DEFINE_FLOAT(RC14_MIN, 1000); /** * RC channel 14 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -897,7 +897,7 @@ PARAM_DEFINE_FLOAT(RC15_MIN, 1000); /** * RC channel 15 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -957,7 +957,7 @@ PARAM_DEFINE_FLOAT(RC16_MIN, 1000); /** * RC channel 16 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -1017,7 +1017,7 @@ PARAM_DEFINE_FLOAT(RC17_MIN, 1000); /** * RC channel 17 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 @@ -1077,7 +1077,7 @@ PARAM_DEFINE_FLOAT(RC18_MIN, 1000); /** * RC channel 18 trim * - * Mid point value (has to be set to the same as min for throttle channel). + * Mid point value * * @min 800.0 * @max 2200.0 diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index a81e4ac9f6e4..af4931c472ff 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -201,6 +201,21 @@ void RCUpdate::parameters_updated() } } } + + // Center throttle trim when it's set to the minimum to correct for hardcoded QGC RC calibration + // See https://github.com/mavlink/qgroundcontrol/commit/0577af2e944a0f53919aeb1367d580f744004b2c + const int8_t throttle_channel = _rc.function[rc_channels_s::FUNCTION_THROTTLE]; + + if (throttle_channel >= 0 && throttle_channel < RC_MAX_CHAN_COUNT) { + const uint16_t throttle_min = _parameters.min[throttle_channel]; + const uint16_t throttle_trim = _parameters.trim[throttle_channel]; + const uint16_t throttle_max = _parameters.max[throttle_channel]; + + if (throttle_min == throttle_trim) { + const uint16_t new_throttle_trim = (throttle_min + throttle_max) / 2; + _parameters.trim[throttle_channel] = new_throttle_trim; + } + } } void RCUpdate::update_rc_functions()