Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Scale manual control setpoint up/down stick -1 to 1 #15949

Merged
merged 4 commits into from Nov 28, 2022
Merged

Scale manual control setpoint up/down stick -1 to 1 #15949

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
8073a70
mavlink_receiver: refactor forgotten manual_control_setpoint naming
MaEtUgR Jun 25, 2020
34729cf
Switch manual_control_setpoint.z scaling from [0,1] to [-1,1]
MaEtUgR Oct 12, 2020
03f8842
manual_control_setpoint: change stick axes naming
MaEtUgR Jun 27, 2022
3835c51
rc_update: adapt throttle trim calibration for [-1,1]
MaEtUgR Nov 24, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
38 changes: 13 additions & 25 deletions msg/ManualControlSetpoint.msg
View file
Open in desktop
Expand Up @@ -16,31 +16,15 @@ 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

# Stick positions [-1,1]
# on a common RC mode 1/2/3/4 remote/joystick the stick deflection: -1 is down/left, 1 is up/right
# Note: QGC sends throttle/z in range [0,1000] - [0,1]. The MAVLink input conversion [0,1] to [-1,1] is at the moment kept backwards compatible.
# Positive values are generally used for:
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Previously the mappings were vehicle and flight mode agnostic. An MC vehicle in position mode does not roll or pitch - it move forward or left-right. So you've made this very unintuitive except for fixed wing vehicles.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Previously the mappings were vehicle and flight mode agnostic.

I do not agree, previously the naming according to the cartesian coordinate system's axes x, y, z only fully made sense for multirotor position mode where the values moved the drone along the respective body axis. r I could never make sense of.

An MC vehicle in position mode does not roll or pitch - it move forward or left-right.

I agree it doesn't fit entirely and it's in my eyes impossible to find a naming that fully fits every single possible mode, that's why I favored consistency across drone remotes.

So you've made this very unintuitive except for fixed wing vehicles.

I disagree. For someone who never worked with drones before and has no reference from any other project or product you could argue the previous naming was equal as suitable. But this is clearly not the case. As I elaborated in #15949 (comment) people come with previous knowledge and or use a remote of some brand and the roll, pitch, yaw, throttle naming is the most popular and consistent one out in the hobby and industry. So it's not just me who got confused when joining PX4 why it needs to have different naming than everyone else, every time has to think twice about which axis it now is and keep a translation table in the comments but numerous other people I asked and found reference for in the code.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

After 5 years working with PX4 I still had to look up every time what x,y,z and r are linked to, so for me these new namings are massively more intuitive.

float32 roll # move right, positive roll rotation, right side down
float32 pitch # move forward, negative pitch rotation, nose down
float32 yaw # positive yaw rotation, clockwise when seen top down
float32 throttle # move up, positive thrust, -1 is minimum available 0% or -100% +1 is 100% thrust

float32 flaps # flap position

Expand All @@ -54,3 +38,7 @@ float32 aux6
bool sticks_moving

# TOPICS manual_control_setpoint manual_control_input
# DEPRECATED: float32 x
# DEPRECATED: float32 y
# DEPRECATED: float32 z
# DEPRECATED: float32 r
12 changes: 6 additions & 6 deletions src/drivers/actuators/modalai_esc/modalai_esc.cpp
View file
Open in desktop
Expand Up @@ -892,9 +892,9 @@ void ModalaiEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
const float flip_pwr_mult = 1.0f - ((float)_parameters.turtle_motor_expo / 100.0f);

// Sitck deflection
const float stick_def_p_abs = fabsf(_manual_control_setpoint.x);
const float stick_def_r_abs = fabsf(_manual_control_setpoint.y);
const float stick_def_y_abs = fabsf(_manual_control_setpoint.r);
const float stick_def_r_abs = fabsf(_manual_control_setpoint.roll);
const float stick_def_p_abs = fabsf(_manual_control_setpoint.pitch);
const float stick_def_y_abs = fabsf(_manual_control_setpoint.yaw);

const float stick_def_p_expo = flip_pwr_mult * stick_def_p_abs + powf(stick_def_p_abs,
3.0) * (1 - flip_pwr_mult);
Expand All @@ -903,9 +903,9 @@ void ModalaiEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
const float stick_def_y_expo = flip_pwr_mult * stick_def_y_abs + powf(stick_def_y_abs,
3.0) * (1 - flip_pwr_mult);

float sign_p = _manual_control_setpoint.x < 0 ? 1 : -1;
float sign_r = _manual_control_setpoint.y < 0 ? 1 : -1;
float sign_y = _manual_control_setpoint.r < 0 ? 1 : -1;
float sign_r = _manual_control_setpoint.roll < 0 ? 1 : -1;
float sign_p = _manual_control_setpoint.pitch < 0 ? 1 : -1;
float sign_y = _manual_control_setpoint.yaw < 0 ? 1 : -1;

float stick_def_len = sqrtf(powf(stick_def_p_abs, 2.0) + powf(stick_def_r_abs, 2.0));
float stick_def_expo_len = sqrtf(powf(stick_def_p_expo, 2.0) + powf(stick_def_r_expo, 2.0));
Expand Down
8 changes: 4 additions & 4 deletions src/lib/mixer_module/functions/FunctionManualRC.hpp
View file
Open in desktop
Expand Up @@ -57,10 +57,10 @@ class FunctionManualRC : public FunctionProviderBase
manual_control_setpoint_s manual_control_setpoint;

if (_topic.update(&manual_control_setpoint)) {
_data[0] = manual_control_setpoint.y; // roll
_data[1] = manual_control_setpoint.x; // pitch
_data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle
_data[3] = manual_control_setpoint.r; // yaw
_data[0] = manual_control_setpoint.roll;
_data[1] = manual_control_setpoint.pitch;
_data[2] = manual_control_setpoint.throttle;
_data[3] = manual_control_setpoint.yaw;
_data[4] = manual_control_setpoint.flaps;
_data[5] = manual_control_setpoint.aux1;
_data[6] = manual_control_setpoint.aux2;
Expand Down
14 changes: 7 additions & 7 deletions src/modules/airship_att_control/airship_att_control.hpp
View file
Open in desktop
Expand Up @@ -82,20 +82,20 @@ class AirshipAttitudeControl : public ModuleBase<AirshipAttitudeControl>, public
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);

uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< parameter updates subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};

uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};

uORB::Publication<actuator_controls_s> _actuator_controls_0_pub;
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};

manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */
vehicle_status_s _vehicle_status {}; /**< vehicle status */
actuator_controls_s _actuator_controls {}; /**< actuator controls */
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_status_s _vehicle_status{};
actuator_controls_s _actuator_controls{};

perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _loop_perf;

};
8 changes: 4 additions & 4 deletions src/modules/airship_att_control/airship_att_control_main.cpp
View file
Open in desktop
Expand Up @@ -91,9 +91,9 @@ AirshipAttitudeControl::publish_actuator_controls()

} else {
_actuator_controls.control[0] = 0.0f;
_actuator_controls.control[1] = _manual_control_sp.x;
_actuator_controls.control[2] = _manual_control_sp.r;
_actuator_controls.control[3] = _manual_control_sp.z;
_actuator_controls.control[1] = _manual_control_setpoint.pitch;
_actuator_controls.control[2] = _manual_control_setpoint.yaw;
_actuator_controls.control[3] = (_manual_control_setpoint.throttle + 1.f) * .5f;
}

// note: _actuator_controls.timestamp_sample is set in AirshipAttitudeControl::Run()
Expand Down Expand Up @@ -150,7 +150,7 @@ AirshipAttitudeControl::Run()
publishThrustSetpoint(angular_velocity.timestamp_sample);

/* check for updates in manual control topic */
_manual_control_sp_sub.update(&_manual_control_sp);
_manual_control_setpoint_sub.update(&_manual_control_setpoint);

/* check for updates in vehicle status topic */
_vehicle_status_sub.update(&_vehicle_status);
Expand Down
4 changes: 2 additions & 2 deletions src/modules/commander/Commander.cpp
View file
Open in desktop
Expand Up @@ -2714,8 +2714,8 @@ void Commander::manualControlCheck()

if (manual_control_updated && manual_control_setpoint.valid) {

_is_throttle_above_center = (manual_control_setpoint.z > 0.6f);
_is_throttle_low = (manual_control_setpoint.z < 0.1f);
_is_throttle_above_center = (manual_control_setpoint.throttle > 0.2f);
_is_throttle_low = (manual_control_setpoint.throttle < -0.8f);
Comment on lines +2717 to +2718
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not directly related, but this I think may be better handled if we have a function to just calculate these flags off of raw manual control setpoint struct data (preferably a class):

// If it is a C++ Class
manual_control_setpoint.isThrottleAboveCenter() const
{
    return (_data.throttle > 0.2f);
}

Copy link
Member Author

@MaEtUgR MaEtUgR Nov 21, 2022 •
edited

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Definitely. I plan to do a library next that allows to get this information and directly scaled throttle z, ... from the raw manual_control_setpoint message. I think that's much nicer than having repeated thresholds everywhere. But shouldn't be part of this pr.


if (_arm_state_machine.isArmed()) {
// Abort autonomous mode and switch to position mode if sticks are moved significantly
Expand Down
6 changes: 3 additions & 3 deletions src/modules/commander/rc_calibration.cpp
View file
Open in desktop
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.y * roll_scale + roll_trim_active;
float p = manual_control_setpoint.roll * roll_scale + roll_trim_active;
int p1r = param_set(param_find("TRIM_ROLL"), &p);
/*
we explicitly swap sign here because the trim is added to the actuator controls
which are moving in an inverse sense to manual pitch inputs
*/
p = -manual_control_setpoint.x * pitch_scale + pitch_trim_active;
p = -manual_control_setpoint.pitch * pitch_scale + pitch_trim_active;
int p2r = param_set(param_find("TRIM_PITCH"), &p);
p = manual_control_setpoint.r * yaw_scale + yaw_trim_active;
p = manual_control_setpoint.yaw * yaw_scale + yaw_trim_active;
int p3r = param_set(param_find("TRIM_YAW"), &p);

if (p1r != 0 || p2r != 0 || p3r != 0) {
Expand Down
9 changes: 4 additions & 5 deletions src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
View file
Open in desktop
Expand Up @@ -51,11 +51,10 @@ bool Sticks::checkAndUpdateStickInputs()

if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// Linear scale
_positions(0) = manual_control_setpoint.x; // NED x, pitch [-1,1]
_positions(1) = manual_control_setpoint.y; // NED y, roll [-1,1]
_positions(2) = -(math::constrain(manual_control_setpoint.z, 0.0f,
1.0f) - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
junwoo091400 marked this conversation as resolved.
Show resolved Hide resolved
_positions(3) = manual_control_setpoint.r; // yaw [-1,1]
_positions(0) = manual_control_setpoint.pitch;
_positions(1) = manual_control_setpoint.roll;
_positions(2) = -manual_control_setpoint.throttle;
_positions(3) = manual_control_setpoint.yaw;

// Exponential scale
_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
Expand Down
33 changes: 17 additions & 16 deletions src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View file
Open in desktop
Expand Up @@ -141,18 +141,20 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)

if (!_vcontrol_mode.flag_control_climb_rate_enabled) {

const float throttle = (_manual_control_setpoint.throttle + 1.f) * .5f;

if (_vcontrol_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs

_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());

_att_sp.pitch_body = -_manual_control_setpoint.x * radians(_param_fw_man_p_max.get())
_att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get())
+ radians(_param_fw_psp_off.get());
_att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));

_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
_att_sp.thrust_body[0] = throttle;

Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
Expand All @@ -168,24 +170,23 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)

// RATE mode we need to generate the rate setpoint from manual user inputs
_rates_sp.timestamp = hrt_absolute_time();
_rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get());
_rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get());
_rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get());
_rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
_rates_sp.roll = _manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get());
_rates_sp.pitch = -_manual_control_setpoint.pitch * radians(_param_fw_acro_y_max.get());
_rates_sp.yaw = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get());
_rates_sp.thrust_body[0] = throttle;

_rate_sp_pub.publish(_rates_sp);

} else {
/* manual/direct control */
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
_manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
-_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f,
1.0f);
_landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.r;
_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = throttle;
_landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.yaw;
}
}
}
Expand Down Expand Up @@ -550,7 +551,7 @@ void FixedwingAttitudeControl::Run()

/* add yaw rate setpoint from sticks in all attitude-controlled modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.r * radians(_param_fw_man_yr_max.get()),
body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.yaw * radians(_param_fw_man_yr_max.get()),
-radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get()));
}

Expand Down Expand Up @@ -592,11 +593,11 @@ void FixedwingAttitudeControl::Run()

if (_vcontrol_mode.flag_control_manual_enabled) {
// always direct control of steering wheel with yaw stick in manual modes
wheel_u = _manual_control_setpoint.r;
wheel_u = _manual_control_setpoint.yaw;

} else {
// XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from
// position controller during auto modes _manual_control_setpoint.r gets passed
// position controller during auto modes _manual_control_setpoint.yaw gets passed
// whenever nudging is enabled, otherwise zero
wheel_u = wheel_control ? _wheel_ctrl.control_bodyrate(dt, control_input)
+ _att_sp.yaw_sp_move_rate : 0.f;
Expand Down