Skip to content

Commit

Permalink
Use backwards compatible manual_control_setpoint instead of manual_co…
Browse files Browse the repository at this point in the history
…ntrol_input
  • Loading branch information
MaEtUgR committed Nov 8, 2021
1 parent 0f82b49 commit effc78c
Show file tree
Hide file tree
Showing 30 changed files with 252 additions and 208 deletions.
1 change: 0 additions & 1 deletion msg/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
52 changes: 0 additions & 52 deletions msg/manual_control_input.msg

This file was deleted.

54 changes: 52 additions & 2 deletions msg/manual_control_setpoint.msg
Original file line number Diff line number Diff line change
@@ -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 changes: 3 additions & 3 deletions src/examples/fixedwing_control/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down
22 changes: 11 additions & 11 deletions src/lib/mixer_module/functions.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand Down
6 changes: 3 additions & 3 deletions src/modules/airship_att_control/airship_att_control_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
8 changes: 4 additions & 4 deletions src/modules/commander/Commander.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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 {
Expand Down Expand Up @@ -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);
Expand Down
6 changes: 3 additions & 3 deletions src/modules/commander/rc_calibration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
8 changes: 4 additions & 4 deletions src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down
36 changes: 18 additions & 18 deletions src/modules/fw_att_control/FixedwingAttitudeControl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
}
}
Expand Down Expand Up @@ -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)) {
Expand Down Expand Up @@ -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();
Expand All @@ -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) {
Expand Down Expand Up @@ -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) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
Loading

0 comments on commit effc78c

Please sign in to comment.