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

Use backwards compatible manual_control_setpoint instead of manual_control_input

This commit is contained in:
Matthias Grob 2021-11-03 11:33:05 +01:00
parent 423aadcc4f
commit fabf865411
30 changed files with 252 additions and 208 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

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

52
msg/manual_control_input.msg
View File

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

54
msg/manual_control_setpoint.msg
View File

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

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

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

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

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

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

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

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

36
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.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) {

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

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

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.chosen_input.aux1 > 0.3f);
desired_state = (manual_control_setpoint.aux1 > 0.3f);
updated = true;
}

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

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

5
src/modules/manual_control/ManualControl.hpp
View File

@ -43,7 +43,6 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/topics/action_request.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
@ -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},

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

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

5
src/modules/manual_control/ManualControlSelector.hpp
View File

@ -34,7 +34,6 @@
#pragma once
#include <stdint.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_setpoint.h>
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};

52
src/modules/manual_control/ManualControlSelectorTest.cpp
View File

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

49
src/modules/manual_control/MovingDiffTest.cpp Normal file
View File

@ -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 <gtest/gtest.h>
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
}

4
src/modules/mavlink/mavlink_receiver.cpp
View File

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

4
src/modules/mavlink/mavlink_receiver.h
View File

@ -80,7 +80,7 @@
#include <uORB/topics/irlock_report.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/log_message.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/onboard_computer_status.h>
@ -326,7 +326,7 @@ private:
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
uORB::PublicationMulti<manual_control_input_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
uORB::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping)};
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)};

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

10
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.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();

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

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

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

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

@ -661,9 +661,9 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
void RCUpdate::UpdateManualControlInput(const hrt_abstime &timestamp_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);

4
src/modules/rc_update/rc_update.h
View File

@ -53,7 +53,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/rc_channels.h>
@ -166,7 +166,7 @@ private:
uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)};
uORB::Publication<rc_channels_s> _rc_channels_pub{ORB_ID(rc_channels)};
uORB::PublicationMulti<manual_control_input_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
uORB::Publication<manual_control_switches_s> _manual_control_switches_pub{ORB_ID(manual_control_switches)};
uORB::Publication<actuator_controls_s> _actuator_group_3_pub{ORB_ID(actuator_controls_3)};

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

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

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