diff --git a/msg/ManualControlSetpoint.msg b/msg/ManualControlSetpoint.msg index bbbdd0bc6660..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 0..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 - -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 9ec2db2b6d9c..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 * 2.f - 1.f; // 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.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..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_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.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() @@ -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/commander/Commander.cpp b/src/modules/commander/Commander.cpp index cf023dd3a25d..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.6f); - _is_throttle_low = (manual_control_setpoint.z < 0.1f); + _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 f43eff4fba60..af2d28b41b7e 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -51,11 +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) = -(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(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 36194a5f24df..214bc534f4e0 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -141,18 +141,20 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) if (!_vcontrol_mode.flag_control_climb_rate_enabled) { + 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())); _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); @@ -168,24 +170,23 @@ 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.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + _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); } 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(); - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, - 1.0f); - _landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.r; + _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.yaw; } } } @@ -550,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())); } @@ -592,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 1e7ec5fb6867..714358aa4f8e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -316,21 +316,21 @@ 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_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 = -(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.throttle; + _manual_control_setpoint_for_airspeed = _manual_control_setpoint.pitch; } // 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; @@ -829,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 } @@ -1435,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 @@ -1803,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 @@ -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; } @@ -1928,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); @@ -1958,13 +1949,13 @@ 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; } /* 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) { @@ -2037,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) { @@ -2659,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/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..c3b42c258e2c 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(_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. @@ -315,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); @@ -335,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.1f) && (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); @@ -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.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 af3fae916359..f42b0a6e1e5d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2129,22 +2129,23 @@ 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.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.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); } void 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 b4d09217b010..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 (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f - || _param_mc_airmode.get() == 2) { + } 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(math::constrain(_manual_control_setpoint.z, 0.f, 1.f)); + 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 716ac105ed93..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) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f); + _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/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 c2858c176180..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() @@ -661,10 +676,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); } }