fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 06:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Position controller: add option to swap throttle and pitch stick

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2020-09-30 10:06:30 +02:00
parent ad0de67df7
commit 67a0e1993a
3 changed files with 48 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+29 -12
View File
@@ -202,6 +202,24 @@ FixedwingPositionControl::airspeed_poll()
}
}
void
FixedwingPositionControl::manual_control_setpoint_poll()
{
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_manual_control_setpoint_altitude = _manual_control_setpoint.x;
_manual_control_setpoint_airspeed = _manual_control_setpoint.z;
if (_param_fw_posctl_inv_st.get()) {
/* 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_altitude = -(_manual_control_setpoint.z * 2.f - 1.f);
_manual_control_setpoint_airspeed = _manual_control_setpoint.x / 2.f + 0.5f;
}
}
void
FixedwingPositionControl::vehicle_attitude_poll()
{
@@ -229,17 +247,17 @@ FixedwingPositionControl::get_demanded_airspeed()
float altctrl_airspeed = 0;
// neutral throttle corresponds to trim airspeed
if (_manual_control_setpoint.z < 0.5f) {
if (_manual_control_setpoint_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.z * 2;
_manual_control_setpoint_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.z * 2 - 1);
(_manual_control_setpoint_airspeed * 2 - 1);
}
return altctrl_airspeed;
@@ -483,15 +501,15 @@ FixedwingPositionControl::update_desired_altitude(float dt)
* an axis. Positive X means to rotate positively around
* the X axis in NED frame, which is pitching down
*/
if (_manual_control_setpoint.x > deadBand) {
if (_manual_control_setpoint_altitude > deadBand) {
/* pitching down */
float pitch = -(_manual_control_setpoint.x - deadBand) / factor;
float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor;
_hold_alt += (_param_fw_t_sink_max.get() * dt) * pitch;
_was_in_deadband = false;
} else if (_manual_control_setpoint.x < - deadBand) {
} else if (_manual_control_setpoint_altitude < - deadBand) {
/* pitching up */
float pitch = -(_manual_control_setpoint.x + deadBand) / factor;
float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
_hold_alt += (_param_fw_t_clmb_max.get() * dt) * pitch;
_was_in_deadband = false;
climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH);
@@ -927,7 +945,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
/* throttle limiting */
throttle_max = _param_fw_thr_max.get();
if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint.z) < THROTTLE_THRESH)) {
if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
@@ -1029,7 +1047,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
/* throttle limiting */
throttle_max = _param_fw_thr_max.get();
if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint.z) < THROTTLE_THRESH)) {
if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
@@ -1659,14 +1677,13 @@ FixedwingPositionControl::Run()
_alt_reset_counter = _local_pos.vz_reset_counter;
_pos_reset_counter = _local_pos.vxy_reset_counter;
airspeed_poll();
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
// reset the altitude foh (first order hold) logic
_min_current_sp_distance_xy = FLT_MAX;
}
manual_control_setpoint_poll();
_pos_sp_triplet_sub.update(&_pos_sp_triplet);
vehicle_attitude_poll();
vehicle_command_poll();
vehicle_control_mode_poll();