dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

msg: re-use manual_control_input in setpoint 

This way we avoid duplication between manual_control_input and
manual_control_setpoint.
This commit is contained in:
Julian Oes 2021-06-10 10:56:27 +02:00 committed by Matthias Grob
parent baf81abbab
commit 6a6b8d49fc
21 changed files with 125 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
msg/manual_control_input.msg
View File

@ -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

50
msg/manual_control_setpoint.msg
View File

@ -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

6
src/examples/fixedwing_control/main.cpp
View File

@ -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 */

22
src/lib/mixer_module/functions.cpp
View File

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

6
src/modules/airship_att_control/airship_att_control_main.cpp
View File

@ -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()

4
src/modules/commander/Commander.cpp
View File

@ -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 {

6
src/modules/commander/rc_calibration.cpp
View File

@ -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) {

8
src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
View File

@ -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());

37
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -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) {

4
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
View File

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

22
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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) {

2
src/modules/logger/logger.cpp
View File

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

19
src/modules/manual_control/ManualControl.cpp
View File

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

40
src/modules/manual_control/ManualControlSelector.cpp
View File

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

8
src/modules/mavlink/streams/MANUAL_CONTROL.hpp
View File

@ -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{};

11
src/modules/mc_att_control/mc_att_control_main.cpp
View File

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

4
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
View File

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

8
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -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{};

8
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

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

6
src/modules/uuv_att_control/uuv_att_control.cpp
View File

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

12
src/modules/vmount/input_rc.cpp
View File

@ -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;