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

Switch manual_control_setpoint.z scaling from [0,1] to [-1,1] 

To be consistent with all other axes of stick input and avoid future
rescaling confusion.

Note: for the MAVLink message 69 MANUAL_CONTROL it's using the full range
according to the message specs now [-1000,1000].
This commit is contained in:
Matthias Grob 2020-10-12 20:03:31 +02:00
parent 5579e319ff
commit 83246c84cf
13 changed files with 38 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/ManualControlSetpoint.msg
View File

@ -32,11 +32,11 @@ float32 y # stick position in y direction -1..1
# 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
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.5 means up and any value < 0.5 means down
# 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

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

@ -59,7 +59,7 @@ public:
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[2] = manual_control_setpoint.z; // throttle
_data[3] = manual_control_setpoint.r; // yaw
_data[4] = manual_control_setpoint.flaps;
_data[5] = manual_control_setpoint.aux1;

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

@ -93,7 +93,7 @@ AirshipAttitudeControl::publish_actuator_controls()
_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;
_actuator_controls.control[3] = (_manual_control_setpoint.z + 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.6f);
_is_throttle_low = (manual_control_setpoint.z < 0.1f);
_is_throttle_above_center = (manual_control_setpoint.z > 0.2f);
_is_throttle_low = (manual_control_setpoint.z < -0.8f);
if (_arm_state_machine.isArmed()) {
// Abort autonomous mode and switch to position mode if sticks are moved significantly

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

@ -53,8 +53,7 @@ bool Sticks::checkAndUpdateStickInputs()
// 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]
_positions(2) = -manual_control_setpoint.z; // NED z, thrust [-1,1]
_positions(3) = manual_control_setpoint.r; // yaw [-1,1]
// Exponential scale

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

@ -141,6 +141,8 @@ 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;
if (_vcontrol_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs
@ -152,7 +154,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_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);
@ -171,7 +173,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
_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.thrust_body[0] = throttle;
_rate_sp_pub.publish(_rates_sp);
@ -183,8 +185,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
-_manual_control_setpoint.x * _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);
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = throttle;
_landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.r;
}
}

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

@ -317,20 +317,20 @@ 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 = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
_manual_control_setpoint_for_airspeed = _manual_control_setpoint.z;
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 = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
_manual_control_setpoint_for_height_rate = -_manual_control_setpoint.z;
_manual_control_setpoint_for_airspeed = _manual_control_setpoint.x;
}
// send neutral setpoints if no update for 1 s
if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) {
_manual_control_setpoint_for_height_rate = 0.f;
_manual_control_setpoint_for_airspeed = 0.5f;
_manual_control_setpoint_for_airspeed = 0.f;
}
}
@ -379,18 +379,9 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) {
// neutral throttle corresponds to trim airspeed
if (_manual_control_setpoint_for_airspeed < 0.5f) {
// lower half of throttle is min to trim airspeed
altctrl_airspeed = _param_fw_airspd_min.get() +
(_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) *
_manual_control_setpoint_for_airspeed * 2;
} else {
// upper half of throttle is trim to max airspeed
altctrl_airspeed = _param_fw_airspd_trim.get() +
(_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) *
(_manual_control_setpoint_for_airspeed * 2 - 1);
}
return math::interpolateNXY(_manual_control_setpoint_for_airspeed,
{-1.f, 0.f, 1.f},
{_param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()});
} else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) {
altctrl_airspeed = _commanded_manual_airspeed_setpoint;
@ -1910,7 +1901,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
float throttle_max = _param_fw_thr_max.get();
// enable the operator to kill the throttle on ground
if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
@ -1958,7 +1949,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
float throttle_max = _param_fw_thr_max.get();
// enable the operator to kill the throttle on ground
if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}

4
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -127,7 +127,7 @@ static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s;
static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s;
// [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float THROTTLE_THRESH = 0.05f;
static constexpr float THROTTLE_THRESH = -.9f;
// [m/s/s] slew rate limit for airspeed setpoint changes
static constexpr float ASPD_SP_SLEW_RATE = 1.f;
@ -293,7 +293,7 @@ private:
// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
float _manual_control_setpoint_for_height_rate{0.0f};
// [.] normalized setpoint for manual airspeed control [0,1]; 0,0.5,1 maps to min,cruise,max airspeed commands
// [.] normalized setpoint for manual airspeed control [-1,1]; -1,0,1 maps to min,cruise,max airspeed commands
float _manual_control_setpoint_for_airspeed{0.0f};
// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED

1
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@ -24,7 +24,6 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/vehicle_air_data.h>

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

@ -162,15 +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();
const bool rpy_moving = (fabsf(_x_diff.update(_selector.setpoint().x, dt_s)) > minimum_stick_change)
|| (fabsf(_y_diff.update(_selector.setpoint().y, dt_s)) > minimum_stick_change)
|| (fabsf(_r_diff.update(_selector.setpoint().r, dt_s)) > minimum_stick_change);
// Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1]
const bool throttle_moving =
(fabsf(_z_diff.update(_selector.setpoint().z, dt_s)) * 2.f) > minimum_stick_change;
_selector.setpoint().sticks_moving = rpy_moving || throttle_moving;
_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);
if (switches_updated) {
// Only use switches if current source is RC as well.
@ -336,7 +331,7 @@ 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.1f) && (input.r > 0.9f);
const bool left_stick_lower_right = (input.z < -0.8f) && (input.r > 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);
@ -346,7 +341,7 @@ void ManualControl::processStickArming(const manual_control_setpoint_s &input)
}
// Disarm gesture
const bool left_stick_lower_left = (input.z < 0.1f) && (input.r < -0.9f);
const bool left_stick_lower_left = (input.z < -0.8f) && (input.r < -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);

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

@ -2138,10 +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.0f;
manual_control_setpoint.y = mavlink_manual_control.y / 1000.0f;
manual_control_setpoint.r = mavlink_manual_control.r / 1000.0f;
manual_control_setpoint.z = mavlink_manual_control.z / 1000.0f;
manual_control_setpoint.x = mavlink_manual_control.x / 1000.f;
manual_control_setpoint.y = 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.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);

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

@ -116,8 +116,8 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
if (reset_yaw_sp) {
_man_yaw_sp = yaw;
} else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f
|| _param_mc_airmode.get() == 2) {
} else if ((_manual_control_setpoint.z > -.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;
@ -200,7 +200,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
q_sp.copyTo(attitude_setpoint.q_d);
attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.z, 0.f, 1.f));
attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.z + 1.f) * .5f);
attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);

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

@ -178,7 +178,7 @@ MulticopterRateControl::Run()
math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
_rates_setpoint = man_rate_sp.emult(_acro_rate_max);
_thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
_thrust_setpoint(2) = -(manual_control_setpoint.z + 1.f) * .5f;
_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;
// publish rate setpoint