From 331cb21deee632d836e09e34b8d884ea62ea56d1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 27 Jun 2022 18:16:05 +0200 Subject: [PATCH] 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 | 36 +++++++------------ .../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 | 25 ++++++------- .../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, 118 insertions(+), 133 deletions(-) diff --git a/msg/ManualControlSetpoint.msg b/msg/ManualControlSetpoint.msg index a3162f4f07..486aba8edd 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 0d31483585..e5c2832392 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 32a8477e88..38f7d096a9 100644 --- a/src/lib/mixer_module/functions/FunctionManualRC.hpp +++ b/src/lib/mixer_module/functions/FunctionManualRC.hpp @@ -57,10 +57,10 @@ public: 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 2256d12b4f..0a4b8bdab3 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 fccb7e88a3..1e012bafe0 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 973e651aa2..ff8ba001e2 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 f2c3b8496a..af2d28b41b 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 c1e1f069d5..214bc534f4 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 55ef99fa2d..714358aa4f 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 cf4e8fc1e2..c3b42c258e 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 738fd92ea5..d28166112e 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -117,10 +117,10 @@ private: 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 38a30d2c53..f42b0a6e1e 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 1f1c9e67cc..8be68fdc3f 100644 --- a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp +++ b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -68,10 +68,10 @@ private: 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 c176a0266b..fd85488737 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 @@ private: 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 169ec718af..ad8f22d531 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(); + _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 (x, y), so we use a perpendicular axis angle vector in the XY-plane - Vector2f v = Vector2f(y, -x); + // 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 40ce8ea685..2a982c36c5 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 64dc214cea..6890d15888 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 c2858c1761..a81e4ac9f6 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 eb3e39c0e9..5e620948c4 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 e7ec5b94ec..2ea576d0d3 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); } }