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); } }