diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 4314fb82ef..cd03a04c18 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -103,7 +103,6 @@ set(msg_files logger_status.msg mag_worker_data.msg magnetometer_bias_estimate.msg - manual_control_input.msg manual_control_setpoint.msg manual_control_switches.msg mavlink_log.msg diff --git a/msg/manual_control_input.msg b/msg/manual_control_input.msg deleted file mode 100644 index 133d322b35..0000000000 --- a/msg/manual_control_input.msg +++ /dev/null @@ -1,52 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -uint64 timestamp_sample # the timestamp of the raw data (microseconds) - -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 - diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 05241083e8..bbbdd0bc66 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -1,6 +1,56 @@ uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) bool valid -px4/manual_control_input chosen_input -bool user_override +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 sticks_moving + +# TOPICS manual_control_setpoint manual_control_input diff --git a/src/examples/fixedwing_control/main.cpp b/src/examples/fixedwing_control/main.cpp index 3c34b4271a..a37b93983e 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.chosen_input.z) && - (manual_control_setpoint.chosen_input.z >= 0.6f) && - (manual_control_setpoint.chosen_input.z <= 1.0f)) { + if (PX4_ISFINITE(manual_control_setpoint.z) && + (manual_control_setpoint.z >= 0.6f) && + (manual_control_setpoint.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 aaa1d17ee2..73d39672aa 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.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; + _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; } } 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 d3346acad2..57ff120e13 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.chosen_input.x; - _actuators.control[2] = _manual_control_sp.chosen_input.r; - _actuators.control[3] = _manual_control_sp.chosen_input.z; + _actuators.control[1] = _manual_control_sp.x; + _actuators.control[2] = _manual_control_sp.r; + _actuators.control[3] = _manual_control_sp.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 995a1f6a34..e3ddd00890 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2360,7 +2360,7 @@ Commander::run() } const bool mode_switch_mapped = (_param_rc_map_fltmode.get() > 0) || (_param_rc_map_mode_sw.get() > 0); - const bool is_mavlink = manual_control_setpoint.chosen_input.data_source > manual_control_input_s::SOURCE_RC; + const bool is_mavlink = manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; if (!_armed.armed && (is_mavlink || !mode_switch_mapped) && (_internal_state.main_state_changes == 0)) { // if there's never been a mode change force position control as initial state @@ -2369,8 +2369,8 @@ Commander::run() } _status.rc_signal_lost = false; - _is_throttle_above_center = manual_control_setpoint.chosen_input.z > 0.6f; - _is_throttle_low = manual_control_setpoint.chosen_input.z < 0.1f; + _is_throttle_above_center = manual_control_setpoint.z > 0.6f; + _is_throttle_low = manual_control_setpoint.z < 0.1f; _last_valid_manual_control_setpoint = manual_control_setpoint.timestamp; } else { @@ -2400,7 +2400,7 @@ Commander::run() && _armed.armed && !_status_flags.rc_input_blocked && manual_control_setpoint.valid - && manual_control_setpoint.user_override + && manual_control_setpoint.sticks_moving && override_enabled) { const transition_result_t posctl_result = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state); diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 23f077092c..973e651aa2 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.chosen_input.y * roll_scale + roll_trim_active; + float p = manual_control_setpoint.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.chosen_input.x * pitch_scale + pitch_trim_active; + p = -manual_control_setpoint.x * pitch_scale + pitch_trim_active; int p2r = param_set(param_find("TRIM_PITCH"), &p); - p = manual_control_setpoint.chosen_input.r * yaw_scale + yaw_trim_active; + p = manual_control_setpoint.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 08116426ec..a3890968b6 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.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, + _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.chosen_input.r; // yaw [-1,1] + _positions(3) = manual_control_setpoint.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 6f8f29a5be..9580ccaed5 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.chosen_input.y * radians(_param_fw_man_r_max.get()); + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); - _att_sp.pitch_body = -_manual_control_setpoint.chosen_input.x * radians(_param_fw_man_p_max.get()) + _att_sp.pitch_body = -_manual_control_setpoint.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.chosen_input.z, 0.0f, 1.0f); + _att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.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,22 @@ 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.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); + _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); _rate_sp_pub.publish(_rates_sp); } else { /* manual/direct control */ _actuators.control[actuator_controls_s::INDEX_ROLL] = - _manual_control_setpoint.chosen_input.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); + _manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); _actuators.control[actuator_controls_s::INDEX_PITCH] = - -_manual_control_setpoint.chosen_input.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); + -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); _actuators.control[actuator_controls_s::INDEX_YAW] = - _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, + _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); } } @@ -563,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.chosen_input.r; + _actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r; } if (!PX4_ISFINITE(yaw_u)) { @@ -644,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.chosen_input.aux1; + _actuators.control[5] = _manual_control_setpoint.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.chosen_input.aux3; + _actuators.control[7] = _manual_control_setpoint.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); @@ -672,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.chosen_input.flaps) && _vcontrol_mode.flag_control_manual_enabled + if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled && fabsf(_param_fw_flaps_scl.get()) > 0.01f) { - flap_control = _manual_control_setpoint.chosen_input.flaps * _param_fw_flaps_scl.get(); + flap_control = _manual_control_setpoint.flaps * _param_fw_flaps_scl.get(); } else if (_vcontrol_mode.flag_control_auto_enabled && fabsf(_param_fw_flaps_scl.get()) > 0.01f) { @@ -706,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.chosen_input.aux2) && _vcontrol_mode.flag_control_manual_enabled + if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { - flaperon_control = 0.5f * (_manual_control_setpoint.chosen_input.aux2 + 1.0f) * _param_fw_flaperon_scl.get(); + flaperon_control = 0.5f * (_manual_control_setpoint.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 80ce363246..d58835f8b2 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.chosen_input.x) > 0.2f) - || (fabsf(manual_control_setpoint.chosen_input.y) > 0.2f))) { + || (fabsf(manual_control_setpoint.x) > 0.2f) + || (fabsf(manual_control_setpoint.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 e9de87f817..d526c606b6 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.chosen_input.x; - _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f, 1.0f); + _manual_control_setpoint_altitude = _manual_control_setpoint.x; + _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.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.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; + _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; } } @@ -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.chosen_input.y * radians(_param_fw_man_r_max.get()); + _att_sp.roll_body = _manual_control_setpoint.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.chosen_input.y * radians(_param_fw_man_r_max.get()); + _att_sp.roll_body = _manual_control_setpoint.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.chosen_input.y) < HDG_HOLD_MAN_INPUT_THRESH && - fabsf(_manual_control_setpoint.chosen_input.r) < HDG_HOLD_MAN_INPUT_THRESH) { + if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH && + fabsf(_manual_control_setpoint.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.chosen_input.y) >= HDG_HOLD_MAN_INPUT_THRESH || - fabsf(_manual_control_setpoint.chosen_input.r) >= HDG_HOLD_MAN_INPUT_THRESH) { + if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH || + fabsf(_manual_control_setpoint.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.chosen_input.y * radians(_param_fw_man_r_max.get()); + float roll_sp_new = _manual_control_setpoint.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 901414956f..16bd6903da 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.chosen_input.aux1 > 0.3f); + desired_state = (manual_control_setpoint.aux1 > 0.3f); updated = true; } diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 2616e02e10..d087c37e75 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -83,9 +83,9 @@ void ManualControl::Run() _selector.updateValidityOfChosenInput(now); for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++) { - manual_control_input_s manual_control_input; + manual_control_setpoint_s manual_control_input; - if (_manual_control_input_subs[i].update(&manual_control_input)) { + if (_manual_control_setpoint_subs[i].update(&manual_control_input)) { _selector.updateWithNewInputSample(now, manual_control_input, i); } } @@ -96,25 +96,25 @@ void ManualControl::Run() if (_selector.setpoint().valid) { _published_invalid_once = false; - processStickArming(_selector.setpoint().chosen_input); + processStickArming(_selector.setpoint()); // User override by stick const float dt_s = (now - _last_time) / 1e6f; const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); - const bool rpy_moved = (fabsf(_x_diff.update(_selector.setpoint().chosen_input.x, dt_s)) > minimum_stick_change) - || (fabsf(_y_diff.update(_selector.setpoint().chosen_input.y, dt_s)) > minimum_stick_change) - || (fabsf(_r_diff.update(_selector.setpoint().chosen_input.r, dt_s)) > minimum_stick_change); + 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_moved = - (fabsf(_z_diff.update(_selector.setpoint().chosen_input.z, dt_s)) * 2.f) > minimum_stick_change; + const bool throttle_moving = + (fabsf(_z_diff.update(_selector.setpoint().z, dt_s)) * 2.f) > minimum_stick_change; - _selector.setpoint().user_override = rpy_moved || throttle_moved; + _selector.setpoint().sticks_moving = rpy_moving || throttle_moving; if (switches_updated) { // Only use switches if current source is RC as well. - if (_selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC) { + if (_selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC) { if (_previous_switches_initialized) { if (switches.mode_slot != _previous_switches.mode_slot) { evaluateModeSlot(switches.mode_slot); @@ -223,11 +223,11 @@ void ManualControl::Run() if (instance != _previous_manual_control_input_instance) { if ((0 <= _previous_manual_control_input_instance) && (_previous_manual_control_input_instance < MAX_MANUAL_INPUT_COUNT)) { - _manual_control_input_subs[_previous_manual_control_input_instance].unregisterCallback(); + _manual_control_setpoint_subs[_previous_manual_control_input_instance].unregisterCallback(); } if ((0 <= instance) && (instance < MAX_MANUAL_INPUT_COUNT)) { - _manual_control_input_subs[instance].registerCallback(); + _manual_control_setpoint_subs[instance].registerCallback(); } _previous_manual_control_input_instance = instance; @@ -258,7 +258,7 @@ void ManualControl::Run() perf_end(_loop_perf); } -void ManualControl::processStickArming(const manual_control_input_s &input) +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); diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 7df2cacf16..d74a4f54fe 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include #include @@ -78,7 +77,7 @@ private: static constexpr int MAX_MANUAL_INPUT_COUNT = 3; void Run() override; - void processStickArming(const manual_control_input_s &input); + void processStickArming(const manual_control_setpoint_s &input); void evaluateModeSlot(uint8_t mode_slot); void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0); @@ -90,7 +89,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; int _previous_manual_control_input_instance{-1}; - uORB::SubscriptionCallbackWorkItem _manual_control_input_subs[MAX_MANUAL_INPUT_COUNT] { + uORB::SubscriptionCallbackWorkItem _manual_control_setpoint_subs[MAX_MANUAL_INPUT_COUNT] { {this, ORB_ID(manual_control_input), 0}, {this, ORB_ID(manual_control_input), 1}, {this, ORB_ID(manual_control_input), 2}, diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp index e736fc13d9..d3a6ca9aa7 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -35,43 +35,43 @@ void ManualControlSelector::updateValidityOfChosenInput(uint64_t now) { - if (!isInputValid(_setpoint.chosen_input, now)) { + if (!isInputValid(_setpoint, now)) { _setpoint.valid = false; _instance = -1; } } -void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_control_input_s &input, int instance) +void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_control_setpoint_s &input, int instance) { // First check if the chosen input got invalid, so it can get replaced updateValidityOfChosenInput(now); - const bool update_existing_input = _setpoint.valid && input.data_source == _setpoint.chosen_input.data_source; + const bool update_existing_input = _setpoint.valid && input.data_source == _setpoint.data_source; const bool start_using_new_input = !_setpoint.valid; // Switch to new input if it's valid and we don't already have a valid one if (isInputValid(input, now) && (update_existing_input || start_using_new_input)) { - _setpoint.chosen_input = input; + _setpoint = input; _setpoint.valid = true; _instance = instance; } } -bool ManualControlSelector::isInputValid(const manual_control_input_s &input, uint64_t now) const +bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input, uint64_t now) const { // Check for timeout const bool sample_from_the_past = now >= input.timestamp_sample; const bool sample_newer_than_timeout = now - input.timestamp_sample < _timeout; // Check if source matches the configuration - const bool source_rc_matched = (_rc_in_mode == 0) && (input.data_source == manual_control_input_s::SOURCE_RC); + const bool source_rc_matched = (_rc_in_mode == 0) && (input.data_source == manual_control_setpoint_s::SOURCE_RC); const bool source_mavlink_matched = (_rc_in_mode == 1) && - (input.data_source == manual_control_input_s::SOURCE_MAVLINK_0 - || input.data_source == manual_control_input_s::SOURCE_MAVLINK_1 - || input.data_source == manual_control_input_s::SOURCE_MAVLINK_2 - || input.data_source == manual_control_input_s::SOURCE_MAVLINK_3 - || input.data_source == manual_control_input_s::SOURCE_MAVLINK_4 - || input.data_source == manual_control_input_s::SOURCE_MAVLINK_5); + (input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0 + || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_1 + || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_2 + || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3 + || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_4 + || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_5); const bool source_any_matched = (_rc_in_mode == 3); return sample_from_the_past && sample_newer_than_timeout diff --git a/src/modules/manual_control/ManualControlSelector.hpp b/src/modules/manual_control/ManualControlSelector.hpp index 827407f35e..0a11e0b60b 100644 --- a/src/modules/manual_control/ManualControlSelector.hpp +++ b/src/modules/manual_control/ManualControlSelector.hpp @@ -34,7 +34,6 @@ #pragma once #include -#include #include class ManualControlSelector @@ -43,12 +42,12 @@ public: void setRcInMode(int32_t rc_in_mode) { _rc_in_mode = rc_in_mode; } void setTimeout(uint64_t timeout) { _timeout = timeout; } void updateValidityOfChosenInput(uint64_t now); - void updateWithNewInputSample(uint64_t now, const manual_control_input_s &input, int instance); + void updateWithNewInputSample(uint64_t now, const manual_control_setpoint_s &input, int instance); manual_control_setpoint_s &setpoint(); int instance() const { return _instance; }; private: - bool isInputValid(const manual_control_input_s &input, uint64_t now) const; + bool isInputValid(const manual_control_setpoint_s &input, uint64_t now) const; manual_control_setpoint_s _setpoint{}; uint64_t _timeout{0}; diff --git a/src/modules/manual_control/ManualControlSelectorTest.cpp b/src/modules/manual_control/ManualControlSelectorTest.cpp index 8aa0f05c4f..01a4e88647 100644 --- a/src/modules/manual_control/ManualControlSelectorTest.cpp +++ b/src/modules/manual_control/ManualControlSelectorTest.cpp @@ -48,16 +48,16 @@ TEST(ManualControlSelector, RcInputContinuous) uint64_t timestamp = some_time; // Now provide input with the correct source. - manual_control_input_s input {}; - input.data_source = manual_control_input_s::SOURCE_RC; + manual_control_setpoint_s input {}; + input.data_source = manual_control_setpoint_s::SOURCE_RC; input.timestamp_sample = timestamp; for (int i = 0; i < 5; i++) { selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); - EXPECT_EQ(selector.setpoint().chosen_input.timestamp_sample, timestamp); + EXPECT_EQ(selector.setpoint().timestamp_sample, timestamp); EXPECT_EQ(selector.instance(), 1); - EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); timestamp += 100_ms; input.timestamp_sample = timestamp; } @@ -71,8 +71,8 @@ TEST(ManualControlSelector, RcInputOnly) uint64_t timestamp = some_time; - manual_control_input_s input {}; - input.data_source = manual_control_input_s::SOURCE_MAVLINK_0; + manual_control_setpoint_s input {}; + input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 0); @@ -81,12 +81,12 @@ TEST(ManualControlSelector, RcInputOnly) timestamp += 100_ms; // Now provide input with the correct source. - input.data_source = manual_control_input_s::SOURCE_RC; + input.data_source = manual_control_setpoint_s::SOURCE_RC; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); - EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); EXPECT_EQ(selector.instance(), 1); } @@ -98,8 +98,8 @@ TEST(ManualControlSelector, MavlinkInputOnly) uint64_t timestamp = some_time; - manual_control_input_s input {}; - input.data_source = manual_control_input_s::SOURCE_RC; + manual_control_setpoint_s input {}; + input.data_source = manual_control_setpoint_s::SOURCE_RC; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 0); @@ -108,23 +108,23 @@ TEST(ManualControlSelector, MavlinkInputOnly) timestamp += 100_ms; // Now provide input with the correct source. - input.data_source = manual_control_input_s::SOURCE_MAVLINK_3; + input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_3; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); - EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_3); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3); EXPECT_EQ(selector.instance(), 1); timestamp += 100_ms; // But only the first MAVLink source wins, others are too late. - input.data_source = manual_control_input_s::SOURCE_MAVLINK_4; + input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_4; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); - EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_3); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3); EXPECT_EQ(selector.instance(), 1); } @@ -136,35 +136,35 @@ TEST(ManualControlSelector, AutoInput) uint64_t timestamp = some_time; - manual_control_input_s input {}; - input.data_source = manual_control_input_s::SOURCE_RC; + manual_control_setpoint_s input {}; + input.data_source = manual_control_setpoint_s::SOURCE_RC; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 0); EXPECT_TRUE(selector.setpoint().valid); - EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); EXPECT_EQ(selector.instance(), 0); timestamp += 100_ms; // Now provide input from MAVLink as well which should get ignored. - input.data_source = manual_control_input_s::SOURCE_MAVLINK_0; + input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); - EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); EXPECT_EQ(selector.instance(), 0); timestamp += 500_ms; // Now we'll let RC time out, so it should switch to MAVLINK. - input.data_source = manual_control_input_s::SOURCE_MAVLINK_0; + input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 1); EXPECT_TRUE(selector.setpoint().valid); - EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_MAVLINK_0); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0); EXPECT_EQ(selector.instance(), 1); } @@ -176,13 +176,13 @@ TEST(ManualControlSelector, RcTimeout) uint64_t timestamp = some_time; - manual_control_input_s input {}; - input.data_source = manual_control_input_s::SOURCE_RC; + manual_control_setpoint_s input {}; + input.data_source = manual_control_setpoint_s::SOURCE_RC; input.timestamp_sample = timestamp; selector.updateWithNewInputSample(timestamp, input, 0); EXPECT_TRUE(selector.setpoint().valid); - EXPECT_TRUE(selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC); + EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC); EXPECT_EQ(selector.instance(), 0); timestamp += 600_ms; @@ -202,8 +202,8 @@ TEST(ManualControlSelector, RcOutdated) uint64_t timestamp = some_time; - manual_control_input_s input {}; - input.data_source = manual_control_input_s::SOURCE_RC; + manual_control_setpoint_s input {}; + input.data_source = manual_control_setpoint_s::SOURCE_RC; input.timestamp_sample = timestamp - 600_ms; // First sample is already outdated selector.updateWithNewInputSample(timestamp, input, 0); diff --git a/src/modules/manual_control/MovingDiffTest.cpp b/src/modules/manual_control/MovingDiffTest.cpp new file mode 100644 index 0000000000..3f70be76fa --- /dev/null +++ b/src/modules/manual_control/MovingDiffTest.cpp @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MovingDiff.hpp" +#include + +TEST(MovingDiffTest, RcInputContinuous) +{ + MovingDiff _diff; + EXPECT_FLOAT_EQ(_diff.update(0.0f, 0.0f), 0.f); // 0,0,0 + EXPECT_FLOAT_EQ(_diff.update(1.0f, 1.0f), 0.f); // 1*,0,0 + EXPECT_FLOAT_EQ(_diff.update(0.0f, 1.0f), 0.f); // 1,-1*,0 + EXPECT_FLOAT_EQ(_diff.update(0.0f, 1.0f), 0.f); // 0,-1,0* + EXPECT_FLOAT_EQ(_diff.update(0.0f, 1.0f), 0.f); // 0*,-1,0 + EXPECT_FLOAT_EQ(_diff.update(1.0f, 1.0f), 0.f); // 0,1*,0 + EXPECT_FLOAT_EQ(_diff.update(0.0f, 1.0f), 0.f); // 0,1,-1* + EXPECT_FLOAT_EQ(_diff.update(2.0f, 1.0f), 1.f); // 2*,1,-1 + EXPECT_FLOAT_EQ(_diff.update(4.0f, 1.0f), 2.f); // 2,2*,-1 +} diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5f101591c6..33d2ad077e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2094,12 +2094,12 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) return; } - manual_control_input_s manual{}; + 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_input_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); + 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); } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 85e30a5942..af9bd267be 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -80,7 +80,7 @@ #include #include #include -#include +#include #include #include #include @@ -326,7 +326,7 @@ private: uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; - uORB::PublicationMulti _manual_control_input_pub{ORB_ID(manual_control_input)}; + uORB::PublicationMulti _manual_control_input_pub{ORB_ID(manual_control_input)}; uORB::PublicationMulti _ping_pub{ORB_ID(ping)}; uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status)}; diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp index b7bc8832c6..1f1c9e67cc 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.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; + 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; 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 ae23a653fe..f24d7df603 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.chosen_input.z, 0.0f, 1.0f) > 0.05f + } else if (math::constrain(_manual_control_setpoint.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.chosen_input.r * yaw_rate; + attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.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.chosen_input.x * _man_tilt_max); - _man_y_input_filter.update(_manual_control_setpoint.chosen_input.y * _man_tilt_max); + _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(); @@ -211,7 +211,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.chosen_input.z, 0.0f, + attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f)); attitude_setpoint.timestamp = hrt_absolute_time(); 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 b1360ec983..bf71b0b1e6 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.chosen_input.x) > 0.05f) - || (fabsf(manual_control_setpoint.chosen_input.y) > 0.05f))) { + || (fabsf(manual_control_setpoint.x) > 0.05f) + || (fabsf(manual_control_setpoint.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 13dba90396..bcd246e3ef 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -175,12 +175,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.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())}; + 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())}; _rates_sp = man_rate_sp.emult(_acro_rate_max); - _thrust_sp = math::constrain(manual_control_setpoint.chosen_input.z, 0.0f, 1.0f); + _thrust_sp = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f); // publish rate setpoint vehicle_rates_setpoint_s v_rates_sp{}; diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 1ecaeff77c..1c40495f0c 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -661,9 +661,9 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample) void RCUpdate::UpdateManualControlInput(const hrt_abstime ×tamp_sample) { - manual_control_input_s manual_control_input{}; + manual_control_setpoint_s manual_control_input{}; manual_control_input.timestamp_sample = timestamp_sample; - manual_control_input.data_source = manual_control_input_s::SOURCE_RC; + 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); diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 95426d705c..100dfd549a 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include #include #include @@ -166,7 +166,7 @@ private: uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)}; uORB::Publication _rc_channels_pub{ORB_ID(rc_channels)}; - uORB::PublicationMulti _manual_control_input_pub{ORB_ID(manual_control_input)}; + uORB::PublicationMulti _manual_control_input_pub{ORB_ID(manual_control_input)}; uORB::Publication _manual_control_switches_pub{ORB_ID(manual_control_switches)}; uORB::Publication _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index ff60f54265..f45ac9e189 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.chosen_input.y * yaw_rate; + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.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.chosen_input.z; + _att_sp.thrust_body[0] = _manual_control_setpoint.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.chosen_input.y; // Nominally yaw: _manual_control_setpoint.r; + _manual_control_setpoint.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.chosen_input.z; + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.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 e5a208cb19..719f4c59d6 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.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); + constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x, + _manual_control_setpoint.r, + _manual_control_setpoint.z, 0.f, 0.f); } } diff --git a/src/modules/vmount/input_rc.cpp b/src/modules/vmount/input_rc.cpp index c4c4765ad5..de5a5d8633 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.chosen_input.aux1; + return manual_control_setpoint.aux1; case 2: - return manual_control_setpoint.chosen_input.aux2; + return manual_control_setpoint.aux2; case 3: - return manual_control_setpoint.chosen_input.aux3; + return manual_control_setpoint.aux3; case 4: - return manual_control_setpoint.chosen_input.aux4; + return manual_control_setpoint.aux4; case 5: - return manual_control_setpoint.chosen_input.aux5; + return manual_control_setpoint.aux5; case 6: - return manual_control_setpoint.chosen_input.aux6; + return manual_control_setpoint.aux6; default: return 0.0f;