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

View File

@ -103,7 +103,6 @@ set(msg_files
logger_status.msg logger_status.msg
mag_worker_data.msg mag_worker_data.msg
magnetometer_bias_estimate.msg magnetometer_bias_estimate.msg
manual_control_input.msg
manual_control_setpoint.msg manual_control_setpoint.msg
manual_control_switches.msg manual_control_switches.msg
mavlink_log.msg mavlink_log.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

View File

@ -1,6 +1,56 @@
uint64 timestamp # time since system start (microseconds) uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
bool valid 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

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 */ /* 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) && if (PX4_ISFINITE(manual_control_setpoint.z) &&
(manual_control_setpoint.chosen_input.z >= 0.6f) && (manual_control_setpoint.z >= 0.6f) &&
(manual_control_setpoint.chosen_input.z <= 1.0f)) { (manual_control_setpoint.z <= 1.0f)) {
} }
/* get the system status and the flight mode we're in */ /* get the system status and the flight mode we're in */

View File

@ -119,17 +119,17 @@ void FunctionManualRC::update()
manual_control_setpoint_s manual_control_setpoint; manual_control_setpoint_s manual_control_setpoint;
if (_topic.update(&manual_control_setpoint)) { if (_topic.update(&manual_control_setpoint)) {
_data[0] = manual_control_setpoint.chosen_input.y; // roll _data[0] = manual_control_setpoint.y; // roll
_data[1] = manual_control_setpoint.chosen_input.x; // pitch _data[1] = manual_control_setpoint.x; // pitch
_data[2] = manual_control_setpoint.chosen_input.z * 2.f - 1.f; // throttle _data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle
_data[3] = manual_control_setpoint.chosen_input.r; // yaw _data[3] = manual_control_setpoint.r; // yaw
_data[4] = manual_control_setpoint.chosen_input.flaps; _data[4] = manual_control_setpoint.flaps;
_data[5] = manual_control_setpoint.chosen_input.aux1; _data[5] = manual_control_setpoint.aux1;
_data[6] = manual_control_setpoint.chosen_input.aux2; _data[6] = manual_control_setpoint.aux2;
_data[7] = manual_control_setpoint.chosen_input.aux3; _data[7] = manual_control_setpoint.aux3;
_data[8] = manual_control_setpoint.chosen_input.aux4; _data[8] = manual_control_setpoint.aux4;
_data[9] = manual_control_setpoint.chosen_input.aux5; _data[9] = manual_control_setpoint.aux5;
_data[10] = manual_control_setpoint.chosen_input.aux6; _data[10] = manual_control_setpoint.aux6;
} }
} }

View File

@ -91,9 +91,9 @@ AirshipAttitudeControl::publish_actuator_controls()
} else { } else {
_actuators.control[0] = 0.0f; _actuators.control[0] = 0.0f;
_actuators.control[1] = _manual_control_sp.chosen_input.x; _actuators.control[1] = _manual_control_sp.x;
_actuators.control[2] = _manual_control_sp.chosen_input.r; _actuators.control[2] = _manual_control_sp.r;
_actuators.control[3] = _manual_control_sp.chosen_input.z; _actuators.control[3] = _manual_control_sp.z;
} }
// note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run() // note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run()

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 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 (!_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 // 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; _status.rc_signal_lost = false;
_is_throttle_above_center = manual_control_setpoint.chosen_input.z > 0.6f; _is_throttle_above_center = manual_control_setpoint.z > 0.6f;
_is_throttle_low = manual_control_setpoint.chosen_input.z < 0.1f; _is_throttle_low = manual_control_setpoint.z < 0.1f;
_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp; _last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
} else { } else {
@ -2400,7 +2400,7 @@ Commander::run()
&& _armed.armed && _armed.armed
&& !_status_flags.rc_input_blocked && !_status_flags.rc_input_blocked
&& manual_control_setpoint.valid && manual_control_setpoint.valid
&& manual_control_setpoint.user_override && manual_control_setpoint.sticks_moving
&& override_enabled) { && override_enabled) {
const transition_result_t posctl_result = const transition_result_t posctl_result =
main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state); main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);

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 /* set parameters: the new trim values are the combination of active trim values
and the values coming from the remote control of the user 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); int p1r = param_set(param_find("TRIM_ROLL"), &p);
/* /*
we explicitly swap sign here because the trim is added to the actuator controls 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 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); 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); int p3r = param_set(param_find("TRIM_YAW"), &p);
if (p1r != 0 || p2r != 0 || p3r != 0) { if (p1r != 0 || p2r != 0 || p3r != 0) {

View File

@ -51,11 +51,11 @@ bool Sticks::checkAndSetStickInputs()
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// Linear scale // Linear scale
_positions(0) = manual_control_setpoint.chosen_input.x; // NED x, pitch [-1,1] _positions(0) = manual_control_setpoint.x; // NED x, pitch [-1,1]
_positions(1) = manual_control_setpoint.chosen_input.y; // NED y, roll [-1,1] _positions(1) = manual_control_setpoint.y; // NED y, roll [-1,1]
_positions(2) = -(math::constrain(manual_control_setpoint.chosen_input.z, 0.0f, _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] 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 // Exponential scale
_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get()); _positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());

View File

@ -149,15 +149,15 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
if (_vcontrol_mode.flag_control_attitude_enabled) { if (_vcontrol_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs // 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()); + radians(_param_fw_psp_off.get());
_att_sp.pitch_body = constrain(_att_sp.pitch_body, _att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); -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.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)); Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d); 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 // RATE mode we need to generate the rate setpoint from manual user inputs
_rates_sp.timestamp = hrt_absolute_time(); _rates_sp.timestamp = hrt_absolute_time();
_rates_sp.roll = _manual_control_setpoint.chosen_input.y * radians(_param_fw_acro_x_max.get()); _rates_sp.roll = _manual_control_setpoint.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.pitch = -_manual_control_setpoint.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.yaw = _manual_control_setpoint.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.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
_rate_sp_pub.publish(_rates_sp); _rate_sp_pub.publish(_rates_sp);
} else { } else {
/* manual/direct control */ /* manual/direct control */
_actuators.control[actuator_controls_s::INDEX_ROLL] = _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] = _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] = _actuators.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.chosen_input.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); _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.chosen_input.z, 0.0f, _actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f,
1.0f); 1.0f);
} }
} }
@ -563,7 +563,7 @@ void FixedwingAttitudeControl::Run()
/* add in manual rudder control in manual modes */ /* add in manual rudder control in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) { 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)) { if (!PX4_ISFINITE(yaw_u)) {
@ -644,10 +644,10 @@ void FixedwingAttitudeControl::Run()
* constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); * constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; _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; _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future // 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 */ /* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time(); _actuators.timestamp = hrt_absolute_time();
@ -672,9 +672,9 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
float flap_control = 0.0f; float flap_control = 0.0f;
/* map flaps by default to manual if valid */ /* 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) { && 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 } else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) { && fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
@ -706,10 +706,10 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
float flaperon_control = 0.0f; float flaperon_control = 0.0f;
/* map flaperons by default to manual if valid */ /* 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) { && 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 } else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {

View File

@ -423,8 +423,8 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
if (_state != state::wait_for_disarm if (_state != state::wait_for_disarm
&& _state != state::idle && _state != state::idle
&& (((now - _state_start_time) > 20_s) && (((now - _state_start_time) > 20_s)
|| (fabsf(manual_control_setpoint.chosen_input.x) > 0.2f) || (fabsf(manual_control_setpoint.x) > 0.2f)
|| (fabsf(manual_control_setpoint.chosen_input.y) > 0.2f))) { || (fabsf(manual_control_setpoint.y) > 0.2f))) {
_state = state::fail; _state = state::fail;
_state_start_time = now; _state_start_time = now;
} }

View File

@ -280,15 +280,15 @@ FixedwingPositionControl::manual_control_setpoint_poll()
{ {
_manual_control_setpoint_sub.update(&_manual_control_setpoint); _manual_control_setpoint_sub.update(&_manual_control_setpoint);
_manual_control_setpoint_altitude = _manual_control_setpoint.chosen_input.x; _manual_control_setpoint_altitude = _manual_control_setpoint.x;
_manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f, 1.0f); _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
if (_param_fw_posctl_inv_st.get()) { if (_param_fw_posctl_inv_st.get()) {
/* Alternate stick allocation (similar concept as for multirotor systems: /* Alternate stick allocation (similar concept as for multirotor systems:
* demanding up/down with the throttle stick, and move faster/break with the pitch one. * 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_altitude = -(math::constrain(_manual_control_setpoint.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_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 /* reset setpoints from other modes (auto) otherwise we won't
* level out without new manual input */ * 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 _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, tecs_status_s::TECS_MODE_NORMAL,
height_rate_sp); 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 _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
/* Copy thrust and pitch values from tecs */ /* Copy thrust and pitch values from tecs */
@ -1750,8 +1750,8 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
height_rate_sp); height_rate_sp);
/* heading control */ /* heading control */
if (fabsf(_manual_control_setpoint.chosen_input.y) < HDG_HOLD_MAN_INPUT_THRESH && if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
fabsf(_manual_control_setpoint.chosen_input.r) < HDG_HOLD_MAN_INPUT_THRESH) { fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */ /* heading / roll is zero, lock onto current heading */
if (fabsf(_yawrate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { 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 || if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
fabsf(_manual_control_setpoint.chosen_input.r) >= HDG_HOLD_MAN_INPUT_THRESH) { fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
_hdg_hold_enabled = false; _hdg_hold_enabled = false;
_yaw_lock_engaged = false; _yaw_lock_engaged = false;
// do slew rate limiting on roll if enabled // 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()); const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get());
if (dt > 0.f && roll_rate_slew_rad > 0.f) { if (dt > 0.f && roll_rate_slew_rad > 0.f) {

View File

@ -1085,7 +1085,7 @@ bool Logger::start_stop_logging()
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { 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; updated = true;
} }

View File

@ -83,9 +83,9 @@ void ManualControl::Run()
_selector.updateValidityOfChosenInput(now); _selector.updateValidityOfChosenInput(now);
for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++) { 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); _selector.updateWithNewInputSample(now, manual_control_input, i);
} }
} }
@ -96,25 +96,25 @@ void ManualControl::Run()
if (_selector.setpoint().valid) { if (_selector.setpoint().valid) {
_published_invalid_once = false; _published_invalid_once = false;
processStickArming(_selector.setpoint().chosen_input); processStickArming(_selector.setpoint());
// User override by stick // User override by stick
const float dt_s = (now - _last_time) / 1e6f; const float dt_s = (now - _last_time) / 1e6f;
const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); 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) const bool rpy_moving = (fabsf(_x_diff.update(_selector.setpoint().x, dt_s)) > minimum_stick_change)
|| (fabsf(_y_diff.update(_selector.setpoint().chosen_input.y, dt_s)) > minimum_stick_change) || (fabsf(_y_diff.update(_selector.setpoint().y, dt_s)) > minimum_stick_change)
|| (fabsf(_r_diff.update(_selector.setpoint().chosen_input.r, 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] // Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1]
const bool throttle_moved = const bool throttle_moving =
(fabsf(_z_diff.update(_selector.setpoint().chosen_input.z, dt_s)) * 2.f) > minimum_stick_change; (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) { if (switches_updated) {
// Only use switches if current source is RC as well. // 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 (_previous_switches_initialized) {
if (switches.mode_slot != _previous_switches.mode_slot) { if (switches.mode_slot != _previous_switches.mode_slot) {
evaluateModeSlot(switches.mode_slot); evaluateModeSlot(switches.mode_slot);
@ -223,11 +223,11 @@ void ManualControl::Run()
if (instance != _previous_manual_control_input_instance) { if (instance != _previous_manual_control_input_instance) {
if ((0 <= _previous_manual_control_input_instance) if ((0 <= _previous_manual_control_input_instance)
&& (_previous_manual_control_input_instance < MAX_MANUAL_INPUT_COUNT)) { && (_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)) { 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; _previous_manual_control_input_instance = instance;
@ -258,7 +258,7 @@ void ManualControl::Run()
perf_end(_loop_perf); perf_end(_loop_perf);
} }
void ManualControl::processStickArming(const manual_control_input_s &input) void ManualControl::processStickArming(const manual_control_setpoint_s &input)
{ {
// Arm gesture // Arm gesture
const bool right_stick_centered = (fabsf(input.x) < 0.1f) && (fabsf(input.y) < 0.1f); const bool right_stick_centered = (fabsf(input.x) < 0.1f) && (fabsf(input.y) < 0.1f);

View File

@ -43,7 +43,6 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/topics/action_request.h> #include <uORB/topics/action_request.h>
#include <uORB/topics/landing_gear.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_switches.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
@ -78,7 +77,7 @@ private:
static constexpr int MAX_MANUAL_INPUT_COUNT = 3; static constexpr int MAX_MANUAL_INPUT_COUNT = 3;
void Run() override; 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 evaluateModeSlot(uint8_t mode_slot);
void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0); 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}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
int _previous_manual_control_input_instance{-1}; 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), 0},
{this, ORB_ID(manual_control_input), 1}, {this, ORB_ID(manual_control_input), 1},
{this, ORB_ID(manual_control_input), 2}, {this, ORB_ID(manual_control_input), 2},

View File

@ -35,43 +35,43 @@
void ManualControlSelector::updateValidityOfChosenInput(uint64_t now) void ManualControlSelector::updateValidityOfChosenInput(uint64_t now)
{ {
if (!isInputValid(_setpoint.chosen_input, now)) { if (!isInputValid(_setpoint, now)) {
_setpoint.valid = false; _setpoint.valid = false;
_instance = -1; _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 // First check if the chosen input got invalid, so it can get replaced
updateValidityOfChosenInput(now); 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; 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 // 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)) { if (isInputValid(input, now) && (update_existing_input || start_using_new_input)) {
_setpoint.chosen_input = input; _setpoint = input;
_setpoint.valid = true; _setpoint.valid = true;
_instance = instance; _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 // Check for timeout
const bool sample_from_the_past = now >= input.timestamp_sample; const bool sample_from_the_past = now >= input.timestamp_sample;
const bool sample_newer_than_timeout = now - input.timestamp_sample < _timeout; const bool sample_newer_than_timeout = now - input.timestamp_sample < _timeout;
// Check if source matches the configuration // 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) && const bool source_mavlink_matched = (_rc_in_mode == 1) &&
(input.data_source == manual_control_input_s::SOURCE_MAVLINK_0 (input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_1 || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_1
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_2 || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_2
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_3 || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_4 || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_4
|| input.data_source == manual_control_input_s::SOURCE_MAVLINK_5); || input.data_source == manual_control_setpoint_s::SOURCE_MAVLINK_5);
const bool source_any_matched = (_rc_in_mode == 3); const bool source_any_matched = (_rc_in_mode == 3);
return sample_from_the_past && sample_newer_than_timeout return sample_from_the_past && sample_newer_than_timeout

View File

@ -34,7 +34,6 @@
#pragma once #pragma once
#include <stdint.h> #include <stdint.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
class ManualControlSelector class ManualControlSelector
@ -43,12 +42,12 @@ public:
void setRcInMode(int32_t rc_in_mode) { _rc_in_mode = rc_in_mode; } void setRcInMode(int32_t rc_in_mode) { _rc_in_mode = rc_in_mode; }
void setTimeout(uint64_t timeout) { _timeout = timeout; } void setTimeout(uint64_t timeout) { _timeout = timeout; }
void updateValidityOfChosenInput(uint64_t now); 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(); manual_control_setpoint_s &setpoint();
int instance() const { return _instance; }; int instance() const { return _instance; };
private: 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{}; manual_control_setpoint_s _setpoint{};
uint64_t _timeout{0}; uint64_t _timeout{0};

View File

@ -48,16 +48,16 @@ TEST(ManualControlSelector, RcInputContinuous)
uint64_t timestamp = some_time; uint64_t timestamp = some_time;
// Now provide input with the correct source. // Now provide input with the correct source.
manual_control_input_s input {}; manual_control_setpoint_s input {};
input.data_source = manual_control_input_s::SOURCE_RC; input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp; input.timestamp_sample = timestamp;
for (int i = 0; i < 5; i++) { for (int i = 0; i < 5; i++) {
selector.updateWithNewInputSample(timestamp, input, 1); selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid); 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_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; timestamp += 100_ms;
input.timestamp_sample = timestamp; input.timestamp_sample = timestamp;
} }
@ -71,8 +71,8 @@ TEST(ManualControlSelector, RcInputOnly)
uint64_t timestamp = some_time; uint64_t timestamp = some_time;
manual_control_input_s input {}; manual_control_setpoint_s input {};
input.data_source = manual_control_input_s::SOURCE_MAVLINK_0; input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
input.timestamp_sample = timestamp; input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0); selector.updateWithNewInputSample(timestamp, input, 0);
@ -81,12 +81,12 @@ TEST(ManualControlSelector, RcInputOnly)
timestamp += 100_ms; timestamp += 100_ms;
// Now provide input with the correct source. // 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; input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1); selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid); 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); EXPECT_EQ(selector.instance(), 1);
} }
@ -98,8 +98,8 @@ TEST(ManualControlSelector, MavlinkInputOnly)
uint64_t timestamp = some_time; uint64_t timestamp = some_time;
manual_control_input_s input {}; manual_control_setpoint_s input {};
input.data_source = manual_control_input_s::SOURCE_RC; input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp; input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0); selector.updateWithNewInputSample(timestamp, input, 0);
@ -108,23 +108,23 @@ TEST(ManualControlSelector, MavlinkInputOnly)
timestamp += 100_ms; timestamp += 100_ms;
// Now provide input with the correct source. // 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; input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1); selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid); 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); EXPECT_EQ(selector.instance(), 1);
timestamp += 100_ms; timestamp += 100_ms;
// But only the first MAVLink source wins, others are too late. // 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; input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1); selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid); 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); EXPECT_EQ(selector.instance(), 1);
} }
@ -136,35 +136,35 @@ TEST(ManualControlSelector, AutoInput)
uint64_t timestamp = some_time; uint64_t timestamp = some_time;
manual_control_input_s input {}; manual_control_setpoint_s input {};
input.data_source = manual_control_input_s::SOURCE_RC; input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp; input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0); selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid); 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); EXPECT_EQ(selector.instance(), 0);
timestamp += 100_ms; timestamp += 100_ms;
// Now provide input from MAVLink as well which should get ignored. // 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; input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1); selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid); 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); EXPECT_EQ(selector.instance(), 0);
timestamp += 500_ms; timestamp += 500_ms;
// Now we'll let RC time out, so it should switch to MAVLINK. // 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; input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 1); selector.updateWithNewInputSample(timestamp, input, 1);
EXPECT_TRUE(selector.setpoint().valid); 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); EXPECT_EQ(selector.instance(), 1);
} }
@ -176,13 +176,13 @@ TEST(ManualControlSelector, RcTimeout)
uint64_t timestamp = some_time; uint64_t timestamp = some_time;
manual_control_input_s input {}; manual_control_setpoint_s input {};
input.data_source = manual_control_input_s::SOURCE_RC; input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp; input.timestamp_sample = timestamp;
selector.updateWithNewInputSample(timestamp, input, 0); selector.updateWithNewInputSample(timestamp, input, 0);
EXPECT_TRUE(selector.setpoint().valid); 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); EXPECT_EQ(selector.instance(), 0);
timestamp += 600_ms; timestamp += 600_ms;
@ -202,8 +202,8 @@ TEST(ManualControlSelector, RcOutdated)
uint64_t timestamp = some_time; uint64_t timestamp = some_time;
manual_control_input_s input {}; manual_control_setpoint_s input {};
input.data_source = manual_control_input_s::SOURCE_RC; input.data_source = manual_control_setpoint_s::SOURCE_RC;
input.timestamp_sample = timestamp - 600_ms; // First sample is already outdated input.timestamp_sample = timestamp - 600_ms; // First sample is already outdated
selector.updateWithNewInputSample(timestamp, input, 0); selector.updateWithNewInputSample(timestamp, input, 0);

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
}

View File

@ -2094,12 +2094,12 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
return; return;
} }
manual_control_input_s manual{}; manual_control_setpoint_s manual{};
manual.x = man.x / 1000.0f; manual.x = man.x / 1000.0f;
manual.y = man.y / 1000.0f; manual.y = man.y / 1000.0f;
manual.r = man.r / 1000.0f; manual.r = man.r / 1000.0f;
manual.z = man.z / 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.timestamp = manual.timestamp_sample = hrt_absolute_time();
_manual_control_input_pub.publish(manual); _manual_control_input_pub.publish(manual);
} }

View File

@ -80,7 +80,7 @@
#include <uORB/topics/irlock_report.h> #include <uORB/topics/irlock_report.h>
#include <uORB/topics/landing_target_pose.h> #include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/log_message.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/obstacle_distance.h>
#include <uORB/topics/offboard_control_mode.h> #include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/onboard_computer_status.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> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<distance_sensor_s> _flow_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<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<ping_s> _ping_pub{ORB_ID(ping)};
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)}; uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)};

View File

@ -68,10 +68,10 @@ private:
mavlink_manual_control_t msg{}; mavlink_manual_control_t msg{};
msg.target = mavlink_system.sysid; msg.target = mavlink_system.sysid;
msg.x = manual_control_setpoint.chosen_input.x * 1000; msg.x = manual_control_setpoint.x * 1000;
msg.y = manual_control_setpoint.chosen_input.y * 1000; msg.y = manual_control_setpoint.y * 1000;
msg.z = manual_control_setpoint.chosen_input.z * 1000; msg.z = manual_control_setpoint.z * 1000;
msg.r = manual_control_setpoint.chosen_input.r * 1000; msg.r = manual_control_setpoint.r * 1000;
manual_control_switches_s manual_control_switches{}; manual_control_switches_s manual_control_switches{};

View File

@ -127,11 +127,11 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
if (reset_yaw_sp) { if (reset_yaw_sp) {
_man_yaw_sp = yaw; _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) { || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) {
const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); 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); _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_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
_man_y_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_x_input_filter.update(_manual_control_setpoint.x * _man_tilt_max);
_man_y_input_filter.update(_manual_control_setpoint.chosen_input.y * _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 x = _man_x_input_filter.getState();
const float y = _man_y_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); Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
q_sp.copyTo(attitude_setpoint.q_d); 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)); 1.0f));
attitude_setpoint.timestamp = hrt_absolute_time(); attitude_setpoint.timestamp = hrt_absolute_time();

View File

@ -444,8 +444,8 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
if (_state != state::wait_for_disarm if (_state != state::wait_for_disarm
&& _state != state::idle && _state != state::idle
&& (((now - _state_start_time) > 20_s) && (((now - _state_start_time) > 20_s)
|| (fabsf(manual_control_setpoint.chosen_input.x) > 0.05f) || (fabsf(manual_control_setpoint.x) > 0.05f)
|| (fabsf(manual_control_setpoint.chosen_input.y) > 0.05f))) { || (fabsf(manual_control_setpoint.y) > 0.05f))) {
_state = state::fail; _state = state::fail;
_state_start_time = now; _state_start_time = now;
} }

View File

@ -175,12 +175,12 @@ MulticopterRateControl::Run()
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// manual rates control - ACRO mode // manual rates control - ACRO mode
const Vector3f man_rate_sp{ 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.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.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.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
_rates_sp = man_rate_sp.emult(_acro_rate_max); _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 // publish rate setpoint
vehicle_rates_setpoint_s v_rates_sp{}; vehicle_rates_setpoint_s v_rates_sp{};

View File

@ -661,9 +661,9 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
void RCUpdate::UpdateManualControlInput(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.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 // limit controls
manual_control_input.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f); manual_control_input.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f);

View File

@ -53,7 +53,7 @@
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp> #include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h> #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/manual_control_switches.h>
#include <uORB/topics/input_rc.h> #include <uORB/topics/input_rc.h>
#include <uORB/topics/rc_channels.h> #include <uORB/topics/rc_channels.h>
@ -166,7 +166,7 @@ private:
uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)}; uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)};
uORB::Publication<rc_channels_s> _rc_channels_pub{ORB_ID(rc_channels)}; 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<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)}; uORB::Publication<actuator_controls_s> _actuator_group_3_pub{ORB_ID(actuator_controls_3)};

View File

@ -136,12 +136,12 @@ RoverPositionControl::manual_control_setpoint_poll()
} else { } else {
const float yaw_rate = math::radians(_param_gnd_man_y_max.get()); 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); _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt);
} }
_att_sp.yaw_body = _manual_yaw_sp; _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)); Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d); 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; _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x;
// Set heading from the manual roll input channel // Set heading from the manual roll input channel
_act_controls.control[actuator_controls_s::INDEX_YAW] = _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 // 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; _reset_yaw_sp = true;
} }

View File

@ -280,9 +280,9 @@ void UUVAttitudeControl::Run()
// returning immediately and this loop will eat up resources. // returning immediately and this loop will eat up resources.
if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) { if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
/* manual/direct control */ /* manual/direct control */
constrain_actuator_commands(_manual_control_setpoint.chosen_input.y, -_manual_control_setpoint.chosen_input.x, constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x,
_manual_control_setpoint.chosen_input.r, _manual_control_setpoint.r,
_manual_control_setpoint.chosen_input.z, 0.f, 0.f); _manual_control_setpoint.z, 0.f, 0.f);
} }
} }

View File

@ -155,22 +155,22 @@ float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_se
switch (_aux_channels[channel_idx]) { switch (_aux_channels[channel_idx]) {
case 1: case 1:
return manual_control_setpoint.chosen_input.aux1; return manual_control_setpoint.aux1;
case 2: case 2:
return manual_control_setpoint.chosen_input.aux2; return manual_control_setpoint.aux2;
case 3: case 3:
return manual_control_setpoint.chosen_input.aux3; return manual_control_setpoint.aux3;
case 4: case 4:
return manual_control_setpoint.chosen_input.aux4; return manual_control_setpoint.aux4;
case 5: case 5:
return manual_control_setpoint.chosen_input.aux5; return manual_control_setpoint.aux5;
case 6: case 6:
return manual_control_setpoint.chosen_input.aux6; return manual_control_setpoint.aux6;
default: default:
return 0.0f; return 0.0f;