Skip to content

Commit aa38cec

Browse files
mahima-yogasfuhrer
mahima-yoga
authored and
sfuhrer
committed
FixedWingModeManager: use Sticks library
1 parent ab8ab3c commit aa38cec

File tree

3 files changed

+24
-27
lines changed

3 files changed

+24
-27
lines changed

‎src/modules/fw_mode_manager/CMakeLists.txt

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,8 @@ set(POSCONTROL_DEPENDENCIES
4141
SlewRate
4242
tecs
4343
motion_planning
44-
performance_model
44+
performance_model
45+
Sticks
4546
)
4647

4748
if(CONFIG_FIGURE_OF_EIGHT)

‎src/modules/fw_mode_manager/FixedWingModeManager.cpp

Lines changed: 18 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -207,23 +207,18 @@ FixedWingModeManager::wind_poll(const hrt_abstime now)
207207
void
208208
FixedWingModeManager::manual_control_setpoint_poll()
209209
{
210-
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
210+
_sticks.checkAndUpdateStickInputs();
211211

212-
_manual_control_setpoint_for_height_rate = _manual_control_setpoint.pitch;
213-
_manual_control_setpoint_for_airspeed = _manual_control_setpoint.throttle;
212+
_manual_control_setpoint_for_height_rate = _sticks.getPitch();
213+
_manual_control_setpoint_for_airspeed = _sticks.getThrottleZeroCentered();
214214

215215
if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) {
216216
/* Alternate stick allocation (similar concept as for multirotor systems:
217217
* demanding up/down with the throttle stick, and move faster/break with the pitch one.
218218
*/
219-
_manual_control_setpoint_for_height_rate = -_manual_control_setpoint.throttle;
220-
_manual_control_setpoint_for_airspeed = _manual_control_setpoint.pitch;
221-
}
222219

223-
// send neutral setpoints if no update for 1 s
224-
if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) {
225-
_manual_control_setpoint_for_height_rate = 0.f;
226-
_manual_control_setpoint_for_airspeed = 0.f;
220+
_manual_control_setpoint_for_height_rate = -_sticks.getThrottleZeroCentered();
221+
_manual_control_setpoint_for_airspeed = _sticks.getPitch();
227222
}
228223
}
229224

@@ -1156,7 +1151,7 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c
11561151
fixed_wing_runway_control_s fw_runway_control{};
11571152
fw_runway_control.timestamp = now;
11581153
fw_runway_control.wheel_steering_enabled = true;
1159-
fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _manual_control_setpoint.yaw : 0.f;
1154+
fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f;
11601155

11611156
_fixed_wing_runway_control_pub.publish(fw_runway_control);
11621157

@@ -1329,7 +1324,7 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const
13291324
fixed_wing_runway_control_s fw_runway_control{};
13301325
fw_runway_control.timestamp = now;
13311326
fw_runway_control.wheel_steering_enabled = true;
1332-
fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _manual_control_setpoint.yaw : 0.f;
1327+
fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f;
13331328

13341329
_fixed_wing_runway_control_pub.publish(fw_runway_control);
13351330

@@ -1578,7 +1573,7 @@ FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, cons
15781573
fw_runway_control.timestamp = now;
15791574
fw_runway_control.wheel_steering_enabled = true;
15801575
fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ?
1581-
_manual_control_setpoint.yaw : 0.f;
1576+
_sticks.getYaw() : 0.f;
15821577

15831578
_fixed_wing_runway_control_pub.publish(fw_runway_control);
15841579

@@ -1743,7 +1738,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
17431738
fw_runway_control.timestamp = now;
17441739
fw_runway_control.wheel_steering_enabled = true;
17451740
fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ?
1746-
_manual_control_setpoint.yaw : 0.f;
1741+
_sticks.getYaw() : 0.f;
17471742

17481743
_fixed_wing_runway_control_pub.publish(fw_runway_control);
17491744

@@ -1795,7 +1790,7 @@ FixedWingModeManager::control_manual_altitude(const float control_interval, cons
17951790
_ctrl_configuration_handler.setPitchMin(min_pitch);
17961791
_ctrl_configuration_handler.setThrottleMax(throttle_max);
17971792

1798-
const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
1793+
const float roll_body = _sticks.getRoll() * radians(_param_fw_r_lim.get());
17991794
const DirectionalGuidanceOutput sp = {.course_setpoint = NAN, .lateral_acceleration_feedforward = rollAngleToLateralAccel(roll_body)};
18001795
fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
18011796
fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
@@ -1831,8 +1826,8 @@ FixedWingModeManager::control_manual_position(const hrt_abstime now, const float
18311826

18321827
/* heading control */
18331828
// TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here)
1834-
if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH &&
1835-
fabsf(_manual_control_setpoint.yaw) < HDG_HOLD_MAN_INPUT_THRESH) {
1829+
if (fabsf(_sticks.getRoll()) < HDG_HOLD_MAN_INPUT_THRESH &&
1830+
fabsf(_sticks.getYaw()) < HDG_HOLD_MAN_INPUT_THRESH) {
18361831

18371832
/* heading / roll is zero, lock onto current heading */
18381833
if (fabsf(_yawrate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
@@ -1891,13 +1886,13 @@ FixedWingModeManager::control_manual_position(const hrt_abstime now, const float
18911886
_ctrl_configuration_handler.setPitchMin(min_pitch);
18921887
_ctrl_configuration_handler.setThrottleMax(throttle_max);
18931888

1894-
if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH ||
1895-
fabsf(_manual_control_setpoint.yaw) >= HDG_HOLD_MAN_INPUT_THRESH) {
1889+
if (!_yaw_lock_engaged || fabsf(_sticks.getRoll()) >= HDG_HOLD_MAN_INPUT_THRESH ||
1890+
fabsf(_sticks.getYaw()) >= HDG_HOLD_MAN_INPUT_THRESH) {
18961891

18971892
_hdg_hold_enabled = false;
18981893
_yaw_lock_engaged = false;
18991894

1900-
const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
1895+
const float roll_body = _sticks.getRoll() * radians(_param_fw_r_lim.get());
19011896
fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
19021897
fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
19031898
fw_lateral_ctrl_sp.lateral_acceleration = rollAngleToLateralAccel(roll_body);
@@ -2380,14 +2375,14 @@ FixedWingModeManager::initializeAutoLanding(const hrt_abstime &now, const positi
23802375
Vector2f
23812376
FixedWingModeManager::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position)
23822377
{
2383-
if (fabsf(_manual_control_setpoint.yaw) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE
2378+
if (fabsf(_sticks.getYaw()) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE
23842379
&& _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled
23852380
&& !_flare_states.flaring) {
23862381
// laterally nudge touchdown location with yaw stick
23872382
// positive is defined in the direction of a right hand turn starting from the approach vector direction
23882383
const float signed_deadzone_threshold = MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE * math::signNoZero(
2389-
_manual_control_setpoint.yaw);
2390-
_lateral_touchdown_position_offset += (_manual_control_setpoint.yaw - signed_deadzone_threshold) *
2384+
_sticks.getYaw());
2385+
_lateral_touchdown_position_offset += (_sticks.getYaw() - signed_deadzone_threshold) *
23912386
MAX_TOUCHDOWN_POSITION_NUDGE_RATE * control_interval;
23922387
_lateral_touchdown_position_offset = math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(),
23932388
_param_fw_lnd_td_off.get());

‎src/modules/fw_mode_manager/FixedWingModeManager.hpp

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@
5252
#include <lib/mathlib/mathlib.h>
5353
#include <lib/perf/perf_counter.h>
5454
#include <lib/slew_rate/SlewRate.hpp>
55+
#include <lib/sticks/Sticks.hpp>
5556
#include <px4_platform_common/px4_config.h>
5657
#include <px4_platform_common/defines.h>
5758
#include <px4_platform_common/module.h>
@@ -71,7 +72,6 @@
7172
#include <uORB/topics/fixed_wing_runway_control.h>
7273
#include <uORB/topics/landing_gear.h>
7374
#include <uORB/topics/launch_detection_status.h>
74-
#include <uORB/topics/manual_control_setpoint.h>
7575
#include <uORB/topics/normalized_unsigned_setpoint.h>
7676
#include <uORB/topics/parameter_update.h>
7777
#include <uORB/topics/position_controller_landing_status.h>
@@ -180,7 +180,6 @@ class FixedWingModeManager final : public ModuleBase<FixedWingModeManager>, publ
180180
uORB::Subscription _wind_sub{ORB_ID(wind)};
181181
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
182182
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
183-
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
184183
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
185184
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
186185
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
@@ -201,7 +200,6 @@ class FixedWingModeManager final : public ModuleBase<FixedWingModeManager>, publ
201200
uORB::Publication<fixed_wing_lateral_guidance_status_s> _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)};
202201
uORB::Publication<fixed_wing_runway_control_s> _fixed_wing_runway_control_pub{ORB_ID(fixed_wing_runway_control)};
203202

204-
manual_control_setpoint_s _manual_control_setpoint{};
205203
position_setpoint_triplet_s _pos_sp_triplet{};
206204
vehicle_control_mode_s _control_mode{};
207205
vehicle_local_position_s _local_pos{};
@@ -243,6 +241,9 @@ class FixedWingModeManager final : public ModuleBase<FixedWingModeManager>, publ
243241
STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT = (1 << 1)
244242
};
245243

244+
245+
Sticks _sticks{this};
246+
246247
// VEHICLE STATES
247248

248249
double _current_latitude{0};

0 commit comments

Comments
 (0)