diff --git a/msg/manual_control_input.msg b/msg/manual_control_input.msg index b6d3d586ad..133d322b35 100644 --- a/msg/manual_control_input.msg +++ b/msg/manual_control_input.msg @@ -43,10 +43,10 @@ float32 r # yaw stick/twist position, -1..1 float32 flaps # flap position -float32 aux1 # default function: camera yaw / azimuth -float32 aux2 # default function: camera pitch / tilt -float32 aux3 # default function: camera trigger -float32 aux4 # default function: camera roll -float32 aux5 # default function: payload drop +float32 aux1 +float32 aux2 +float32 aux3 +float32 aux4 +float32 aux5 float32 aux6 diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 976d1cf60c..419734c90e 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -1,57 +1,9 @@ uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +px4/manual_control_input chosen_input bool valid -uint8 SOURCE_UNKNOWN = 0 -uint8 SOURCE_RC = 1 # radio control (input_rc) -uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 -uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1 -uint8 SOURCE_MAVLINK_2 = 4 # mavlink instance 2 -uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 3 -uint8 SOURCE_MAVLINK_4 = 6 # mavlink instance 4 -uint8 SOURCE_MAVLINK_5 = 7 # mavlink instance 5 - -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 - -float32 flaps # flap position - -float32 aux1 -float32 aux2 -float32 aux3 -float32 aux4 -float32 aux5 -float32 aux6 - bool arm_gesture bool disarm_gesture diff --git a/src/examples/fixedwing_control/main.cpp b/src/examples/fixedwing_control/main.cpp index a37b93983e..3c34b4271a 100644 --- a/src/examples/fixedwing_control/main.cpp +++ b/src/examples/fixedwing_control/main.cpp @@ -388,9 +388,9 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ - if (PX4_ISFINITE(manual_control_setpoint.z) && - (manual_control_setpoint.z >= 0.6f) && - (manual_control_setpoint.z <= 1.0f)) { + if (PX4_ISFINITE(manual_control_setpoint.chosen_input.z) && + (manual_control_setpoint.chosen_input.z >= 0.6f) && + (manual_control_setpoint.chosen_input.z <= 1.0f)) { } /* get the system status and the flight mode we're in */ diff --git a/src/lib/mixer_module/functions.cpp b/src/lib/mixer_module/functions.cpp index 73d39672aa..aaa1d17ee2 100644 --- a/src/lib/mixer_module/functions.cpp +++ b/src/lib/mixer_module/functions.cpp @@ -119,17 +119,17 @@ void FunctionManualRC::update() 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[4] = manual_control_setpoint.flaps; - _data[5] = manual_control_setpoint.aux1; - _data[6] = manual_control_setpoint.aux2; - _data[7] = manual_control_setpoint.aux3; - _data[8] = manual_control_setpoint.aux4; - _data[9] = manual_control_setpoint.aux5; - _data[10] = manual_control_setpoint.aux6; + _data[0] = manual_control_setpoint.chosen_input.y; // roll + _data[1] = manual_control_setpoint.chosen_input.x; // pitch + _data[2] = manual_control_setpoint.chosen_input.z * 2.f - 1.f; // throttle + _data[3] = manual_control_setpoint.chosen_input.r; // yaw + _data[4] = manual_control_setpoint.chosen_input.flaps; + _data[5] = manual_control_setpoint.chosen_input.aux1; + _data[6] = manual_control_setpoint.chosen_input.aux2; + _data[7] = manual_control_setpoint.chosen_input.aux3; + _data[8] = manual_control_setpoint.chosen_input.aux4; + _data[9] = manual_control_setpoint.chosen_input.aux5; + _data[10] = manual_control_setpoint.chosen_input.aux6; } } 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 57ff120e13..d3346acad2 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 { _actuators.control[0] = 0.0f; - _actuators.control[1] = _manual_control_sp.x; - _actuators.control[2] = _manual_control_sp.r; - _actuators.control[3] = _manual_control_sp.z; + _actuators.control[1] = _manual_control_sp.chosen_input.x; + _actuators.control[2] = _manual_control_sp.chosen_input.r; + _actuators.control[3] = _manual_control_sp.chosen_input.z; } // note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run() diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1fb0f6d0e9..635fc013bd 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2420,8 +2420,8 @@ Commander::run() } _status.rc_signal_lost = false; - _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.chosen_input.z > 0.6f; + _is_throttle_low = manual_control_setpoint.chosen_input.z < 0.1f; _last_valid_manual_control_setpoint = manual_control_setpoint.timestamp; } else { diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 973e651aa2..23f077092c 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.chosen_input.y * 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.chosen_input.x * 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.chosen_input.r * 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 a3890968b6..08116426ec 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -51,11 +51,11 @@ bool Sticks::checkAndSetStickInputs() 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, + _positions(0) = manual_control_setpoint.chosen_input.x; // NED x, pitch [-1,1] + _positions(1) = manual_control_setpoint.chosen_input.y; // NED y, roll [-1,1] + _positions(2) = -(math::constrain(manual_control_setpoint.chosen_input.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(3) = manual_control_setpoint.chosen_input.r; // yaw [-1,1] // 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 a7c17b5e2b..6f8f29a5be 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -149,15 +149,15 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) 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.chosen_input.y * 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.chosen_input.x * 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] = math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f, 1.0f); Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -171,22 +171,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.chosen_input.y * radians(_param_fw_acro_x_max.get()); + _rates_sp.pitch = -_manual_control_setpoint.chosen_input.x * radians(_param_fw_acro_y_max.get()); + _rates_sp.yaw = _manual_control_setpoint.chosen_input.r * radians(_param_fw_acro_z_max.get()); + _rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f, 1.0f); _rate_sp_pub.publish(_rates_sp); } else { /* manual/direct control */ _actuators.control[actuator_controls_s::INDEX_ROLL] = - _manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); + _manual_control_setpoint.chosen_input.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); _actuators.control[actuator_controls_s::INDEX_PITCH] = - -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); + -_manual_control_setpoint.chosen_input.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); _actuators.control[actuator_controls_s::INDEX_YAW] = - _manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + _manual_control_setpoint.chosen_input.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f, + 1.0f); } } } @@ -562,7 +563,7 @@ void FixedwingAttitudeControl::Run() /* add in manual rudder control in manual modes */ if (_vcontrol_mode.flag_control_manual_enabled) { - _actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r; + _actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.chosen_input.r; } if (!PX4_ISFINITE(yaw_u)) { @@ -643,10 +644,10 @@ void FixedwingAttitudeControl::Run() * constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; - _actuators.control[5] = _manual_control_setpoint.aux1; + _actuators.control[5] = _manual_control_setpoint.chosen_input.aux1; _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied; // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future - _actuators.control[7] = _manual_control_setpoint.aux3; + _actuators.control[7] = _manual_control_setpoint.chosen_input.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); @@ -671,9 +672,9 @@ void FixedwingAttitudeControl::control_flaps(const float dt) float flap_control = 0.0f; /* map flaps by default to manual if valid */ - if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled + if (PX4_ISFINITE(_manual_control_setpoint.chosen_input.flaps) && _vcontrol_mode.flag_control_manual_enabled && fabsf(_param_fw_flaps_scl.get()) > 0.01f) { - flap_control = _manual_control_setpoint.flaps * _param_fw_flaps_scl.get(); + flap_control = _manual_control_setpoint.chosen_input.flaps * _param_fw_flaps_scl.get(); } else if (_vcontrol_mode.flag_control_auto_enabled && fabsf(_param_fw_flaps_scl.get()) > 0.01f) { @@ -705,10 +706,10 @@ void FixedwingAttitudeControl::control_flaps(const float dt) float flaperon_control = 0.0f; /* map flaperons by default to manual if valid */ - if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled + if (PX4_ISFINITE(_manual_control_setpoint.chosen_input.aux2) && _vcontrol_mode.flag_control_manual_enabled && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { - flaperon_control = 0.5f * (_manual_control_setpoint.aux2 + 1.0f) * _param_fw_flaperon_scl.get(); + flaperon_control = 0.5f * (_manual_control_setpoint.chosen_input.aux2 + 1.0f) * _param_fw_flaperon_scl.get(); } else if (_vcontrol_mode.flag_control_auto_enabled && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index d58835f8b2..80ce363246 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -423,8 +423,8 @@ void FwAutotuneAttitudeControl::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.2f) - || (fabsf(manual_control_setpoint.y) > 0.2f))) { + || (fabsf(manual_control_setpoint.chosen_input.x) > 0.2f) + || (fabsf(manual_control_setpoint.chosen_input.y) > 0.2f))) { _state = state::fail; _state_start_time = now; } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index d526c606b6..e9de87f817 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -280,15 +280,15 @@ FixedwingPositionControl::manual_control_setpoint_poll() { _manual_control_setpoint_sub.update(&_manual_control_setpoint); - _manual_control_setpoint_altitude = _manual_control_setpoint.x; - _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + _manual_control_setpoint_altitude = _manual_control_setpoint.chosen_input.x; + _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f, 1.0f); if (_param_fw_posctl_inv_st.get()) { /* 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_altitude = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f); - _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f; + _manual_control_setpoint_altitude = -(math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f, 1.0f) * 2.f - 1.f); + _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.chosen_input.x, -1.0f, 1.0f) / 2.f + 0.5f; } } @@ -680,7 +680,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool /* 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_man_r_max.get()); + _att_sp.roll_body = _manual_control_setpoint.chosen_input.y * radians(_param_fw_man_r_max.get()); _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } @@ -1690,7 +1690,7 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const tecs_status_s::TECS_MODE_NORMAL, height_rate_sp); - _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); + _att_sp.roll_body = _manual_control_setpoint.chosen_input.y * radians(_param_fw_man_r_max.get()); _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw /* Copy thrust and pitch values from tecs */ @@ -1750,8 +1750,8 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const height_rate_sp); /* 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.chosen_input.y) < HDG_HOLD_MAN_INPUT_THRESH && + fabsf(_manual_control_setpoint.chosen_input.r) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ if (fabsf(_yawrate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { @@ -1801,14 +1801,14 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const } } - 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.chosen_input.y) >= HDG_HOLD_MAN_INPUT_THRESH || + fabsf(_manual_control_setpoint.chosen_input.r) >= 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_man_r_max.get()); + float roll_sp_new = _manual_control_setpoint.chosen_input.y * radians(_param_fw_man_r_max.get()); const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get()); if (dt > 0.f && roll_rate_slew_rad > 0.f) { diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 16bd6903da..901414956f 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -1085,7 +1085,7 @@ bool Logger::start_stop_logging() if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - desired_state = (manual_control_setpoint.aux1 > 0.3f); + desired_state = (manual_control_setpoint.chosen_input.aux1 > 0.3f); updated = true; } diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 2679e55799..0c58a5c564 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -116,9 +116,12 @@ void ManualControl::Run() _published_invalid_once = false; // user arm/disarm gesture - const bool right_stick_centered = (fabsf(_selector.setpoint().x) < 0.1f) && (fabsf(_selector.setpoint().y) < 0.1f); - const bool stick_lower_left = (_selector.setpoint().z < 0.1f) && (_selector.setpoint().r < -0.9f); - const bool stick_lower_right = (_selector.setpoint().z < 0.1f) && (_selector.setpoint().r > 0.9f); + const bool right_stick_centered = (fabsf(_selector.setpoint().chosen_input.x) < 0.1f) + && (fabsf(_selector.setpoint().chosen_input.y) < 0.1f); + const bool stick_lower_left = (_selector.setpoint().chosen_input.z < 0.1f) + && (_selector.setpoint().chosen_input.r < -0.9f); + const bool stick_lower_right = (_selector.setpoint().chosen_input.z < 0.1f) + && (_selector.setpoint().chosen_input.r > 0.9f); _stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, _selector.setpoint().timestamp); _stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, _selector.setpoint().timestamp); @@ -153,14 +156,14 @@ void ManualControl::Run() _selector.setpoint().user_override = rpy_moved || throttle_moved; - _x_diff.update(_selector.setpoint().x, dt_s); - _y_diff.update(_selector.setpoint().y, dt_s); - _z_diff.update(_selector.setpoint().z, dt_s); - _r_diff.update(_selector.setpoint().r, dt_s); + _x_diff.update(_selector.setpoint().chosen_input.x, dt_s); + _y_diff.update(_selector.setpoint().chosen_input.y, dt_s); + _z_diff.update(_selector.setpoint().chosen_input.z, dt_s); + _r_diff.update(_selector.setpoint().chosen_input.r, dt_s); if (switches_updated) { // Only use switches if current source is RC as well. - if (_selector.setpoint().data_source == manual_control_input_s::SOURCE_RC) { + if (_selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC) { if (_previous_switches_initialized) { if (switches.mode_slot != _previous_switches.mode_slot) { evaluate_mode_slot(switches.mode_slot); diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp index 38d8e2b157..ad22497f1c 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -39,7 +39,7 @@ namespace manual_control void ManualControlSelector::update_time_only(uint64_t now) { - if (_setpoint.timestamp_sample + _timeout < now) { + if (_setpoint.chosen_input.timestamp_sample + _timeout < now) { _setpoint.valid = false; _instance = -1; } @@ -66,7 +66,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu || input.data_source == manual_control_input_s::SOURCE_MAVLINK_5)) { // We only stick to the first discovered mavlink channel. - if (_setpoint.data_source == input.data_source || !_setpoint.valid) { + if (_setpoint.chosen_input.data_source == input.data_source || !_setpoint.valid) { _setpoint = setpoint_from_input(input); _setpoint.valid = true; _instance = instance; @@ -77,7 +77,7 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu } else if (_rc_in_mode == 3) { // We only stick to the first discovered mavlink channel. - if (_setpoint.data_source == input.data_source || !_setpoint.valid) { + if (_setpoint.chosen_input.data_source == input.data_source || !_setpoint.valid) { _setpoint = setpoint_from_input(input); _setpoint.valid = true; _instance = instance; @@ -91,24 +91,24 @@ void ManualControlSelector::update_manual_control_input(uint64_t now, const manu manual_control_setpoint_s ManualControlSelector::setpoint_from_input(const manual_control_input_s &input) { manual_control_setpoint_s setpoint; - setpoint.timestamp_sample = input.timestamp_sample; - setpoint.x = input.x; - setpoint.y = input.y; - setpoint.z = input.z; - setpoint.r = input.r; + setpoint.chosen_input.timestamp_sample = input.timestamp_sample; + setpoint.chosen_input.x = input.x; + setpoint.chosen_input.y = input.y; + setpoint.chosen_input.z = input.z; + setpoint.chosen_input.r = input.r; // FIXME: what's that? - //setpoint.vx = (input.x - _manual_control_input[i].x) * dt_inv; - //setpoint.vy = (input.y - _manual_control_input[i].y) * dt_inv; - //setpoint.vz = (input.z - _manual_control_input[i].z) * dt_inv; - //setpoint.vr = (input.r - _manual_control_input[i].r) * dt_inv; - setpoint.flaps = input.flaps; - setpoint.aux1 = input.aux1; - setpoint.aux2 = input.aux2; - setpoint.aux3 = input.aux3; - setpoint.aux4 = input.aux4; - setpoint.aux5 = input.aux5; - setpoint.aux6 = input.aux6; - setpoint.data_source = input.data_source; + //setpoint.chosen_input.vx = (input.x - _manual_control_input[i].x) * dt_inv; + //setpoint.chosen_input.vy = (input.y - _manual_control_input[i].y) * dt_inv; + //setpoint.chosen_input.vz = (input.z - _manual_control_input[i].z) * dt_inv; + //setpoint.chosen_input.vr = (input.r - _manual_control_input[i].r) * dt_inv; + setpoint.chosen_input.flaps = input.flaps; + setpoint.chosen_input.aux1 = input.aux1; + setpoint.chosen_input.aux2 = input.aux2; + setpoint.chosen_input.aux3 = input.aux3; + setpoint.chosen_input.aux4 = input.aux4; + setpoint.chosen_input.aux5 = input.aux5; + setpoint.chosen_input.aux6 = input.aux6; + setpoint.chosen_input.data_source = input.data_source; return setpoint; } diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp index 1f1c9e67cc..b7bc8832c6 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.chosen_input.x * 1000; + msg.y = manual_control_setpoint.chosen_input.y * 1000; + msg.z = manual_control_setpoint.chosen_input.z * 1000; + msg.r = manual_control_setpoint.chosen_input.r * 1000; manual_control_switches_s manual_control_switches{}; 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 7cc0103a91..ae23a653fe 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -127,11 +127,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 + } else if (math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f, 1.0f) > 0.05f || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { 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.chosen_input.r * yaw_rate; _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); } @@ -147,8 +147,8 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, */ _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); + _man_x_input_filter.update(_manual_control_setpoint.chosen_input.x * _man_tilt_max); + _man_y_input_filter.update(_manual_control_setpoint.chosen_input.y * _man_tilt_max); const float x = _man_x_input_filter.getState(); const float y = _man_y_input_filter.getState(); @@ -211,7 +211,8 @@ 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.0f, 1.0f)); + attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f, + 1.0f)); attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); 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 bf71b0b1e6..b1360ec983 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 @@ -444,8 +444,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.chosen_input.x) > 0.05f) + || (fabsf(manual_control_setpoint.chosen_input.y) > 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 0a7dd7bdff..3235689b8c 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -172,12 +172,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.chosen_input.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-manual_control_setpoint.chosen_input.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(manual_control_setpoint.chosen_input.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; _rates_sp = man_rate_sp.emult(_acro_rate_max); - _thrust_sp = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f); + _thrust_sp = math::constrain(manual_control_setpoint.chosen_input.z, 0.0f, 1.0f); // publish rate setpoint vehicle_rates_setpoint_s v_rates_sp{}; diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index f45ac9e189..ff60f54265 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.chosen_input.y * 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.chosen_input.z; Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -156,9 +156,9 @@ RoverPositionControl::manual_control_setpoint_poll() _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x; // 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.chosen_input.y; // Nominally yaw: _manual_control_setpoint.r; // 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.chosen_input.z; _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 719f4c59d6..e5a208cb19 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.chosen_input.y, -_manual_control_setpoint.chosen_input.x, + _manual_control_setpoint.chosen_input.r, + _manual_control_setpoint.chosen_input.z, 0.f, 0.f); } } diff --git a/src/modules/vmount/input_rc.cpp b/src/modules/vmount/input_rc.cpp index de5a5d8633..c4c4765ad5 100644 --- a/src/modules/vmount/input_rc.cpp +++ b/src/modules/vmount/input_rc.cpp @@ -155,22 +155,22 @@ float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_se switch (_aux_channels[channel_idx]) { case 1: - return manual_control_setpoint.aux1; + return manual_control_setpoint.chosen_input.aux1; case 2: - return manual_control_setpoint.aux2; + return manual_control_setpoint.chosen_input.aux2; case 3: - return manual_control_setpoint.aux3; + return manual_control_setpoint.chosen_input.aux3; case 4: - return manual_control_setpoint.aux4; + return manual_control_setpoint.chosen_input.aux4; case 5: - return manual_control_setpoint.aux5; + return manual_control_setpoint.chosen_input.aux5; case 6: - return manual_control_setpoint.aux6; + return manual_control_setpoint.chosen_input.aux6; default: return 0.0f;