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

manual_control_setpoint: change stick axes naming 

In review it was requested to have a different name for
manual_control_setpoint.z because of the adjusted range.

I started to investigate what naming is most intuitive and found
that most people recognize the stick axes as roll, pitch, yaw, throttle.
It comes at no surprise because other autopilots
and APIs seem to share this convention.

While changing the code I realized that even within the code base
the axes are usually assigned to a variable with that name or
have comments next to the assignment clarifying the axes
using these names.
This commit is contained in:
Matthias Grob 2022-06-27 18:16:05 +02:00
parent 83246c84cf
commit 331cb21dee
20 changed files with 118 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
msg/ManualControlSetpoint.msg
View File

@ -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 -1..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 means up and any value < 0 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:
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
@ -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
src/drivers/actuators/modalai_esc/modalai_esc.cpp
View File

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

8
src/lib/mixer_module/functions/FunctionManualRC.hpp
View File

@ -57,10 +57,10 @@ public:
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; // 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;

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

@ -91,9 +91,9 @@ AirshipAttitudeControl::publish_actuator_controls()
} else {
_actuator_controls.control[0] = 0.0f;
_actuator_controls.control[1] = _manual_control_setpoint.x;
_actuator_controls.control[2] = _manual_control_setpoint.r;
_actuator_controls.control[3] = (_manual_control_setpoint.z + 1.f) * .5f;
_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()

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

@ -2714,8 +2714,8 @@ void Commander::manualControlCheck()
if (manual_control_updated && manual_control_setpoint.valid) {
_is_throttle_above_center = (manual_control_setpoint.z > 0.2f);
_is_throttle_low = (manual_control_setpoint.z < -0.8f);
_is_throttle_above_center = (manual_control_setpoint.throttle > 0.2f);
_is_throttle_low = (manual_control_setpoint.throttle < -0.8f);
if (_arm_state_machine.isArmed()) {
// Abort autonomous mode and switch to position mode if sticks are moved significantly

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

@ -85,15 +85,15 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub)
/* set parameters: the new trim values are the combination of active trim values
and the values coming from the remote control of the user
*/
float p = manual_control_setpoint.y * roll_scale + roll_trim_active;
float p = manual_control_setpoint.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) {

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

@ -51,10 +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) = -manual_control_setpoint.z; // NED z, thrust [-1,1]
_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());

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

@ -141,14 +141,14 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
const float throttle = (_manual_control_setpoint.z + 1.f) * .5f;
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()));
@ -170,9 +170,9 @@ 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.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);
@ -180,13 +180,13 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
} 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();
_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.r;
_landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.yaw;
}
}
}
@ -551,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()));
}
@ -593,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;

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

@ -316,15 +316,15 @@ FixedwingPositionControl::manual_control_setpoint_poll()
{
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_manual_control_setpoint_for_height_rate = _manual_control_setpoint.x;
_manual_control_setpoint_for_airspeed = _manual_control_setpoint.z;
_manual_control_setpoint_for_height_rate = _manual_control_setpoint.pitch;
_manual_control_setpoint_for_airspeed = _manual_control_setpoint.throttle;
if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) {
/* Alternate stick allocation (similar concept as for multirotor systems:
* demanding up/down with the throttle stick, and move faster/break with the pitch one.
*/
_manual_control_setpoint_for_height_rate = -_manual_control_setpoint.z;
_manual_control_setpoint_for_airspeed = _manual_control_setpoint.x;
_manual_control_setpoint_for_height_rate = -_manual_control_setpoint.throttle;
_manual_control_setpoint_for_airspeed = _manual_control_setpoint.pitch;
}
// send neutral setpoints if no update for 1 s
@ -820,7 +820,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
/* reset setpoints from other modes (auto) otherwise we won't
* level out without new manual input */
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
}
@ -1426,7 +1426,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
// XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface.
if (_param_rwto_nudge.get()) {
_att_sp.yaw_sp_move_rate = _manual_control_setpoint.r;
_att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw;
}
// tune up the lateral position control guidance when on the ground
@ -1794,7 +1794,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
// XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface.
if (_param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled) {
_att_sp.yaw_sp_move_rate = _manual_control_setpoint.r;
_att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw;
}
// blend the height rate controlled throttle setpoints with initial throttle setting over the flare
@ -1919,7 +1919,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
false,
height_rate_sp);
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
@ -1954,8 +1954,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
}
/* heading control */
if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) {
if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH &&
fabsf(_manual_control_setpoint.yaw) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_yawrate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
@ -2028,14 +2028,14 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
false,
height_rate_sp);
if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH ||
fabsf(_manual_control_setpoint.yaw) >= HDG_HOLD_MAN_INPUT_THRESH) {
_hdg_hold_enabled = false;
_yaw_lock_engaged = false;
// do slew rate limiting on roll if enabled
float roll_sp_new = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
float roll_sp_new = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get());
if (control_interval > 0.f && roll_rate_slew_rad > 0.f) {
@ -2650,14 +2650,14 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po
Vector2f
FixedwingPositionControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position)
{
if (fabsf(_manual_control_setpoint.r) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE
if (fabsf(_manual_control_setpoint.yaw) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE
&& _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled
&& !_flare_states.flaring) {
// laterally nudge touchdown location with yaw stick
// positive is defined in the direction of a right hand turn starting from the approach vector direction
const float signed_deadzone_threshold = MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE * math::signNoZero(
_manual_control_setpoint.r);
_lateral_touchdown_position_offset += (_manual_control_setpoint.r - signed_deadzone_threshold) *
_manual_control_setpoint.yaw);
_lateral_touchdown_position_offset += (_manual_control_setpoint.yaw - signed_deadzone_threshold) *
MAX_TOUCHDOWN_POSITION_NUDGE_RATE * control_interval;
_lateral_touchdown_position_offset = math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(),
_param_fw_lnd_td_off.get());

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

@ -162,10 +162,10 @@ void ManualControl::Run()
const float dt_s = (now - _last_time) / 1e6f;
const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
_selector.setpoint().sticks_moving = (fabsf(_x_diff.update(_selector.setpoint().x, dt_s)) > minimum_stick_change)
|| (fabsf(_y_diff.update(_selector.setpoint().y, dt_s)) > minimum_stick_change)
|| (fabsf(_z_diff.update(_selector.setpoint().z, dt_s)) > minimum_stick_change)
|| (fabsf(_r_diff.update(_selector.setpoint().r, dt_s)) > minimum_stick_change);
_selector.setpoint().sticks_moving = (fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > minimum_stick_change)
|| (fabsf(_pitch_diff.update(_selector.setpoint().pitch, dt_s)) > minimum_stick_change)
|| (fabsf(_yaw_diff.update(_selector.setpoint().yaw, dt_s)) > minimum_stick_change)
|| (fabsf(_throttle_diff.update(_selector.setpoint().throttle, dt_s)) > minimum_stick_change);
if (switches_updated) {
// Only use switches if current source is RC as well.
@ -310,10 +310,10 @@ void ManualControl::Run()
_manual_control_setpoint_pub.publish(_selector.setpoint());
}
_x_diff.reset();
_y_diff.reset();
_z_diff.reset();
_r_diff.reset();
_roll_diff.reset();
_pitch_diff.reset();
_yaw_diff.reset();
_throttle_diff.reset();
_stick_arm_hysteresis.set_state_and_update(false, now);
_stick_disarm_hysteresis.set_state_and_update(false, now);
_button_hysteresis.set_state_and_update(false, now);
@ -330,8 +330,8 @@ void ManualControl::Run()
void ManualControl::processStickArming(const manual_control_setpoint_s &input)
{
// Arm gesture
const bool right_stick_centered = (fabsf(input.x) < 0.1f) && (fabsf(input.y) < 0.1f);
const bool left_stick_lower_right = (input.z < -0.8f) && (input.r > 0.9f);
const bool right_stick_centered = (fabsf(input.pitch) < 0.1f) && (fabsf(input.roll) < 0.1f);
const bool left_stick_lower_right = (input.throttle < -0.8f) && (input.yaw > 0.9f);
const bool previous_stick_arm_hysteresis = _stick_arm_hysteresis.get_state();
_stick_arm_hysteresis.set_state_and_update(left_stick_lower_right && right_stick_centered, input.timestamp);
@ -341,7 +341,7 @@ void ManualControl::processStickArming(const manual_control_setpoint_s &input)
}
// Disarm gesture
const bool left_stick_lower_left = (input.z < -0.8f) && (input.r < -0.9f);
const bool left_stick_lower_left = (input.throttle < -0.8f) && (input.yaw < -0.9f);
const bool previous_stick_disarm_hysteresis = _stick_disarm_hysteresis.get_state();
_stick_disarm_hysteresis.set_state_and_update(left_stick_lower_left && right_stick_centered, input.timestamp);

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

@ -117,10 +117,10 @@ private:
ManualControlSelector _selector;
bool _published_invalid_once{false};
MovingDiff _x_diff{};
MovingDiff _y_diff{};
MovingDiff _z_diff{};
MovingDiff _r_diff{};
MovingDiff _roll_diff{};
MovingDiff _pitch_diff{};
MovingDiff _yaw_diff{};
MovingDiff _throttle_diff{};
manual_control_switches_s _previous_switches{};
bool _previous_switches_initialized{false};

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

@ -2138,11 +2138,11 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
}
manual_control_setpoint_s manual_control_setpoint{};
manual_control_setpoint.x = mavlink_manual_control.x / 1000.f;
manual_control_setpoint.y = mavlink_manual_control.y / 1000.f;
manual_control_setpoint.pitch = mavlink_manual_control.x / 1000.f;
manual_control_setpoint.roll = mavlink_manual_control.y / 1000.f;
// For backwards compatibility at the moment interpret throttle in range [0,1000]
manual_control_setpoint.z = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f;
manual_control_setpoint.r = mavlink_manual_control.r / 1000.f;
manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f;
manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f;
manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time();
_manual_control_input_pub.publish(manual_control_setpoint);

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

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

4
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -118,8 +118,8 @@ private:
float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */
float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */
AlphaFilter<float> _man_x_input_filter;
AlphaFilter<float> _man_y_input_filter;
AlphaFilter<float> _man_roll_input_filter;
AlphaFilter<float> _man_pitch_input_filter;
hrt_abstime _last_run{0};
hrt_abstime _last_attitude_setpoint{0};

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

@ -116,11 +116,11 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
if (reset_yaw_sp) {
_man_yaw_sp = yaw;
} else if ((_manual_control_setpoint.z > -.9f)
} else if ((_manual_control_setpoint.throttle > -.9f)
|| (_param_mc_airmode.get() == 2)) {
const float yaw_rate = math::radians(_param_mpc_man_y_max.get());
attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;
attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate;
_man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt);
}
@ -128,21 +128,18 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
* Input mapping for roll & pitch setpoints
* ----------------------------------------
* We control the following 2 angles:
* - tilt angle, given by sqrt(x*x + y*y)
* - tilt angle, given by sqrt(roll*roll + pitch*pitch)
* - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion
*
* This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick
* points to, and changes of the stick input are linear.
*/
_man_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
_man_y_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
_man_x_input_filter.update(_manual_control_setpoint.x * _man_tilt_max);
_man_y_input_filter.update(_manual_control_setpoint.y * _man_tilt_max);
const float x = _man_x_input_filter.getState();
const float y = _man_y_input_filter.getState();
_man_roll_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
_man_pitch_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
// we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane
Vector2f v = Vector2f(y, -x);
// we want to fly towards the direction of (roll, pitch)
Vector2f v = Vector2f(_man_roll_input_filter.update(_manual_control_setpoint.roll * _man_tilt_max),
-_man_pitch_input_filter.update(_manual_control_setpoint.pitch * _man_tilt_max));
float v_norm = v.norm(); // the norm of v defines the tilt angle
if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle
@ -200,7 +197,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
q_sp.copyTo(attitude_setpoint.q_d);
attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.z + 1.f) * .5f);
attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.throttle + 1.f) * .5f);
attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
@ -316,8 +313,8 @@ MulticopterAttitudeControl::Run()
attitude_setpoint_generated = true;
} else {
_man_x_input_filter.reset(0.f);
_man_y_input_filter.reset(0.f);
_man_roll_input_filter.reset(0.f);
_man_pitch_input_filter.reset(0.f);
}
Vector3f rates_sp = _attitude_control.update(q);

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

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

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

@ -173,12 +173,12 @@ MulticopterRateControl::Run()
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// manual rates control - ACRO mode
const Vector3f man_rate_sp{
math::superexpo(manual_control_setpoint.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(-manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
math::superexpo(manual_control_setpoint.roll, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(-manual_control_setpoint.pitch, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(manual_control_setpoint.yaw, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
_rates_setpoint = man_rate_sp.emult(_acro_rate_max);
_thrust_setpoint(2) = -(manual_control_setpoint.z + 1.f) * .5f;
_thrust_setpoint(2) = -(manual_control_setpoint.throttle + 1.f) * .5f;
_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;
// publish rate setpoint

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

@ -661,10 +661,10 @@ void RCUpdate::UpdateManualControlInput(const hrt_abstime &timestamp_sample)
manual_control_input.data_source = manual_control_setpoint_s::SOURCE_RC;
// limit controls
manual_control_input.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f);
manual_control_input.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f);
manual_control_input.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f);
manual_control_input.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f);
manual_control_input.roll = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f);
manual_control_input.pitch = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f);
manual_control_input.yaw = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f);
manual_control_input.throttle = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f);
manual_control_input.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f);
manual_control_input.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f);
manual_control_input.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f);

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

@ -136,12 +136,12 @@ RoverPositionControl::manual_control_setpoint_poll()
} else {
const float yaw_rate = math::radians(_param_gnd_man_y_max.get());
_att_sp.yaw_sp_move_rate = _manual_control_setpoint.y * yaw_rate;
_att_sp.yaw_sp_move_rate = _manual_control_setpoint.roll * yaw_rate;
_manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt);
}
_att_sp.yaw_body = _manual_yaw_sp;
_att_sp.thrust_body[0] = _manual_control_setpoint.z;
_att_sp.thrust_body[0] = _manual_control_setpoint.throttle;
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
@ -152,13 +152,13 @@ RoverPositionControl::manual_control_setpoint_poll()
_attitude_sp_pub.publish(_att_sp);
} else {
_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y;
_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x;
_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.roll;
_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.pitch;
// Set heading from the manual roll input channel
_act_controls.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.y; // Nominally yaw: _manual_control_setpoint.r;
_manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw;
// Set throttle from the manual throttle channel
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.throttle;
_reset_yaw_sp = true;
}

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

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